'  Move forward
'
' { $STAMP BS2 }
' {$PBASIC 2.5}

'|||||||||||||||||| Variables ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||


PauseTime VAR WORD                    '  Create a 16 Bit Variable
CountMoves VAR NIB                    '  Create a  4 Bit Variable
RobotData VAR BYTE                    '  Data Byte to Send to/Receive from Robot


'||||||||||||||||||| Constants ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

RobotStop     CON  0      'Stop the Robot
RobotForward  CON  5      '  Move Forward for 200 msecs
RobotReverse  CON  6      '  Move Reverse for 200 msecs
RobotLeft     CON  7      '  Turn Left for 200 msecs
RobotRight    CON  8      '  Turn Right for 200 msecs
RobotPWM0     CON 11      '  PWM = 0% Duty Cycle
RobotPWM1     CON 12      '  PWM = 1st "Notch"
RobotPWM2     CON 13      '  PWM = 2nd "Notch"
RobotPWM3     CON 14      '  PWM = 3rd "Notch"
RobotPWM4     CON 15      '  PWM = 100% Duty Cycle
LED CON 11        '  LED, Negative Active On
SC CON 14         '  Define the Serial communications Clock Pin
SD CON 15         '  Define the Serial communications Data Pin


'|||||||||||||||||||| Main Code |||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||

  HIGH SC                             '  Set the I/O Bits As O/P - Required by the 505
  HIGH SD                             '   and High               - Required by the 505
  HIGH LED                            '  Turn off the LED

  PauseTime = 80                     ' msecs between commands
  PAUSE 100                             '  Time for the electronic circuits to startup

  RobotData = RobotPWM0               ' Set the speed to 0 of 4 since Behavior 1 will probably start.
  GOSUB RobotSend


  RobotData = RobotPWM4               ' Set the speed to 0 of 4 since Behavior 1 will probably start.
  GOSUB RobotSend

FOR CountMoves = 0 TO 9               ' Do the loop 10 times
  RobotData = RobotForward            ' Move forward
    GOSUB RobotSend
  PAUSE PauseTime                     ' Wait msecs between RobotSend commands
NEXT

  RobotData = RobotStop               ' Stop the Robot
  GOSUB RobotSend

DO                                    ' Infinite Do loop
  PAUSE 10                            ' Wait for 5 seconds
LOOP




RobotSend:        '  Send the Byte in "RobotData"
  LOW SC          '  Hold Low for 1 msec before
  PAUSE 1         '   Shifting in Data
  SHIFTOUT SD, SC, LSBFIRST, [RobotData]
  HIGH SC
RETURN






 