' -----[ Title ]---------------------------------------------------------------- ' Home Center Walker HC_Walker.bs2 ' Toddler Program 4.2: Advanced Walking corrected and modified to use BBDI Co-Processor ' {$STAMP BS2} ' This program is taken from the one written by Parallax Inc. for their Toddler robot ' That original program is copyright Parallax, Inc. ' The changes are copyright 2002 by Blue bell Design Inc. ' -----[ I/O Definitions ]------------------------------------------------------ To_Co_Proc CON 10 ' pin for data to BBDI Co-Processor Frm_Co_Proc CON 9 ' pin for data from BBDI Co-Processor Timer_Exp VAR in11 ' Timer Expired I/O pin from BBDI Co-Processor Rst_Co_Proc CON 12 ' pin for reset to BBDI Co-Processor when wired BAUD CON 84 ' 2400 BAUD => BS2 = 396, BS2p = 1021 = CoProc V1.0 ' 9600 BAUD => BS2 = 84, BS2p = 240 ' CoProc V1.1 pin 21 pulled up = 2400 Baud ' CoProc V1.1 pin 21 grounded = 9600 Baud TiltServo CON 136 ' use Servo 0 = Tilt (136 = write setpoint command) StrideServo CON 137 ' use Servo 1 = Stride ' -----[ Constants ]------------------------------------------------------------ TILT_RAMP CON 144 ' start value to write servo 0 ramp ' (command = 128 + 16 + ch0 = 144) TiltStep CON 3 ' TiltServo step size - was 5 steps/20ms RightTilt CON 40 ' started with 61 needed more right tilt ' tried 59 + readjusted linkage - still needs more ' 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 STRIDE_RAMP CON 145 ' write servo 1 ramp = 1 more than servo 0 command StrideStep CON 3 ' 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 lf con 10 'Line Feed ' -----[ Variables ]------------------------------------------------------------ DLY_TIME VAR Byte ' # of 20 cycles to wait for move to complete SerDIn VAR DLY_TIME ' variable where Co-Processor data comes back - reused FigureLoop VAR Nib MoveLoop VAR Byte ' Loop for repeat movements MoveLoopLimit VAR Byte SubMoveLoop VAR Byte ' Loop for repeat submovements SubMoveLoopLimit VAR Byte CurrentTilt VAR Byte ' was a word CurrentStride VAR Byte ' was a word NewValue VAR Byte ' was a word Dx VAR Byte ' was a shared word Mx VAR Word ' stays a word - addresses ROM DATA MxCurrent VAR Word ' stays a word - addresses ROM DATA Sx VAR Word ' stays a word - addresses ROM DATA SxCurrent VAR Word ' stays a word - addresses ROM DATA ' -----[ Movement Support Codes ]------------------------------------------------------------ ' ' The following state tables are lists of movement state numbers. ' A xx indicates the end of a list. ' These are used with the Movement routine. TL CON 0 TC CON 1 TR CON 2 SL CON 3 SC CON 4 SR CON 5 xx CON 255 ' -----[ Movement Value Tables ]------------------------------------------------------------ ' ' These can be used with the Movement routine. ' The tables can contain Basic Movement Codes. ' ' Note: ALL movement tables must be in this section LeftSemicircle DATA 7, bLeftTurn, bLeftTurn, bForward, xx RightSemicircle DATA 7, bRightTurn, bRightTurn, bForward, xx WalkForward3 DATA 3, bForward, xx WalkForward8 DATA 8, bForward, xx WalkForward1 DATA 1, bForward, xx WalkBackward8 DATA 8, bBackward, xx PivotLeft4 DATA 4, bPivotLeft, bForward, xx ' -----[ Basic Movement Codes ]------------------------------------------------------ ' ' Used in Movement tables. ' Referenced below using LOOKUP statement. bFinish CON 0 bForward CON 1 bBackward CON 2 bLeftTurn CON 3 bRightTurn CON 4 bPivotLeft CON 5 bPivotRight CON 6 ' -----[ Basic Movement Tables ]------------------------------------------------------ ' ' These tables can contain Movement Support Codes. BasicMovements CON Forward Forward DATA 1, TR, SL, TL, SR, xx Backward DATA 1, TR, SR, TL, SL, xx ' in error in original code LeftTurn DATA 1, TL, SR, TC, SL, TL, SR, TR, SL, xx RightTurn DATA 1, TR, SL, TC, SR, TR, SL, TL, SR, xx PivotLeft DATA 3, TL, SR, TC, SL, TR, SR, TC, SL, xx PivotRight DATA 3, TR, SL, TC, SR, TL, SL, TC, SR, xx Finish DATA 1, TR, SC, TC, xx ' -----[ Initialization Code ]-------------------------------------------------- SEROUT To_Co_Proc, BAUD, [116] SEROUT To_Co_Proc, BAUD, [116] 'Reset Co-Processor LOW Rst_Co_Proc 'Reset Co-Processor if wired to port PAUSE 20 HIGH Rst_Co_Proc SEROUT To_Co_Proc, BAUD, [TILT_RAMP,(TiltStep*4)] 'write rate to servo 0 SEROUT To_Co_Proc, BAUD, [STRIDE_RAMP,(StrideStep*4)] 'write rate to servo 1 GOSUB ResetCC 'debug "Forward = ", hex Forward, cr,lf 'debug "Backward = ", hex Backward, cr,lf 'debug "LeftTurn = ", hex LeftTurn, cr,lf 'debug "RightTurn = ", hex RightTurn, cr,lf 'debug "PivotLeft = ", hex PivotLeft, cr,lf 'debug "PivotRight = ", hex PivotRight, cr,lf 'debug "LeftSemicircle = ", hex LeftSemicircle, cr,lf 'debug "RightSemicircle = ", hex RightSemicircle, cr,lf 'debug "WalkForward3 = ", hex WalkForward3, cr,lf 'debug "WalkForward8 = ", hex WalkForward8, cr,lf 'debug "WalkForward1 = ", hex WalkForward1, cr,lf 'debug "WalkBackward8 = ", hex WalkBackward8, cr,lf 'debug "PivotLeft4 = ", hex PivotLeft4, cr,lf 'debug "Finish = ", hex Finish, cr,lf LOW 0 ' for 'debugging - lights if Done is hit *********** ' -----[ Main Code ]------------------------------------------------------------ Main_Program: GOSUB ResetCC ' Make a Figure 8 FOR FigureLoop = 1 to 5 Mx = LeftSemicircle GOSUB Movement Mx = WalkForward3 GOSUB Movement Mx = RightSemicircle GOSUB Movement Mx = WalkForward3 GOSUB Movement NEXT ' Make a big polygon FOR FigureLoop = 1 to 5 Mx = PivotRight GOSUB Movement Mx = WalkForward8 GOSUB Movement NEXT Mx = Finish GOSUB Movement END ' -----[ Subroutines ]---------------------------------------------------------- ' ----- Movement: Move feet using DATA table referenced by Mx ----- ' ' Input: Mx = movement table index, table ends in xx ' or ' Mx = submovement table index, table ends in xx ' ' Note: All submovement tables come after the movment tables in this file. Movement: IF Mx < BasicMovements THEN SetupMovement 'debug cr,"use submovements",cr Sx = Mx ' setup to use submovement table only GOSUB StartSubMovement ' only one pass through submovement table RETURN SetupMovement: READ Mx, MoveLoopLimit ' read movement table repeat count Mx = Mx + 1 StartMovement: FOR MoveLoop = 1 to MoveLoopLimit 'debug cr,cr, hex (Mx-1), " Start Movement ", dec MoveLoop, " of ", dec MoveLoopLimit,cr MxCurrent = Mx ' start of movements MovementLoop: READ MxCurrent, Sx ' read next submovment byte MxCurrent = MxCurrent + 1 'debug cr, " SX = ", dec Sx, " movement" IF Sx = xx THEN MovementDone ' skip if end of list LOOKUP Sx,[Finish,Forward,Backward,LeftTurn,RightTurn,PivotLeft,PivotRight],Sx ' lookup submovement table index GOSUB StartSubMovement GOTO MovementLoop MovementDone: NEXT RETURN '-------- StartSubMovement: ' start executing submovement table READ Sx, SubMoveLoopLimit ' read submovement table repeat count Sx = Sx + 1 FOR SubMoveLoop = 1 to SubMoveLoopLimit 'debug cr, " SX = ", hex (Sx-1), " submovement ", dec SubMoveLoop, " of ", dec SubMoveLoopLimit, " ", cr SxCurrent = Sx SubMovementLoop: READ SxCurrent, Dx ' read next submovement action SxCurrent = SxCurrent + 1 IF Dx = xx THEN SubMovementDone ' skip if end of list GOSUB DoMovement ' execute movement GOTO SubMovementLoop SubMovementDone: NEXT RETURN DoMovement: BRANCH Dx,[TiltLeft,TiltCenter,TiltRight,StrideLeft,StrideCenter,StrideRight] ' will fall through if invalid index RETURN ' ---- Movement routines can be called directly as subroutines TiltLeft: NewValue = LeftTilt 'debug "TL, " TL_Dly: DLY_TIME = NewValue - CurrentTilt GOTO MovementTilt '2-3 bytes TiltCenter: NewValue = CenterTilt 'debug "TC, " IF CurrentTilt = RightTilt THEN TL_Dly 'jump if new>current GOTO TR_Dly 'new <= current TiltRight: NewValue = RightTilt 'debug "TR, " TR_Dly: DLY_TIME = CurrentTilt - NewValue MovementTilt: SEROUT To_Co_Proc, BAUD, [136,NewValue] 'Set Servo 0 = Tilt = 8 bytes DLY_TIME = DLY_TIME/TiltStep CurrentTilt = NewValue '4 bytes GOTO Set_Delay StrideLeft: NewValue = LeftStride 'debug "SL, " SL_Dly: DLY_TIME = NewValue - CurrentStride GOTO MovementStride StrideCenter: NewValue = CenterStride 'debug "SC, " IF CurrentStride = RightStride THEN SL_Dly 'jump if new>current GOTO SR_Dly 'new <= current GOTO MovementStride StrideRight: NewValue = RightStride 'debug "SR, " SR_Dly: DLY_TIME = CurrentStride - NewValue MovementStride: SEROUT To_Co_Proc, BAUD, [137,NewValue] 'Set Servo 1 = Stride DLY_TIME = DLY_TIME/StrideStep CurrentStride = NewValue ' USE TIMER TO DELAY HERE Set_Delay: ' write timer 0 IF DLY_TIME > 254 THEN too_big DLY_TIME = DLY_TIME + 1 ' to make up for fractions too_big: 'debug " DLY_TIME = ", DEC DLY_TIME, CR,LF SEROUT To_Co_Proc, BAUD, [(128),DLY_TIME] 'command to load Timer 0 with delay value check_timers: IF Timer_Exp = 0 THEN Do_Good_Stuff 'Check for timer not complete yet '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 " DLY_TIME = expired, SerDIn = ",HEX SerDIn, " i = ", DEC i,CR RETURN Chk_Other_Timers: ' If any other timers are in use, ' add your code to check them here. Chk_for_Timer0: IF (SerDIn & $01) THEN Done_Waiting 'Jump if Timer 0. ' if it might have been other timers instead. ' ****************************** Do_Good_Stuff: 'YOU CAN DO USEFUL STUFF HERE WHILE WAITING FOR THE TIMER (AND SERVOS) TO FINISH!!! ' DON'T FORGET TO PERIODICALLY COME BACK TO KEEP CHECKING IF TIMER FINISHED. ' YOU CAN SPRINKLE "IF Timer_Exp = 1 THEN check_timers" TO DO IT. JUST DON'T FORGET TO ' REMEMBER THE STATE YOU LEFT FROM. FSMs WORK GREAT FOR THAT. ' 'debug " I'M BORED, GIVE ME MORE TO DO!!! ",cr GOTO check_timers ' ****************************** Done_Waiting: 'Go here when all timer work is done No_Delay: 'Leaves lots of time to do other things - RETURN ' ----- Move feet to initial center position ----- ResetCC: CurrentTilt = CenterTilt CurrentStride = CenterStride SEROUT To_Co_Proc, BAUD, [136,CurrentTilt] 'Set Servo 0 = Tilt SEROUT To_Co_Proc, BAUD, [137,CurrentStride] 'Set Servo 1 = Stride 'debug "resetCC", CR, lf PAUSE 2000 ' 2 seconds at start to allow handling DoReturn: RETURN