步行机器人程序

' -----[ Title ]-----------------------------------------------------------
' Toddler Program 2.4: First Steps Forward with State Transitions
' Keeps track of starting and ending states
' {$STAMP BS2}
' {$PBASIC 2.5}
' -----[ I/O Definitions ]-------------------------------------------------
TiltServo CON 13 ' Tilt servo on P12 StrideServo CON 12 ' Stride servo on P13
' -----[ Constants ]-------------------------------------------------------
MoveDelay CON 12 ' in micrcoseconds StrideStep CON 5 ' StrideServo step size TiltStep CON 5 ' TiltServo step size
RightTilt CON 620 ' Tilt limits
CenterTilt CON 750
LeftTilt CON 880
RightStride CON 650 ' Stride limits
CenterStride CON 750
LeftStride CON 850
' -----[ Variables ]-------------------------------------------------------
MoveLoop V AR Nib ' Loop for repeat movements Pulses V AR Word ' Pulse variable
CurrentTilt V AR Word
CurrentStride V AR Word
NewValue V AR Word
Dx V AR Pulses
Mx V AR Word
' -----[ EEPROM Data ]-----------------------------------------------------
' Take three full steps.
'
' 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
Forward DATA TR, SL, SR, SL,TL,SR,SL,SR,SC,TC,xx
WalkForward DATA TR, SL, SR,SC,TL,SR,SL,SR,SC,TC, xx
WalkBackward DA TA TR, SR, TL, SL,SC,TC, TR, SL, TL, SR ,xx
TurnLeft DATA TL, SR, TC, SL, TC,SC, TR, SL, TL, SR, TC,SC,TR,SL,TC,SR, TC,SC TL, SR, TC, SL,xx
FinishForward DATA TR, SC, TC, xx
' -----[ Main Routine ]----------------------------------------------------
'
Main_Program:
GOSUB ResetCC
FOR MoveLoop = 1 TO 3
Mx = WalkForward
GOSUB Movement
NEXT
FOR MoveLoop = 1 TO 3
Mx = TurnLeft
GOSUB Movement
NEXT
FOR MoveLoop = 1 TO 3
Mx = WalkBackward
GOSUB Movement
NEXT
Mx = FinishForward
GOSUB Movement
END
' -----[ Subroutines ]-----------------------------------------------------
' ----- Movement: Move feet using DATA table referenced by Mx -----
'
' Input: Mx = table index, table ends in xx
Movement:
READ Mx, Dx ' read next action
Mx = Mx + 1
IF Dx = xx THEN MovementDone ' skip if end of list
GOSUB DoMovement ' execute movement GOTO Movement ' loop until done
DoMovement:
BRANCH Dx,[TiltLeft,TiltCenter,TiltRight,StrideLeft,
StrideCenter,StrideRight]
' fall through if invalid
' index MovementDone:
RETURN
' ---- Movement routines can be called directly ----
TiltLeft:
NewValue = LeftTilt
GOTO MovementTilt
TiltCenter:
NewValue = CenterTilt
GOTO MovementTilt
TiltRight:
NewValue = RightTilt
MovementTilt:
FOR Pulses = CurrentTilt TO NewValue STEP TiltStep
PULSOUT TiltServo, Pulses
PULSOUT StrideServo, CurrentStride
PAUSE MoveDelay
NEXT
CurrentTilt = NewValue
RETURN
StrideLeft:
NewValue = LeftStride
GOTO MovementStride
StrideCenter:
NewValue = CenterStride
GOTO MovementStride
StrideRight:
NewValue = RightStride
MovementStride:
FOR Pulses = CurrentStride TO NewValue STEP StrideStep
PULSOUT TiltServo, CurrentTilt
PULSOUT StrideServo, Pulses
PAUSE MoveDelay
NEXT
CurrentStride = NewValue
RETURN
' ----- Move feet to initial center position -----
ResetCC:
CurrentTilt = CenterTilt
CurrentStride = CenterStride
FOR Pulses = 1 TO 100 STEP StrideStep
PULSOUT TiltServo, CenterTilt
PULSOUT StrideServo, CenterStride
PAUSE MoveDelay
NEXT
DoReturn:
RETURN
说明：该程序实现的是步行机器人跳舞（起始动作站立，然后身体右偏，左腿向前向后，身体左偏，右腿向前向后、向前走两步之后向后走两步、左转向前走再右转再左转、最后停止的操作。

）。