' -----[ Title ]--------------------------------------------------------------- ' Modified for new Co-Processor board and production Twinkle Toes 7/3/03 ' filename = Toddler CoP Bd Program A.1.BS2 ' Taken from Toddler Program 8.3 FSM Walker.bs2 ' 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} ' 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 ' -----[ 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 ' -----[ Constants ]----------------------------------------------------------- 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 StrideServoPos CON 25 ' read stride servo (1) current position ' in Co-Proc ' Comments below hold original values so, when tuning, you can get back ' to where you started. These values are for my Toddler, yours may be different. TiltStep CON 4 ' TiltServo step size - was 4 steps/20ms RightTilt CON 40 ' started with 61 needed more right tilt ' Tilt limits (Toddler was 620 = 1240us) ' CoP = (61*4us)+ 994 = 1238us CenterTilt CON 127 ' (Toddler was 750 = 1500us) ' CoP = (127*4us)+ 994 = 1502us LeftTilt CON 191 ' (Toddler was 880 = 1760us) ' CoP = (191*4us)+ 994 = 1758us StrideStep CON 5 ' StrideServo step size - was 5 steps/20ms RightStride CON 76 ' Stride limits (Toddler was 650 = 1300us) ' CoP = (76*4us)+ 994 = 1298us CenterStride CON 127 ' (Toddler was 750 = 1500us) ' CoP = (127*4us)+ 994 = 1502us LeftStride CON 176 ' (Toddler was 850 = 1700us) ' CoP = (176*4us)+ 994 = 1698us 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 doTiltFlag VAR Bit ' flag: 1 = tilt servo has new value ' flag: 0 = stride servo has new value 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 bMovmnt VAR Byte ' table value for lookup movement currentTilt VAR Byte ' used to be Words currentStride VAR Byte ' - saves variable space newValue VAR Byte newStride VAR Byte ' -----[ EEPROM Data ]----------------------------------------------------- ' ' These are actual values saved in the Basic Movement tables. TL CON 0 ' use lower nibble for these TC CON 1 TR CON 2 SL CON 3 SC CON 4 SR CON 5 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 TR, SL, TL, SR, xx ' possible rt bumper hit while SR ' left bumper hit while SL PivotLeft DATA TR, SL, TL, SR ' copy of forward, index into here ' by keeping old MxOffset value from Forward move. DATA TC, SL, TL, SR, xx ' this is the actual pivot move. PivotRight DATA TR, SL, TL, SR ' copy of forward, index into here ' by keeping old MxOffset value from Forward move. DATA TR, SL, TC, SR, xx ' this is the actual pivot move. RBumper DATA SL, TR, SR, TC, SL, TL, SR, xx 'back up 1 step and pivot left ' bumper hit assumes SR when hit happened. LBumper DATA SR, TL, SL, TC, SR, TR, SL, TL, SR, xx 'back up 1 step and pivot right ' bumper hit assumes SL when hit happened. ' -----[ 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,(TiltStep*4)] 'write rate to servo 0 SEROUT To_Co_Proc, Baud, [StrideRamp,(StrideStep*4)] 'write rate to servo 1 ResetFeet: CurrentTilt = CenterTilt CurrentStride = CenterStride SEROUT To_Co_Proc, Baud, [TiltServo,CurrentTilt] 'Set Servo 0 = Tilt SEROUT To_Co_Proc, Baud, [StrideServo,CurrentStride] 'Set Servo 1 = Stride 'DEBUG "resetCC", CR, lf '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! They are not driven in this program. 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 start ' newMove = Fwd ' for testing single moves - ' newMove = PivL ' comment out GOSUBs to vision ' newMove = PivR ' and bump or the value might be ' newMove = BumpL ' overwritten ' newMove = BumpR ' -----[ Main Code ]------------------------------------------------------------ Main_Program: DO TOGGLE CycleIndicator ' indicate speed of loop on pin ' (typically 7 -> 11ms) ' this time will slow down as you add ' more code but that is not a problem. ' THIS IS NOT RELATED TO THE SERVO ' REFRESH TIME! Refresh stays at 20 ms. ' This program only takes about 40% of ' the BS2's memory. IF Timer_Exp = 1 THEN ' check if any timers done ' 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] ' data comes back into SerDIn 'DEBUG " DelayTime = expired, SerDIn = ",HEX SerDIn, " i = ", DEC i,CR IF (SerDIn.BIT0) THEN doneMoveFlag = 1 ' servo is finished when Timer 0 expires. ' 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 GOSUB New_Movemt ' generates next move from table GOSUB Do_Movement ' gives servo pulses out 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 ' 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 IF (doneMoveFlag = 1) THEN ' done moving - lookup new move READ (Mx + MxOffset), bMovmnt ' read next basic move byte 'DEBUG "Mx = ", HEX Mx, ", MxOffset = ", HEX MxOffset MxOffset = MxOffset + 1 IF (bMovmnt < xx) THEN ' end code so do new move 'DEBUG " bMovmnt = ", HEX bMovmnt, " " SELECT bMovmnt ' set and display movement type CASE TL newValue = LeftTilt doTiltFlag = 1 'DEBUG "TL, ", cr CASE TC newValue = CenterTilt doTiltFlag = 1 'DEBUG "TC, ", cr CASE TR newValue = RightTilt doTiltFlag = 1 'DEBUG "TR, ", cr CASE SL newValue = LeftStride doTiltFlag = 0 'DEBUG "SL, ", cr CASE SC newValue = CenterStride doTiltFlag = 0 'DEBUG "SC, ", cr CASE SR newValue = RightStride doTiltFlag = 0 'DEBUG "SR, ", cr ENDSELECT ' will fall through if invalid index ELSE '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 GOSUB Do_Vision ' since done previous movement, GOSUB Do_Bumper ' figure out new one GOTO New_Movemt ' get new table and offset ENDIF ENDIF 'Done_New_Move: RETURN ' ignores move if invalid '--------------- Do_Movement: IF (doneMoveFlag = 1) THEN ' only do this when the move is done DelayTime = 0 IF (doTiltFlag = 1) THEN ' only do this when the move is tilt IF (currentTilt < newValue) THEN DelayTime = newValue - currentTilt ELSE DelayTime = currentTilt - newValue ' coming from left tilt = decrement ENDIF SEROUT To_Co_Proc, Baud, [TiltServo,newValue] 'Set Servo 0 = Tilt DelayTime = DelayTime/TiltStep CurrentTilt = newValue ELSE ' IF (doTiltFlag = 1) IF (currentStride < newValue) THEN DelayTime = newValue - currentStride ELSE DelayTime = currentStride - newValue ENDIF SEROUT To_Co_Proc, Baud, [StrideServo,newValue] 'Set Servo 1 = Stride DelayTime = DelayTime/StrideStep CurrentStride = newValue ENDIF ' IF (doTiltFlag = 1) IF DelayTime < 255 THEN DelayTime = DelayTime + 1 ' to make up for fractions from divide 'DEBUG " DelayTime = ", DEC DelayTime, CR,LF SEROUT To_Co_Proc, Baud, [(128),DelayTime] 'write Timer 0 with delay value doneMoveFlag = 0 ' reset flag ENDIF ' IF (doneMoveFlag = 1) RETURN