' -----[ Title ]---------------------------------------------------------------- ' Modified for new Co-Processor board and production Twinkle Toes 8/7/03 ' filename = Toddler CoP Bd Program A.6.BS2 ' Taken from Toddler Program A.1 ' This program has real time servo programming and sensor integration. ' Toddler now wanders in a straight line until it sees or hits something. ' by H. W. Lewis (www.bluebelldesign.com) ' {$STAMP BS2} ' {$PBASIC 2.5} ' This program steps the stride and tilt movements so they are less jerky. ' For the first and last "delta(Stride/Tilt)" steps, the servos run at ' (Stride/Tilt)StepSlow. ' In between, servos run at (Stride/Tilt)StepFast. ' DON'T FORGET TO USE THE RESET SWITCH WHEN POWERING UP. ' Servo 0 = Tilt, Servo channel 1 = Stride ' The ground (or black) wire goes closest to PCB and ' the control (or white) wire goes furthest from PCB ' see I/O Definitions below for other pin connections ' -----[ I/O Definitions ]------------------------------------------------------ To_Co_Proc PIN 9 ' data to BBDI Co-Processor(Sin) Frm_Co_Proc PIN 10 ' data from BBDI Co-Processor(Sout) Timer_Exp PIN 7 ' Timer Expired from BBDI Co-Processor Rst_Co_Proc PIN 8 ' reset to BBDI Co-Processor LeftIRLED PIN 4 ' left IRLED RightIRLED PIN 15 ' right IRLED LeftIRSensor PIN 11 ' left IR sensor input RightIRSensor PIN 14 ' right IR sensor input RightBumper PIN 1 ' Toddler Toes LeftBumper PIN 2 Spkr PIN 13 ' beeper pin CycleIndicator PIN 12 ' shows how fast main loop is going ' - toggles each time through - NOT REALLY NEEDED 'EMPTY PIN 5 ' spare Stamp I/O pin 'EMPTY PIN 6 ' spare Stamp I/O pin ' -----[ #DEFINES ]------------------------------------------------------------- #DEFINE SwayMode = 0 ' 1 will tilt Toddler back and forth ' only - no striding - good for setup ' See end of program for notes on cable ' -----[ Constants ]------------------------------------------------------------ ' Comments below sometiomes hold original values so, when tuning, you can get ' back to where you started. These values are for my Toddler, ' yours are probably different. Tilt limits can be found by using Sway and ' CalibrateFlat routines. Comment them out in Main when you are done tuning. TiltStepSlow CON 4 ' TiltServo step size slow TiltStepFast CON 20 ' TiltServo step size fast ' in units of 1/4 step per 20ms ' so 16 => 4 full servo steps/20ms TiltDelta CON 20 ' steps taken while running slow RightTilt CON 65 ' Tilt limits (Toddler was 620 = 1240us) ' CoP = (65*4us)+ 994 = 1254us LeftTilt CON 210 ' (Toddler was 880 = 1760us) ' CoP = (191*4us)+ 994 = 1758us CenterTilt CON 127 ' (Toddler was 750 = 1500us) ' CoP = (127*4us)+ 994 = 1502us DelayTltDelta CON TiltDelta*4/TiltStepSlow DelayTilt CON LeftTilt-RightTilt-TiltDelta-TiltDelta*4/TiltStepFast DelayShrtTilt CON LeftTilt-CenterTilt-TiltDelta-TiltDelta*4/TiltStepFast+1 ' + 1 takes care of fractional result ' DelayTltDelta and DelayTilt came out even. StrideStepSlow CON 16 ' StrideServo step size start and stop StrideStepFast CON 28 ' step size while running at ' full speeed 28 = 7 steps/20ms StrideDelta CON 8 ' steps taken while running slow RightStride CON 77 ' Stride limits ' (Toddler was 650 = 1300us) ' CoP = (77*4us)+ 994 = 1302us CenterStride CON 127 ' (Toddler was 750 = 1500us) ' CoP = (127*4us)+ 994 = 1502us LeftStride CON 177 ' (Toddler was 850 = 1700us) ' CoP = (177*4us)+ 994 = 1702us DelayStrDelta CON StrideDelta*4/StrideStepSlow DelayStride CON LeftStride-RightStride-StrideDelta-StrideDelta*4/StrideStepFast DelayShrtStride CON LeftStride-CenterStride-StrideDelta-StrideDelta*4/StrideStepFast+1 ' + 1 takes care of fractional result ' DelayStrDelta and DelayStride came ' out even. Baud CON 84 ' 9600 Baud => BS2 = 84, BS2p = 240 TiltServo CON 136 ' use Servo 0 = Tilt ' (136 = WRITE setpoint command) StrideServo CON 137 ' use Servo 1 = Stride TiltServoPos CON 24 ' read tilt servo (0) current position ' in Co-Proc StrideServoPos CON 25 ' read stride servo (1) current position ' in Co-Proc TiltRamp CON 144 ' start value to write servo 0 ramp ' (command = 128 + 16 + ch0 = 144) StrideRamp CON 145 ' write servo 1 ramp ' = 1 more than servo 0 command ' Co-Proc servo channels can also be used as general outputs. ' Here channels 6 AND 7 are used for vision indicator LED drive. ' 6 = Left vision, 7 = Right vision, High out = on. LeftVisionLED_Off CON 246 ' left LED - 0 = Off LeftVisionLED_On CON 254 ' left LED - 1 = On ' executing vision sequence for left side RightVisionLED_Off CON 247 ' right LED - 0 = Off RightVisionLED_On CON 255 ' right LED - 1 = On ' executing vision sequence for right side ctrlLEDs CON %1111 ' define DIRs as constants to prepBumper CON %1001 ' simplify LED/bumper setups bothGreen CON %0110 ' OUTa register sets red/green bothRed CON %1001 ' status on P0-P3 for Twinkle Toes rightRed CON %0101 ' object detection states leftRed CON %1010 bothLEDsOff CON %1001 Fwd CON 0 ' codes to pick movement table PivL CON 1 ' set newMove to one of these PivR CON 2 ' values and New_Movemt will LOOKUP BumpL CON 3 ' the right movement table BumpR CON 4 ' -----[ Variables ]------------------------------------------------------------ delayTime VAR Byte ' # of 20 ms cycles to wait for ' move to complete serDIn VAR delayTime ' variable where Co-Processor ' DATA comes back - reused doneMoveFlag VAR Bit ' flag: 1 = servos hit new values rightIR_Flag VAR Bit ' flag: 1 = something on right side leftIR_Flag VAR Bit ' flag: 1 = something left side rightBumpFlag VAR Bit ' flag: 1 = bumper hit right side leftBumpFlag VAR Bit ' flag: 1 = bumper hit left side sensors VAR Nib ' lower 2 bits of the sensors var ' used to store IR detector values currentMove VAR Nib newMove VAR Nib Mx VAR Word ' index for movement tables MxOffset VAR Byte ' added to Mx for index bMovment VAR Byte ' values looked up from movement table ' Also used to run ramping state ' machine in timer routine. currentTilt VAR Byte ' used to be Words currentStride VAR Byte ' - saves variable space ' -----[ EEPROM Data ]---------------------------------------------------------- ' ' These are actual values saved in the Basic Movement tables. ' They are coded so the bits can be used rather than a CASE statement. ' This way it saves a lot of code and flags. It will also allow for ' using the upper nibble for something else. ' TLR = Tilt Left to Right and so on for the rest of the table TLR CON %00000001 ' 0+0+0+0+1 = 1 TLC CON %00000010 ' 0+0+0+2+0 = 2 TRL CON %00000100 ' 0+0+4+0+0 = 4 TRC CON %00000110 ' 0+0+4+2+0 = 6 TCL CON %00001000 ' 0+8+0+0+0 = 8 TCR CON %00001001 ' 0+8+0+0+1 = 9 TXL CON %00001100 ' 0+8+4+0+0 = 12 TXR CON %00001101 ' 0+8+4+0+1 = 13 ' SLR = Stride Left to Right and so on for the rest of the table SLR CON %00010001 ' 16+0+0+0+1 = 17 SLC CON %00010010 ' 16+0+0+2+0 = 18 SRL CON %00010100 ' 16+0+4+0+0 = 20 SRC CON %00010110 ' 16+0+4+2+0 = 22 SCL CON %00011000 ' 16+8+0+0+0 = 24 SCR CON %00011001 ' 16+8+0+0+1 = 25 SXL CON %00011100 ' 16+8+4+0+0 = 28 SXR CON %00011101 ' 16+8+4+0+1 = 29 xx CON 255 ' table end code ' ------ Basic Movement Tables ------ ' ' These tables contain Basic Movements consisting of bytes containing ' above Basic Movement Codes to describe movement sequences. ' An xx indicates the end of a list. ' PivotLeft and PivotRight aren't entered at the start of their tables Forward DATA TXR, SXL, TRL, SLR, xx ' possible rt bumper hit while SR ' left bumper hit while SL PivotLeft DATA TXR, SXL, TRL, SLR ' copy of forward, index into here ' by keeping old MxOffset value from Forward move. DATA TLC, SRL, TCL, SLR, xx ' this is the actual pivot move. PivotRight DATA TXR, SXL, TRL, SLR ' copy of forward, index into here ' by keeping old MxOffset value from Forward move. DATA TLR, SRL, TRC, SLR, xx ' this is the actual pivot move. RBumper DATA SXL, TXR, SLR, TRC, SRL, TCL, SLR, xx 'back up 1 step and pivot left ' bumper hit assumes SR (@TL) when hit happened. LBumper DATA SXR, TXL, SRL, TLC, SLR, TCR, SRL, TRL, SLR, xx 'back up 1 step and pivot right ' bumper hit assumes SL (@TR) when hit happened. #IF (SwayMode = 1) #THEN SwayIt DATA TXR, TRL, xx ' used to sway back and ' forth for tuning #ENDIF ' -----[ Initialize ]----------------------------------------------------------- SEROUT To_Co_Proc, BAUD, [116] 'Sets up serial pin LOW Rst_Co_Proc 'Reset Co-Processor PAUSE 20 HIGH Rst_Co_Proc GOSUB Clr_Vision ' reset vision LEDs and flags DIRS = ctrlLEDs ' setup green LEDs for Forward OUTS = bothGreen SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStepSlow] 'write rate to servo 0 SEROUT To_Co_Proc, Baud, [StrideRamp,StrideStepSlow] 'write rate to servo 1 ResetFeet: CurrentTilt = CenterTilt CurrentStride = CenterStride SEROUT To_Co_Proc, Baud, [TiltServo,CurrentTilt] 'Set Servo 0 = Tilt PAUSE 200 ' start only one servo at a time. SEROUT To_Co_Proc, Baud, [StrideServo,CurrentStride] 'Set Servo 1 = Stride 'DEBUG "Forward = ", hex Forward, cr 'DEBUG "PivotLeft = ", hex PivotLeft, cr, "PivotRight = ", hex PivotRight, cr 'DEBUG "RBumper = ", hex RBumper, cr, "LBumper = ", hex LBumper, cr doneMoveFlag = 1 sensors = 0 MxOffset = 0 ' Turn off bumper LEDs if being driven from servo channels 4 & 5 ' - DO NOT DO THIS IF THEY ARE BEING USED AS SERVO CHANNELS! ' The LEDs are not used in this program but may be wired in. SEROUT To_Co_Proc, BAUD, [252] 'output servo ch 4 = 1 -> Co-Processor SEROUT To_Co_Proc, BAUD, [253] 'output servo ch 5 = 1 -> Co-Processor PAUSE 1000 ' wait with the feet straight at startup FREQOUT Spkr, 2000, 3000 ' program start/restart signal rightBumpFlag = 0 leftBumpFlag = 0 currentMove = 15 ' invalid value to assure startup ' -----[ Main Code ]------------------------------------------------------------ Main_Program: DO TOGGLE CycleIndicator ' indicate speed of loop on pin ' typically 10 ms - periodically 30 ms ' THIS IS NOT RELATED TO THE SERVO ' REFRESH TIME! Refresh stays at 20 ms. IF Timer_Exp = 1 THEN ' check if any timers have timed out ' read timeout alarm byte to get and ' clear it SEROUT To_Co_Proc, Baud, [119] ' send the byte command SERIN Frm_Co_Proc, Baud, [serDIn] ' timeout data comes back into serDIn 'DEBUG " delayTime = expired, serDIn = ",HEX serDIn, " i = ", DEC i,CR IF (serDIn.BIT0) THEN ' a servo is done when Timer 0 expires 'DEBUG "bMovment = ", HEX2 bMovment, CR IF (bMovment.BIT4 = 0) THEN GOSUB TiltStateMach ' run the ramp state machine for Tilt ELSE GOSUB StrideStateMach ' run the ramp state machine for Stride ENDIF ' IF (bMovment.BIT4 = 0) ENDIF ' IF (serDIn.BIT0) ' If any other timers are also in use, ' check them here. ENDIF ' IF Timer_Exp = 1 GOSUB Do_Vision ' look for obstacles with IR GOSUB Do_Bumper ' check for obstacles with bumpers #IF (SwayMode = 0) #THEN GOSUB New_Movemt ' gets next move from table #ENDIF IF (doneMoveFlag = 1) THEN ' done moving - lookup new move #IF (SwayMode = 1) #THEN MX = SwayIt ' used to sway back and forth to tune #ENDIF GOSUB Do_Movement ' generates basic moves from tables ' then generates next move from bMovment ENDIF ' IF (doneMoveFlag = 1) LOOP END ' -----[ Subroutines ]---------------------------------------------------------- Do_Vision: FREQOUT LeftIRLED,1,38500 ' pulse left IRLED. sensors.BIT0 = LeftIRSensor ' store IR detector output in RAM FREQOUT RightIRLED,1,38500 ' repeat for the right IR pair. sensors.BIT1 = RightIRSensor ' Check if currently doing an infrared move IF ((leftIR_Flag = 1) & (rightIR_Flag = 1)) THEN See_Both IF (leftIR_Flag = 1) THEN See_Left ' is left only? IF (rightIR_Flag = 1) THEN See_Right ' is right only? ' Load IR detector output values into the lower 2 bits of the sensors ' variable, storing a number between 0 and 3 that the BRANCH command ' can execute the appropriate routine. BRANCH sensors,[See_Both,See_Right,See_Left,See_None] See_Both: newMove = PivR rightIR_Flag = 1 ' flag: 1 = something on right side leftIR_Flag = 1 ' flag: 1 = something left side SEROUT To_Co_Proc, Baud, [LeftVisionLED_On] ' turn on LED SEROUT To_Co_Proc, Baud, [RightVisionLED_On] ' turn on LED RETURN See_Right: newMove = PivL rightIR_Flag = 1 ' flag: 1 = something on right side SEROUT To_Co_Proc, Baud, [RightVisionLED_On] ' turn on LED RETURN See_Left: newMove = PivR leftIR_Flag = 1 ' flag: 1 = see something left side SEROUT To_Co_Proc, Baud, [LeftVisionLED_On] ' turn on LED RETURN See_None: newMove = Fwd RETURN '--------------- Do_Bumper: DIRS = prepBumper OUTS = bothLEDsOff ' Bumpers on each foot. A hit causes Toddler to back and turn. ' Code asssumes leg that hit is on the leg that is moving ' (while leaning on the other leg). ' A hit backs up the hit leg right away then steps back 1 step ' and does a pivot away. ' check for old bumper hits on left and right IF (leftBumpFlag = 1) THEN Bump_LeftA IF (rightBumpFlag = 1) THEN Bump_RightA ' check for new bumper hits now IF (LeftBumper = 0) THEN Bump_Left ' check for bumper hit on left IF (RightBumper = 0) THEN Bump_Right ' check for bumper hit on right DIRS = ctrlLEDs OUTS = bothGreen RETURN Bump_Left: 'DEBUG "bump left", cr ' sends only once on change SEROUT To_Co_Proc, Baud, [StrideServoPos] ' read the stride position SERIN Frm_Co_Proc, Baud, [serDIn] ' data comes back into serDIn CurrentStride = serDIn ' set the current position there ' for new delay calculations Bump_LeftA: leftBumpFlag = 1 ' flag indicates bump response move newMove = BumpL DIRS = ctrlLEDs OUTS = leftRed GOTO Clr_Vision ' goes to Clr_Vision Bump_Right: 'DEBUG "bump right", cr SEROUT To_Co_Proc, Baud, [StrideServoPos] ' read the stride position SERIN Frm_Co_Proc, Baud, [serDIn] ' data comes back into serDIn CurrentStride = serDIn ' set the current position there ' for new delay calculations Bump_RightA: rightBumpFlag = 1 newMove = BumpR DIRS = ctrlLEDs OUTS = rightRed ' falls through to Clr_Vision '--------------- Clr_Vision: ' reset vision LEDs and flags IF rightIR_Flag = 1 THEN rightIR_Flag = 0 ' flag: 1 = see something on right side SEROUT To_Co_Proc, Baud, [RightVisionLED_Off] ' turn off LED ENDIF ' IF was used to prevent constant ' serial transmissions IF leftIR_Flag = 1 THEN leftIR_Flag = 0 ' flag: 1 = see something left side SEROUT To_Co_Proc, Baud, [LeftVisionLED_Off] ' turn off LED ENDIF RETURN '--------------- New_Movemt: ' sequences for forward motion IF (newMove <> currentMove) THEN ' new DATA if movement changed 'DEBUG cr, "start NewMovemt; newMove = ", DEC newMove 'DEBUG ", currentMove = ", DEC currentMove, CR doneMoveFlag = 1 ' stop current move wherever it is currentMove = newMove ' When customizing the program, here is where to integrate between ' moves. You can make the transitions reasonable by using the existing ' Mx value and MxOffset to compute the proper MxOffset to start into ' the next move. IF ((newMove = Fwd) OR (newMove = BumpL) OR (newMove = BumpR)) THEN MxOffset = 0 ' reset MxOffset value for new move ENDIF ' (newMove = Fwd) OR ' note: kept old MxOffset value for lead into pivot left or right LOOKUP newMove,[Forward,PivotLeft,PivotRight,LBumper,RBumper],Mx ' lookup movement table index ENDIF ' newMove <> currentMove RETURN '--------------- Do_Movement: ' first lookup basic move from tables ' then generate next move from bMovment ' then READ (Mx + MxOffset), bMovment ' read next basic move byte 'DEBUG CR,CR, "bMovment = ", HEX2 bMovment, ", " 'DEBUG "Mx = ", HEX Mx, ", MxOffset = ", HEX MxOffset, CR MxOffset = MxOffset + 1 IF (bMovment = xx) THEN ' end code so do new move 'DEBUG " xx = finished table", CR, CR MxOffset = 0 ' finished move sequence so restart leftBumpFlag = 0 ' clear out left bumper hit flag rightBumpFlag = 0 ' right too DIRS = ctrlLEDs OUTS = bothGreen GOSUB Clr_Vision ' reset vision LEDs and flags #IF (SwayMode = 0) #THEN GOSUB Do_Vision ' figure out new movement, GOSUB Do_Bumper GOTO New_Movemt ' get new table and offset #ELSE 'DEBUG CR,CR GOTO Do_Movement ' used to sway back and forth ' for tuning #ENDIF ENDIF ' bMovment = xx ' Now use bMovment to generate the actual move 'DEBUG " bMovment = ", HEX bMovment, " " ' Bit 4, 1 = Stride, 0 = Tilt ' Bit 3, 1 = start at SC, 0 = SR or SL ' 3,2 = 11 means don't know where started ' Bit 2, 1 = start at SR, 0 = SL ' Bit 1, 1 = end at SC, 0 = SR or SL ' Bit 0, 1 = end at SR, 0 = SL ' Bit 7, 0 = first time through = ' speed up and load 2nd intermediate ' position value ' Bit 6, 0 = 2nd time through = ' slow down and put final pos value ' Bit 5, not used IF (bMovment.BIT4 = 1) THEN ' Stride movements IF (bMovment.BIT3 = 1) THEN IF (bMovment.BIT2 = 1) THEN ' don't know where started IF (currentStride < CenterStride) THEN ' from Stride Right currentStride = currentStride + StrideDelta bMovment.BIT3 = 0 ' tell bMovment it came from SR ELSEIF (currentStride > CenterStride) THEN currentStride = currentStride - StrideDelta ' from Stride Left bMovment.BIT3 = 0 ' tell bMovment it came from SL bMovment.BIT2 = 0 ELSE ' from Stride Center bMovment.BIT2 = 0 ' tell bMovment it came from SC IF (bMovment.BIT0 = 0) THEN currentStride = CenterStride + StrideDelta ELSE currentStride = CenterStride - StrideDelta ENDIF ENDIF ' IF (currentStride < CenterStride) ELSE ' started at center IF (bMovment.BIT0 = 0) THEN currentStride = CenterStride + StrideDelta ELSE currentStride = CenterStride - StrideDelta ENDIF ENDIF ' IF (bMovment.BIT2 = 1) ELSE ' started at SR or SL IF (bMovment.BIT2 = 1) THEN currentStride = RightStride + StrideDelta ' from Stride Right ELSE currentStride = LeftStride - StrideDelta ' from Stride Left ENDIF ENDIF ' IF (bMovment.BIT3 = 1) delayTime = DelayStrDelta SEROUT To_Co_Proc, Baud, [StrideRamp,StrideStepSlow] 'write rate to servo 1 SEROUT To_Co_Proc, Baud, [StrideServo,currentStride] 'Set Servo 1 = Stride ELSE ' => Tilt Movements IF (bMovment.BIT3 = 1) THEN IF (bMovment.BIT2 = 1) THEN ' don't know where started IF (currentTilt < CenterTilt) THEN ' from Tilt Right currentTilt = currentTilt + TiltDelta bMovment.BIT3 = 0 ' tell bMovment it came from SR ELSEIF (currentTilt > CenterTilt) THEN currentTilt = currentTilt - TiltDelta ' from Tilt Left bMovment.BIT3 = 0 ' tell bMovment it came from TL bMovment.BIT2 = 0 ELSE ' from Tilt Center bMovment.BIT2 = 0 ' tell bMovment it came from TC IF (bMovment.BIT0 = 0) THEN currentTilt = CenterTilt + TiltDelta ELSE currentTilt = CenterTilt - TiltDelta ENDIF ENDIF ' IF (currentTilt < CenterTilt) ELSE ' started at center IF (bMovment.BIT0 = 0) THEN currentTilt = CenterTilt + TiltDelta ELSE currentTilt = CenterTilt - TiltDelta ENDIF ENDIF ' IF (bMovment.BIT2 = 1) ELSE ' started at TR or TL IF (bMovment.BIT2 = 1) THEN currentTilt = RightTilt + TiltDelta ' from Tilt Right ELSE currentTilt = LeftTilt - TiltDelta ' from Tilt Left ENDIF ENDIF ' IF (bMovment.BIT3 = 1) delayTime = DelayTltDelta SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStepSlow] 'write rate to servo 0 SEROUT To_Co_Proc, Baud, [TiltServo,currentTilt] 'Set Servo 0 = Stride ENDIF ' bMovment.BIT4 = 1 'DEBUG " delayTime = ", DEC delayTime, CR,LF SEROUT To_Co_Proc, Baud, [(128),delayTime] 'write Timer 0 with delay value doneMoveFlag = 0 ' reset flag RETURN '--------------- TiltStateMach: ' State machine for Tilt Ramping ' from bMovment variable ' Bit 4, 1 = Stride, 0 = Tilt ' Bit 3, 1 = start at SC, 0 = SR or SL ' 3,2 = 11 means don't know where started ' Bit 2, 1 = start at SR, 0 = SL ' Bit 1, 1 = end at SC, 0 = SR or SL ' Bit 0, 1 = end at SR, 0 = SL ' Bit 7, 0 = first time through = ' speed up and load 2nd intermediate ' position value ' Bit 6, 0 = 2nd time through = ' slow down and put final pos value ' Bit 5, not used IF (bMovment.BIT7 = 0) THEN ' first time through IF (bMovment.BIT1 = 0) THEN ' end at TiltLeft or TR IF (bMovment.BIT0 = 0) THEN ' end at TL CurrentTilt = LeftTilt-TiltDelta ELSE CurrentTilt = RightTilt+TiltDelta ENDIF IF (bMovment.BIT3 = 0) THEN delayTime = DelayTilt ELSE delayTime = DelayShrtTilt ' from Tilt Center ENDIF ELSE ' end at Stride Center delayTime = DelayShrtTilt ' to Tilt Center IF (bMovment.BIT2 = 0) THEN CurrentTilt = CenterTilt+TiltDelta ELSE CurrentTilt = CenterTilt-TiltDelta ENDIF ENDIF ' IF (bMovment.BIT1 = 0) SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStepFast] 'write rate to servo 0 bMovment.BIT7 = 1 ' done first time through ELSE IF (bMovment.BIT6 = 0) THEN ' 2nd time through IF (bMovment.BIT1 = 0) THEN ' end at TiltLeft or TR IF (bMovment.BIT0 = 0) THEN CurrentTilt = LeftTilt ELSE CurrentTilt = RightTilt ENDIF ELSE CurrentTilt = CenterTilt ENDIF ' IF (bMovment.BIT1 = 0) delayTime = DelayTltDelta SEROUT To_Co_Proc, Baud, [TiltRamp,TiltStepSlow] 'write rate to servo 0 bMovment.BIT6 = 1 ' done second time through ELSE ' this is third time through doneMoveFlag = 1 ENDIF ' IF (bMovment.BIT6 = 0) ENDIF ' IF (bMovment.BIT7 = 0) IF (doneMoveFlag = 0) THEN SEROUT To_Co_Proc, Baud, [TiltServo,CurrentTilt] SEROUT To_Co_Proc, Baud, [(128),delayTime] 'write Timer 0 with delay value ENDIF RETURN '--------------- StrideStateMach: ' State machine for Stride Ramping ' from bMovment variable ' Bit 4, 1 = Stride, 0 = Tilt ' Bit 3, 1 = start at SC, 0 = SR or SL ' 3,2 = 11 means don't know where started ' Bit 2, 1 = start at SR, 0 = SL ' Bit 1, 1 = end at SC, 0 = SR or SL ' Bit 0, 1 = end at SR, 0 = SL ' Bit 7, 0 = first time through = ' speed up and load 2nd intermediate ' position value ' Bit 6, 0 = 2nd time through = ' slow down and put final pos value ' Bit 5, not used IF (bMovment.BIT7 = 0) THEN ' first time through IF (bMovment.BIT1 = 0) THEN ' end at StrideLeft or SR IF (bMovment.BIT0 = 0) THEN ' end at SL CurrentStride = LeftStride-StrideDelta ELSE CurrentStride = RightStride+StrideDelta ENDIF IF (bMovment.BIT3 = 0) THEN delayTime = DelayStride ELSE delayTime = DelayShrtStride ' from Stride Center ENDIF ELSE ' end at Stride Center delayTime = DelayShrtStride ' to Stride Center IF (bMovment.BIT2 = 0) THEN CurrentStride = CenterStride+StrideDelta ELSE CurrentStride = CenterStride-StrideDelta ENDIF ENDIF ' IF (bMovment.BIT1 = 0) SEROUT To_Co_Proc, Baud, [StrideRamp,StrideStepFast] 'write rate to servo 1 bMovment.BIT7 = 1 ' done first time through ELSE IF (bMovment.BIT6 = 0) THEN ' 2nd time through IF (bMovment.BIT1 = 0) THEN ' end at StrideLeft or SR IF (bMovment.BIT0 = 0) THEN CurrentStride = LeftStride ELSE CurrentStride = RightStride ENDIF ELSE CurrentStride = CenterStride ENDIF ' IF (bMovment.BIT1 = 0) delayTime = DelayStrDelta SEROUT To_Co_Proc, Baud, [StrideRamp,StrideStepSlow] 'write rate to servo 1 bMovment.BIT6 = 1 ' done second time through ELSE ' this is third time through doneMoveFlag = 1 ENDIF ' IF (bMovment.BIT6 = 0) ENDIF ' IF (bMovment.BIT7 = 0) IF (doneMoveFlag = 0) THEN SEROUT To_Co_Proc, Baud, [StrideServo,CurrentStride] SEROUT To_Co_Proc, Baud, [(128),delayTime] 'write Timer 0 with delay value ENDIF RETURN ' -----[ Calibration Note ]----------------------------------------------------- ' The serial cable is too stiff for sway to work with it installed. ' If you want to see the values while Toddler is moving, you can make an ' extension with flexible wire and 2 DB9 connectors. That will let Toddler ' move more freely while still logging the values. Wire pins 2,3,4, & 5 from ' the male to the female connector. Wire pin 6 to pin 7 on the female ' connector only.