' -----[ Title ]-----------------------------------------------------------
' Toddler Program 2.1 - First Steps Forward.bs2
' Run Movement Patters M0 to M8 to take several steps
' {$STAMP BS2}
' {$PBASIC 2.5}

' -----[ Declarations ]----------------------------------------------------

TiltStep        CON     5                       ' TiltServo step size
StrideStep      CON     5                       ' StrideServo step size
MoveDelay       CON     25                      ' in micrcoseconds

RightTilt       CON     620                     ' Tilt limits
CenterTilt      CON     750
LeftTilt        CON     880

RightForward    CON     650                     ' Stride limits
StrideCenter    CON     750
LeftForward     CON     850

StrideServo     CON     12                      ' Stride servo on P12
TiltServo       CON     13                      ' Tilt servo on P13

MoveLoop        VAR     Nib                     ' Repeat movements
Pulses          VAR     Word                    ' Pulse variable

' -----[ Initialization ]--------------------------------------------------

' -----[ Main Routine ]----------------------------------------------------
' Take three full steps.


Main_Program:
  GOSUB M0                                      ' center servos
  GOSUB M1                                      ' tilt right
  GOSUB M2                                      ' step left
    FOR MoveLoop = 1 TO 3
      GOSUB M3                                  ' tilt left
      GOSUB M4                                  ' step right
      GOSUB M5                                  ' tilt right
      GOSUB M6                                  ' step left
    NEXT
  GOSUB M3                                      ' tilt left
  GOSUB M7                                      ' center feet
  GOSUB M8                                      ' center servos
END

' -----[ Subroutines ]-----------------------------------------------------

M0:
  FOR Pulses = 1 TO 100 STEP StrideStep
    PULSOUT TiltServo, CenterTilt
    PULSOUT StrideServo, StrideCenter
    PAUSE MoveDelay
  NEXT
  RETURN

M1:
  FOR Pulses = CenterTilt TO RightTilt STEP TiltStep
    PULSOUT TiltServo, Pulses
    PULSOUT StrideServo, StrideCenter
    PAUSE MoveDelay
  NEXT
  RETURN

M2:
  FOR Pulses = StrideCenter TO LeftForward STEP StrideStep
    PULSOUT TiltServo, RightTilt
    PULSOUT StrideServo, Pulses
    PAUSE MoveDelay
  NEXT
  RETURN

M3:
  FOR Pulses = RightTilt TO LeftTilt STEP TiltStep
    PULSOUT TiltServo,Pulses
    PULSOUT StrideServo, LeftForward
    PAUSE MoveDelay
  NEXT
  RETURN

M4:
  FOR Pulses = LeftForward TO RightForward STEP StrideStep
    PULSOUT TiltServo,LeftTilt
    PULSOUT StrideServo, Pulses
    PAUSE MoveDelay
  NEXT
  RETURN

M5:
  FOR Pulses = LeftTilt TO RightTilt STEP TiltStep
    PULSOUT TiltServo,Pulses
    PULSOUT StrideServo, RightForward
    PAUSE MoveDelay
  NEXT
  RETURN

M6:
  FOR Pulses = RightForward TO LeftForward STEP StrideStep
    PULSOUT TiltServo,RightTilt
    PULSOUT StrideServo, Pulses
    PAUSE MoveDelay
  NEXT
  RETURN


M7:
 FOR Pulses = LeftForward TO StrideCenter STEP StrideStep
    PULSOUT TiltServo,LeftTilt
    PULSOUT StrideServo, Pulses
    PAUSE MoveDelay
  NEXT
  RETURN

M8:
  FOR Pulses = LeftTilt TO CenterTilt STEP TiltStep
    PULSOUT TiltServo,Pulses
    PULSOUT StrideServo, StrideCenter
    PAUSE MoveDelay
  NEXT
RETURN