PBasic2Highlighter Test

' {$STAMP BS2}
' {$PBASIC 2.5}

' What does it do?

' Copyright 2006 Tom Most


' Oooo!  Conditional compilation!
#IF (anything)
  #ERROR "Why are you trying to compile this code?"
#ENDIF

DEBUG "#STAMP Started", CR


' ==== Constants ====
' Serial communication
Serial       CON      16
SerialMode   CON      16468
Timeout      CON      3000  ' 3 seconds
' Servo pins
Servo12      PIN      12
Servo13      PIN      13
Servo14      PIN      14
Servo15      PIN      15
' Command bytes
CommandStart CON      ">"
TurnOff      CON      "1"
Servo12Set   CON      "2"
Servo13Set   CON      "3"
Servo14Set   CON      "4"
Servo15Set   CON      "5"
CommandEnd   CON      CR


' ==== Variables ====
' The pulse widths to the servos
s12pulse     VAR      Word
s13pulse     VAR      Word
s14pulse     VAR      Word
s15pulse     VAR      Word
' Command variables
command      VAR      Byte
body         VAR      Word
bodyHigh     VAR      body.HIGHBYTE
bodyLow      VAR      body.LOWBYTE


' ==== Init ====
s12pulse = 750
s13pulse = 750
s14pulse = 750
s15pulse = 750

' ==== Main ====
Main:
  DEBUG CR, "#Main loop", CR
  ' Get any commands.
  GOTO Get_Commands
  ' Pulse 'em
  GOTO Pulse_Servos
GOTO Main


' Repeatedly get commands until no more are 
' sent (then go to No_Command).
Get_Commands:
  SEROUT Serial, SerialMode, ["<ready", CR]
  SERIN Serial, SerialMode, 20, No_Command,
      [ WAIT(CommandStart), command,
        WAIT(" "), DEC body, WAIT(CR) ]
  DEBUG "#Command recieved:", CR
  DEBUG "#", ? command
  DEBUG "#", ? body

  ' Interpret the command.
  SELECT command
    CASE Servo12Set
      s12pulse = body
      DEBUG "#", ? s12pulse
    CASE Servo13Set
      s13pulse = body
      DEBUG "#", ? s13pulse
    CASE Servo14Set
      s14pulse = body
      DEBUG "#", ? s14pulse
    CASE Servo15Set
      s15pulse = body
      DEBUG "#", ? s15pulse
    CASE TurnOff
      DEBUG "#Shutting down.", CR
      END
    CASE ELSE
      DEBUG "#Invalid command: ", command, CR
  ENDSELECT

  GOTO Get_Commands


No_Command:
  DEBUG "#No Command", CR
  DEBUG "<timeout", CR
  GOTO Pulse_Servos
  GOTO Main


Pulse_Servos:
  DEBUG "#Pulsing servos", CR
  PULSOUT Servo12, s12pulse
  PULSOUT Servo13, s13pulse
  PULSOUT Servo14, s14pulse
  PULSOUT Servo15, s15pulse
  GOTO Main