Position Controller, HELP!!! Original Wheel_controller C code to Spin conversio
rpdb
Posts: 101
I have been working on converting the original Wheel_Controller to SPIN.·
I have a personal deadline to get it done before UPEW·this weekend
I have built wheel encoders for 36 ppr at the wheels,·and just·now have installed some USDigital units for 80_000 ppr(wheel revolution)·at the motors. I currently·have 10 inch wheels driven through a 450:1 gear box, the motors are each driven by an HB25 (pwm on mode 0)
I know others might like this code to modify for thier own use, those that stray from standard, and need to optimize for thier own use and purpose.
I am at a loss,· no, stuck in qaugmire for a bit·as to continue on this code for "I cant see the forest for the trees". I need input and morale support, perhaps a blessing to forgive my cussing.
I want to keep this compatible with the orginal API.
The header would read "original code by: K. McCullough
·······························"converted to SPIN by the Propeller Community"
I am asking for help from the Prop community to get this done and bring it to the UPEW as well as the OBEX and forums, as a collabritive effort.
The spin.code I have is in the rar and·is a loose translation from C to SPIN and does not yet work.
· (the original code is .c)
I need help with the following to make PositionController.spin work:
1. to get the ISR routine running IN PositionController.spin using the values provided by the Quadrature_Encoder ie Pos[noparse][[/noparse]0/1] and Encoder.ReadDelta(0/1) instead of positionUpdate(). I was thinking maybe run this value through a moving_average object to get the average speed. I also need to make a method to pass which Pos[noparse][[/noparse]x] to look at when the positioncontroller is called.
2.Move global vars and locals to where they go.
3.write the code to pass recieve longs or byte for commands ie [noparse][[/noparse]TRVL (BYTE/LONG)]·to maintain,·or make possible·backward compatability. I will have to pass longs for Pos or travel.
4.pique the brains of the smartest people I know (Prop-Heads) and make new friends
I attach the original.c and my mess.spin
Post Edited (rpdb) : 6/23/2010 10:43:35 PM GMT
I have a personal deadline to get it done before UPEW·this weekend
I have built wheel encoders for 36 ppr at the wheels,·and just·now have installed some USDigital units for 80_000 ppr(wheel revolution)·at the motors. I currently·have 10 inch wheels driven through a 450:1 gear box, the motors are each driven by an HB25 (pwm on mode 0)
I know others might like this code to modify for thier own use, those that stray from standard, and need to optimize for thier own use and purpose.
I am at a loss,· no, stuck in qaugmire for a bit·as to continue on this code for "I cant see the forest for the trees". I need input and morale support, perhaps a blessing to forgive my cussing.
I want to keep this compatible with the orginal API.
The header would read "original code by: K. McCullough
·······························"converted to SPIN by the Propeller Community"
I am asking for help from the Prop community to get this done and bring it to the UPEW as well as the OBEX and forums, as a collabritive effort.
The spin.code I have is in the rar and·is a loose translation from C to SPIN and does not yet work.
· (the original code is .c)
I need help with the following to make PositionController.spin work:
1. to get the ISR routine running IN PositionController.spin using the values provided by the Quadrature_Encoder ie Pos[noparse][[/noparse]0/1] and Encoder.ReadDelta(0/1) instead of positionUpdate(). I was thinking maybe run this value through a moving_average object to get the average speed. I also need to make a method to pass which Pos[noparse][[/noparse]x] to look at when the positioncontroller is called.
2.Move global vars and locals to where they go.
3.write the code to pass recieve longs or byte for commands ie [noparse][[/noparse]TRVL (BYTE/LONG)]·to maintain,·or make possible·backward compatability. I will have to pass longs for Pos or travel.
4.pique the brains of the smartest people I know (Prop-Heads) and make new friends
I attach the original.c and my mess.spin
Post Edited (rpdb) : 6/23/2010 10:43:35 PM GMT
Comments
('/***************************************************************************** '** "Position_Encoder_v1_01.c" '** Last Modified: Apr 25, 2008 '** Created By: K. McCullough '** '** ----------------------------------------------------------------------- '** '** This program handles the signals from the optical interrupter switches '** to keep track of wheel position/distance. It receives commands and '** returns data via a one-wire UART. Additionally, it can control an HB-25 '** to take care of travelling a specified distance at a specific speed. '** '** The address selection sets which sensor board will listen to commands '** issued by the master. Using address "0" speaks to all devices '** simultaneously. '** con midspeed = 2765 'mid value (1.5ms) for output pulses ILL HAVE TO CHANGE THIS. USED IN ISR FOR OUTPUT TO SERVO PULSEOUT 'ms20 = 36864 'TOP value for 20ms pulse periods TIME OUT PULSE TO RUN ISR? 'pulseout = OCR1A 'Timer1 output compare register OUTPUT THIS TO SERVOS FOR PEKIT OBJECT tx_dly_default = 115 'default value for tx_delay. Wait approx 500uS before reply INCREASE THIS FOR 115200? USED IN Tx OBJECT max_accel_default = 15 'CHANGE THE FOLLOWING FOR 80000 PPR user_spd_default = 36 ' = 1 rotation/0.5 sec Kp_default = 35 'USED IN ISR FOR OUTPUT TO SERVO 'Ki = 0 'Kd = 0 QPOS = 1 'Query Position Accumulator QSPD = 2 'Query Speed CHFA = 3 'Check for Arrival TRVL = 4 'Travel Number of Positions CLRP = 5 'Clear Position SREV = 6 'Set Orientation as Reversed STXD = 7 'Set Tx Delay SMAX = 8 'Set Speed Maximum SSRR = 9 'Set Speed Ramp Rate maxCommand = 9 'maximum value of any commands {//***FUNCTION PROTOTYPES*** void updatePosition(void); void txData(void); void rxByte(void); } VAR '***GLOBAL VARIABLES*** long position 'this 32-bit value stores the current position long orientation '+1 or -1 depending on orientation set by user long oldstate 'this stores the old state of the two encoder inputs phase a phase b long tempData 'temp input data/command register ADDRESS THIS WITH TXRX? DO I NEED THIS WITH FULLDUPLEXSERIAL?????? long tx_delay 'sets how long to wait before replying back long tx_buffer[noparse][[/noparse]2] 'variable holds the data to be sent out. tx_buffer = [noparse][[/noparse]ByteH : ByteL] long tx_index 'number of bytes left to be sent out long pls_accum[noparse][[/noparse]25] 'stores the number of pulses accumulated in 20mS. 'Summed to get pls/0.5sec. Summation is performed every 20mS long sp_spd_accum[noparse][[/noparse]25] 'Summed to get the set point incrementing speed in positions/0.5sec long idx_ctr 'Keeps track of which segment to add pulses to, incremented every 20mS. long current_spd 'stores the current speed value long user_spd 'stores the speed value set by the user long target_spd 'stores the target set_pt incrementing rate (speed) long set_pt_spd 'stores the advancement speed of the set point (positions/0.5sec) long inv_pt_distance 'the distance which is needed to decelerate to a stop given the current set_pt_spd and acceleration value long set_pt_old 'holds the old set point value long pos_var_old 'used for position advancement value calculation to determine output speed long spd_var_old 'used for velocity advancement value calculation to determine change in output speed long old_spd 'stores the previous value of driving speed (or position incrementation) long set_pt 'target position (set-point of PID) long end_pt 'total end point - set by the user - not dependent on the current direction of travel long target_end_pt 'the desired (or "target) end point for the current direction of travel long end_direction 'either +1 or -1 depending on the direction required to travel toward the target_end_pt 'word err_sum 'integral term 'word err_dif 'derivative term 'word err_prev 'previous value of err (for derivative) long old_pwm 'previous value of out_pwm long max_accel 'how quickly the rate of speed can increase long commandStack[noparse][[/noparse]40] Pub Start(MutexID, HB25Pin) 'mutex MUST BE 1 OR 2, 3, 4 AS 0 SPEAKS TO ALL POSITION CONTROLLERS 'pass mutex id and output pin dev_id := MutexID 'OLD CODE CHECKED JUMPERS(((PIND & 0x30)>>4)+1);//check state of ID selection jumpers. 'Passed as param instead HB25 := HB25Pin '//initiallize user variables to defaults tx_delay := tx_dly_default 'set the default transmit delay from CON block for fullduplexserial.spin orientation := 1 'default orientation (positive direction) user_spd := user_spd_default 'default user speed from CON block max_accel := max_accel_default 'default max acceleration value from CON block 'oldstate := ((PIND & 0x0C)>>2) 'mask = 0000,1100 'I am using quadrature_encoder.spin instead position := 0 'clear the position accumulator '//initiallize other stuff to start from scratch... pos_var_old := 0 spd_var_old := 0 target_spd := 0 old_spd := 0 idx_ctr := 0 set_pt := 0 set_pt_old := 0 end_pt := 0 target_end_pt := 0 cognew(Command(dev_id, HB25), @commandStack) DO I HAVE TO CALL @COMMAND STACK[noparse][[/noparse]0/1] repeat PUB stop '' Stop frees a cog if cog cogstop(cog~ - 1) pub save this for later ' 'FOR USE IN ISR TO GET POSITION UPDATES FROM QUADRATURE ENCODERS Abs_Right := Pos[noparse][[/noparse]0] 'Read Right motor's absolute position Delta_Right := Encoder.ReadDelta(0) 'Read Right encoder's delta position (value since last read) Abs_Left := Pos[noparse][[/noparse]1] 'the left Delta_Left := Encoder.ReadDelta(1) Pub Command(dev_id, HB25) 'FIX THIS TO PASS ID FOR EACH MUTEX repeat 'This code section handles all the incoming data and commands IF dataIn := TXRX.rxcheck ' wait for incoming byte (-1 if no data) 'seperate incoming data byte components [noparse][[/noparse]COMMAND:ADDRESS] addr := (dataIn & 0x07) 'mask = (0000,0111) 'FIRST THREE BITS ARE MUTEX ADDRESS FROM WHEEL CONTROLLER command := ((dataIn & 0xf8) >> 3) 'mask = (1111,1000) 'THE COMMAND IS [noparse][[/noparse]QPOS..SSRR] OR [noparse][[/noparse]0..9] Case command QPOS: 'Query Position COMMAND = 1 if (addr == dev_id) '"Single" type command only ' TXRX.txLong Do I HAVE TO WRITE OR CHECK OBEX TXRX.tx(position & 0x00ff) 'low byte I SHOULD TRANSFER A LONG HERE TXRX.tx((position >> 8)& 0x00ff) 'SECOND byte A BYTE AT A TIME TXRX.tx((position >> 16)& 0x00ff) 'THIRD byte TXRX.tx((position >> 24)& 0x00ff) 'high byte else TXRX.rxflush 'discard the bytes recieved UNTIL -1 QSPD: 'Query Speed COMMAND = 2 if (addr == dev_id) 'Single" type command only TXRX.tx(current_spd & 0x00ff) 'low byte I SHOULD TRANSFER A LONG HERE TXRX.tx((current_spd >> 8)& 0x00ff) 'SECOND byte A BYTE AT A TIME TXRX.tx((current_spd >> 16)& 0x00ff) 'THIRD byte TXRX.tx((current_spd >> 24)& 0x00ff) 'high byte else TXRX.rxflush CLRP: 'Clear Position COMMAND = 5 if ((addr == 0)||(addr == dev_id)) '"Single" or "All" type command position := 0 set_pt := 0 target_end_pt := 0 end_pt := 0 pos_var_old := 0 spd_var_old := 0 target_spd := 0 old_spd := 0 idx_ctr := 0 set_pt_old := 0 SREV: 'Set Orientation/Direction as Reversed COMMAND = 6 if (((addr == 0)||(addr == dev_id))) '//&&(((signed char)tempData == -1)||(tempData == 1))) 'if valid address and direction value orientation := -1 STXD: || SSRR: || CHFA: 'COMMAND = 7 || 9 || 3 'STXD: ' 'SSRR: ' 'CHFA: 'if one of these three commands 'receive 1 extra data byte tempData1 := TXRX.rx 'OR A LONG, ONE BYTE AT A TIME tempData2 := TXRX.rx tempData3 := TXRX.rx tempData4 := TXRX.rx tempData := ((tempData4 << 24) + (tempData3 << 16) + (tempData2 << 8) + (tempData1)) tempData := TXRX.rx 'check if valid address if ((addr == 0)||(addr == dev_id)) '"Single" or "All" type command Case Command STXD: 'Set Tx Delay value COMMAND = 7 tx_delay := tempData SSRR: 'Set Speed Ramp Rate COMMAND = 9 IS RAMP RATE A LONG????? MAYBE A BYTE??? if (tempData == 0) tempData := 1 'to avoid a divide by zero in later calculations (commented out because it turns out that it's ok - but not recommended!) else max_accel := tempData CHFA: 'Check for Arrival COMMAND = 3 if (addr != 0) 'can only be a "single" type command if ((current_spd == 0)&&(position >= end_pt-tempData)&&(position <= end_pt+tempData)) TXRX.tx(0xFF) 'yes, it has arrived else TXRX.tx(0x00) 'has not arrived Case SMAX: || TRVL: 'COMMAND = 8 || 4 'case TRVL: 'receive 2 extra data bytes OR RECIEVE 4 BYTES FOR A LONG??????????????? tempData1 := TXRX.rx tempData2 := TXRX.rx tempData3 := TXRX.rx tempData4 := TXRX.rx tempData := ((tempData4 << 24) + (tempData3 << 16) + (tempData2 << 8) + (tempData1)) 'check if valid address if ((addr == 0)||(addr == dev_id)) '"Single" or "All" type command Case SMAX: 'Set Speed Maximum COMMAND = 8 tempData1 := TXRX.rx 'RECIEVE 4 BYTES FOR A LONG?? tempData2 := TXRX.rx tempData3 := TXRX.rx tempData4 := TXRX.rx tempData := ((tempData4 << 24) + (tempData3 << 16) + (tempData2 << 8) + (tempData1)) user_spd := tempData case TRVL: 'TRVL - Travel Number of Positions COMMAND = 4 OR RECIEVE 4 BYTES FOR A LONG??????????????? tempData1 := TXRX.rx 'RECIEVE 4 BYTES FOR A LONG?? tempData2 := TXRX.rx tempData3 := TXRX.rx tempData4 := TXRX.rx tempData := ((tempData4 << 24) + (tempData3 << 16) + (tempData2 << 8) + (tempData1)) position_advance := tempData end_pt += position_advance 'now handle special case of when position_advance variable == 0 if (end_direction > 0) if (position_advance == 0) 'target_end_pt = set_pt + inv_pt_distance; //special case handling 'end_pt = target_end_pt; end_pt := set_pt + inv_pt_distance 'special case handling target_end_pt := end_pt else if (end_pt < (set_pt + inv_pt_distance)) '|| (position_advance == 0)) 'if the end_pt is closer (in the positive direction) than the current position plus how far it needs to decelerate properly target_end_pt := (set_pt + inv_pt_distance) 'load the new end point to be just how far it needs to decelerate else if (end_direction < 0) if (position_advance == 0) '//target_end_pt = set_pt - inv_pt_distance; 'special case handling '//end_pt = target_end_pt; end_pt := set_pt - inv_pt_distance 'special case handling target_end_pt := end_pt else if (end_pt > (set_pt - inv_pt_distance)) '//|| (position_advance == 0)) //if the end_pt is closer (in the positive direction) than the current position plus how far it needs to decelerate properly target_end_pt = (set_pt - inv_pt_distance) 'load the new end point to be just how far it needs to decelerate pub nothing 'this gives a better contrast on the LCD while using proptool on my laptop for the following code: PUB ISR | temp_var_new, delta_spd, new_spd, err, cntr, out_pwm, distance_togo, seconds, dT, T ''This ISR is executed every 20mS. It is the main engine which drives the position controller's operation. dT := clkfreq/50 'setup for accurate time base of 20mS repeat T := cnt waitcnt(T += dT) 'execute every 20ms current_spd := 0 'Gloabals reset 'DO I REALLY NEED TO RESET EACH TIME OR JUST ONCE set_pt_spd := 0 end_direction := 0 'defaults to 0 (no motion), gets set to +1 or -1 later in the code to denote positive or negative direction of travel respectively set_pt_old := set_pt 'This section finds current_spd average of a circular array repeat cntr from 0 to 25 current_spd += pls_accum[noparse][[/noparse]cntr] 'sums the pulse accumulator array set_pt_spd += sp_spd_accum[noparse][[/noparse]cntr] 'current_spd := filter_out 'moving average filter from quadrature encoder 'This section calculates the next velocity advancement for creating an acceleration temp_var_new := (max_accel*(++idx_ctr)/25) delta_spd := temp_var_new - spd_var_old 'This is the magnitude of the needed acceleration. (potential change in speed based on acceleration/deceleration) spd_var_old := temp_var_new inv_pt_distance := ((set_pt_spd*set_pt_spd)/(2*max_accel)) 'calculates the inversion point distance distance_togo = target_end_pt - set_pt if (distance_togo > 0) 'need to travel in the positive direction end_direction := 1 else if (distance_togo < 0) 'need to travel in the negative direction distance_togo := -distance_togo 'produces the absolute value of distance_togo variable end_direction := -1 'Note: the position advancement engine is unsigned (sign is handled later by the end_direction variable) if (distance_togo > inv_pt_distance) 'if not at the inversion point yet... target_spd += delta_spd 'accelerate if (target_spd > user_spd) 'and limit speed to user_spd target_spd := user_spd 'if at or beyond the inversion point... if (target_spd > delta_spd) target_spd -= delta_spd 'decelerate else target_spd := 0 'but limit speed to 0 (cannot go negative) temp_var_new := ((target_spd*idx_ctr)/25) 'calculate the next set_point increment based on the current target_spd new_spd := temp_var_new - pos_var_old 'value to increment the setpoint pos_var_old := temp_var_new if (new_spd > distance_togo) 'make sure that the set_pt doesn't pass up the target_end_pt set_pt := target_end_pt else set_pt += (end_direction*new_spd) 'otherwise simply increment the set_point appropriately if (end_direction > 0) 'travelling in the positive direction if ((signed int)end_pt > (set_pt + inv_pt_distance)) 'if the end_pt is further (in the positive direction) than the current position plus how far it needs to decelerate properly target_end_pt := end_pt 'load the new end point else if (end_direction < 0) 'travelling in the negative direction if ((signed int)end_pt < ((signed int)set_pt - inv_pt_distance)) 'if the end_pt is further (in the negative direction) than the current position plus how far it needs to decelerate properly target_end_pt := end_pt 'load the new end point else 'end_direction == 0 (not travelling at all, stopped) target_end_pt = end_pt 'update the new end point - this will usually be the same as the current (target) end point anyway {{ //***This section is the PID controller - Not implemented in this design (it is only proportional) //err = set_pt - position; //proportional //err_sum += err; //integral //err_dif = err - err_prev; //derivative //err_prev = err; }} '//out_pwm = ((Kp*err) + (Ki*err_sum) + (Kd*err_dif)/3); err = set_pt - position '//this is the error term if (err > 0) err += 2 if (err < 0) err -= 2 out_pwm = ((Kp_default*err) #> -1000 <# 1000 ) '//proportional error term 'if (out_pwm > 1000) '//limit output based on valid range for servo pulses ' out_pwm := 1000 'if (out_pwm < -1000) ' out_pwm := -1000 old_pwm = out_pwm 'output new pulse value servo[noparse][[/noparse]0] := midspeed + out_pwm 'NEED TO FIX THIS SO THAT SERVO[noparse][[/noparse]INDEX] IS PASSED AT CALL 'THIS IS TO CALL THE SERVOS FOR PE KIT 'Reset the idx_ctr value for circular array if (idx_ctr == 25) pos_var_old := 0 spd_var_old := 0 idx_ctr := 0 pls_accum[noparse][[/noparse]idx_ctr] := 0 '//resets the next cell in the pulse accumulator array sp_spd_accum[noparse][[/noparse]idx_ctr] := (set_pt - set_pt_old) /*********************************** 'THIS IS PHASE A OF ONE ENCODER 'THE FOLLOWING SHOULD BE HANDLED BY THE ABOVE ISR CODE ** INT0 interrupt subroutine 'BY LOOKING AT THE Pos[noparse][[/noparse]0] AND Encoder.ReadDelta(0) ***********************************/ ' ISR(INT0_vect) //input pin change interrupt INT 0 (on PD2) { updatePosition(); } /*********************************** 'THIS IS PHASE B OF ONE ENCODER ** INT0 interrupt subroutine ***********************************/ ISR(INT1_vect) //input pin change interrupt INT1 (on PD3) { updatePosition(); } /*********************************** 'THIS HAPPENS IF PHASE A OR B CHANGES ** updatePosition ***********************************/ void updatePosition(void) { //function either increments or decrements the "position" //variable based on the new inputs. This ISR is executed every time //there is a pin change on either sensor pin (PD2, PD3) signed char pos_adv_val; unsigned char newstate; //this loads the new state of the two encoder inputs //mask away all but the correct two bits newstate = ((PIND & 0x0C) >> 2); //mask = 0000,1100 } pos_adv_val = orientation * (signed char)(-1 + (((newstate & 0x01) << 1)^(oldstate & 0x02))); position += pos_adv_val; pls_accum[noparse][[/noparse]idx_ctr] += pos_adv_val; oldstate = newstate; '*********************************** 'REWRITE THIS FOR RX LONGS Pub rxLong ***********************************/ void rxByte(void)//unsigned char numToRx) {//This function waits for new data to arrive in the Rx Buffer //and then stores it in the tempData variable. tempData1 := TXRX.rx 'RECIEVE 4 BYTES FOR A LONG?? tempData2 := TXRX.rx tempData3 := TXRX.rx tempData4 := TXRX.rx tempData := ((tempData4 << 24) + (tempData3 << 16) + (tempData2 << 8) + (tempData1)) while (!CHECKBIT(UCSRA,RXC)); //wait until new data has arrived tempData = UDR; //get incoming data } /*********************************** 'REWRITE THIS FOR TX LONGS Pub txLong ***********************************/ 'void txData(void) TXRX.tx(current_spd & 0x00ff) 'low byte I SHOULD TRANSFER A LONG HERE TXRX.tx((current_spd >> 8)& 0x00ff) 'SECOND byte A BYTE AT A TIME TXRX.tx((current_spd >> 16)& 0x00ff) 'THIRD byte TXRX.tx((current_spd >> 24)& 0x00ff) 'high byte { //The BS2 requires at least a 500uS delay between sending //a byte and then being able to receive a byte, so the delay //is generated with Timer0 OCA. The delay can be user defined //in case a different microcontroller is being used which does //not require a delay (or requires a different delay). //NOTE: Setting (not clearing) the OCF0A flag bit is what // clears the flag. //To properly execute sending data: // 1) set tx_buffer // 2) set tx_index // 3) call txData() CLEARBIT(UCSRB,RXEN); //disables the UART receiver SETBIT(UCSRB,TXEN); //enables the UART transmitter while(tx_index > 0) { OCR0A = (TCNT0 + tx_delay); //setup next timer compare value SETBIT(TIFR,OCF0A); //clears the timer output compare flag while(!CHECKBIT(TIFR,OCF0A)); //wait for the timer compare (OCF0A flag set) to occur UDR = tx_buffer[noparse][[/noparse]--tx_index]; //send byte of data SETBIT(UCSRA,TXC); //clears the Tx Complete flag while(!CHECKBIT(UCSRA,TXC)); //wait for data transmit complete before continuing } CLEARBIT(UCSRB,TXEN); //disables the UART transmitter SETBIT(UCSRB, RXEN); //re-enables the UART receiver } )Why convert it to SPIN? I just compiled the C source under Catalina, and it comes in at 14kb 8kb ** - so even if you have to add in a custom serial comms object and a few other odds and sods, it should easily run on any Propeller. Is there more to the program that you have not shown, or is it just that one C file?
Of course, I shouldn't really pretend that just compiling it is the end of the story - the code I generated will not actually run since it is full of hardware assumptions that relate specifically to the AVR - but you presumably already know all the Prop equivalents or you wouldn't be able to translate it to SPIN.
Also, you'd probably also have to restructure the code slightly because the AVR has to use interrupts to do things that you would do much more logically with a separate cog on the Propeller (ugh! - this kind of stuff is so ugly on chips like the AVR it makes me remember why I love the Propeller so much!) - but it is all doable.
Ross.
** recompiled to exclude floating point, which I assume you don't need.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Catalina - a FREE C compiler for the Propeller - see Catalina
Post Edited (RossH) : 6/23/2010 9:53:28 AM GMT
Briefly looking at the code I can see that we can replace the serial port stuff with FullDuplexSerial or such.
The updatePosition function, called from two pin interrupts can be replaced with a simple Spin or PASM loop in it's own Cog.
The TIMER1_OVF routine perhaps should run in it's own Cog and the main command loop in another.
So we have 4 Cogs here: Main loop, timer tick, position sense and FDS.
There is no way Zog is mature enough to do this yet and I'm having doubts about being able to handle the 20ms timer tick in time. Although if Zog cannot do it it may be hard in Spin also.
Catalina should have no problems.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
For me, the past is not over yet.
Post Edited (heater) : 6/23/2010 10:33:00 AM GMT
Mostly because the code would be open source for use on the Prop. There are C compilers for the Prop, but I don't know how they handle the conversion for interupts.
Second the AVR is running at 14Mhz and the prop at 80Mhz and the encoders I am using are going to need that. I will probably run a 6.25MHz crystal to get 100MHz.
Yes, it is doable. Did you find the PositionController.spin file in the rar? It is close but I need a little help.
Post Edited (rpdb) : 6/23/2010 10:40:22 AM GMT
Basically one can replace those pin interrupt handlers with Spin or PASM code running in a separate COG. The logic there is small so it should be simple. Perhaps you need PASM for the speed. Perhaps using waitpe/waitpne to trigger on pin changes. Anyway that COG would detect the pin changes and update some globally available HUB variables with the acquired data.
That takes care of the high speed real-time part.
The code hanging off the 20ms interrupt would probably need to run in it's own COG. We have to hope that Spin can do this in time. Don't forget Spin is interpreted.
I presume the command loop can be a bit more laid back.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
For me, the past is not over yet.
The main thing that I need help with is the position_controller.spin.
1). The original C code used interupts for each "phase A/B" quadrature encoder to increment posUpdate().
······
/*********************************** ** INT0 interrupt subroutine ***********************************/ ISR(INT0_vect) //input pin change interrupt INT 0 (on PD2) { updatePosition(); }/*********************************** ** INT0 interrupt subroutine ***********************************/ ISR(INT1_vect) //input pin change interrupt INT1 (on PD3) { updatePosition(); }void updatePosition(void) { //function either increments or decrements the "position" //variable based on the new inputs. This ISR is executed every time //there is a pin change on either sensor pin (PD2, PD3) signed char pos_adv_val; unsigned char newstate; //this loads the new state of the two encoder inputs //mask away all but the correct two bits newstate = ((PIND & 0x0C) >> 2); //mask = 0000,1100 pos_adv_val = orientation * (signed char)(-1 + (((newstate & 0x01) << 1)^(oldstate & 0x02))); position += pos_adv_val; pls_accum[noparse][[/noparse]idx_ctr] += pos_adv_val;2). My hope is to convert to SPIN and the inputs will instead be from·Quadrature_Encoder.spin
····· to update position and average speed with the following:
(these are used by PositionController.spin[noparse][[/noparse]0] in a cog for the right wheel) Abs_Right := Pos[noparse][[/noparse]0] 'Read Right motor's absolute position Delta_Right := Encoder.ReadDelta(0) 'Read Right encoder's delta position (value since last read) (these are used by PositionController.spin[noparse][[/noparse]1] in a different cog for the left wheel) Abs_Left := Pos[noparse][[/noparse]1] 'the left Delta_Left := Encoder.ReadDelta(1)·3). I am having difficulty wrapping my mind around the [noparse][[/noparse]idx_ctr]··and the [noparse][[/noparse]cntr] in the··ISR· and PosUpdate routines of·the·original C code.
4). I am·NOT trying to compile the C code with catalina etc or any other C compiler. I wish for help to re-write this to·make this run in SPIN. PositionController.spin is what I have so far as my conversion from PositionController.c.· To make the PosUpdate and ISR work in PositionController.spin, to find·the average speed and position is what I need help with.
5). I am truly grateful for any and all suggestions and help in this endevour.
Post Edited (rpdb) : 6/24/2010 9:51:37 AM GMT
·Here is the latest: