[Robotgroup] Robot Head: BS2 Stamp programming Help?!

[LHudson]

This time, with spaces:

' {$STAMP BS2}
' {$PBASIC 2.5}
 
' ##### Declare the Command Constants #####
 
  ' ~ Speech Commands
  '
  CMD_Mouth_Pin2_ON     CON 21
  CMD_Mouth_Pin2_OFF    CON 20
  CMD_Mouth_Pin1_ON     CON 11
  CMD_Mouth_Pin1_OFF    CON 10
  CMD_Mouth_Pin4_ON     CON 41
  CMD_Mouth_Pin4_OFF    CON 40
  CMD_Mouth_Pin5_ON     CON 51
  CMD_Mouth_Pin5_OFF    CON 50
 
  ' ~ Movement Commands
  '
  CMD_Move_FORWARD      CON 111
  CMD_Move_BACKWARD     CON 100
  CMD_Move_Rotate_LEFT  CON 101
  CMD_Move_Rotate_RIGHT CON 110
  CMD_Move_STOP         CON 199
 
  ' ~ Pan and Tilt Commands
  '
  ' Command Arguments >= 1000 are reserved for Pan and Tilt Commands
  ' The Constant value (0 to 130) times 10 is equal to the PULSOUT rate
  ' which determines the servo position.
  '
  ' The First Digit determines which servo - 1: Pan, 2: Tilt, e.g.:
  ' 1065 - Centers the Pan servo
  ' 2065 - Centers the Tilt servo
  ' 1130 - Rotates the head all the way to the Right
  ' 2130 - Tilts the head all the way Back
  ' 1000 - Rotates the head all the way to the Left
  ' 2000 - Tilts the head all the way to the Front
  
' ##### Initialize Runtime #####
 
  BAUD      CON 84
  COMDATA   VAR Word
  p         VAR Word
 
  ' ##### Initialize Speech #####
 
  FOR p = 1 TO 3
    HIGH 2: PAUSE 100
    HIGH 1: PAUSE 100
    HIGH 4: PAUSE 100
    HIGH 5: PAUSE 400
 
    LOW 2
    LOW 1
    LOW 4
    LOW 5
  NEXT
 
' ##### Initialize Pan and Tilt #####
 
  Component_Arg   VAR Bit
  Center_Arg      CON 650
 
  PIN_PanTilt_Pan  PIN 14
  POS_PanTilt_Pan  VAR Word
  CMD_PanTilt_Pan  VAR Word
 
  PIN_PanTilt_Tilt PIN 15
  POS_PanTilt_Tilt VAR Word
  CMD_PanTilt_Tilt VAR Word
  
  ' Center the Servos
  POS_PanTilt_Pan = Center_Arg
  POS_PanTilt_Tilt = Center_Arg
 
  FOR p = 0 TO 255
    PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
    PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
  NEXT
 
' ##### Loop to get the Commands #####
 
  DO
    '~ SERIN 16\0, BAUD, [DEC COMDATA]
    DEBUG CR, "Enter Command: "
    DEBUGIN DEC COMDATA
    DEBUG CR, "Command = ", COMDATA
 
    IF COMDATA < 100 THEN
 
    ' !-- Execute a Speech Command
      DEBUG "Speech Command: ", COMDATA
 
      SELECT COMDATA
        CASE CMD_Mouth_Pin2_ON:  HIGH 2
        CASE CMD_Mouth_Pin2_OFF:  LOW 2
        CASE CMD_Mouth_Pin1_ON:  HIGH 1
        CASE CMD_Mouth_Pin1_OFF:  LOW 1
        CASE CMD_Mouth_Pin4_ON:  HIGH 4
        CASE CMD_Mouth_Pin4_OFF:  LOW 4
        CASE CMD_Mouth_Pin5_ON:  HIGH 5
        CASE CMD_Mouth_Pin5_OFF:  LOW 5
      ENDSELECT
 
    ELSEIF COMDATA < 1000 THEN
 
    ' !-- Execute a Movement Command
      DEBUG "Movement Command: ", COMDATA
 
    ELSE
 
    ' !-- Execute a Pan or Tilt Command
      IF COMDATA < 2000 THEN
        CMD_PanTilt_Pan = 10 * (COMDATA - 1000)
        DEBUG "Pan Command: ", COMDATA
 
        ' Adjust the Pan to the Command argument
        IF POS_PanTilt_Pan < CMD_PanTilt_Pan THEN
          DO UNTIL POS_PanTilt_Pan = CMD_PanTilt_Pan
            PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
            POS_PanTilt_Pan = POS_PanTilt_Pan + 5
          LOOP
        ENDIF
        IF POS_PanTilt_Pan > CMD_PanTilt_Pan THEN
          DO UNTIL POS_PanTilt_Pan = CMD_PanTilt_Pan
            PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
            POS_PanTilt_Pan = POS_PanTilt_Pan - 5
          LOOP
        ENDIF
 
      ELSE
        CMD_PanTilt_Tilt = 10 * (COMDATA - 2000)
        DEBUG "Tilt Command: ", COMDATA
 
        ' Adjust the Tilt to the Command argument
        IF POS_PanTilt_Tilt < CMD_PanTilt_Tilt THEN
          DO UNTIL POS_PanTilt_Tilt = CMD_PanTilt_Tilt
            PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
            POS_PanTilt_Tilt = POS_PanTilt_Tilt + 5
          LOOP
        ENDIF
        IF POS_PanTilt_Tilt > CMD_PanTilt_Tilt THEN
          DO UNTIL POS_PanTilt_Tilt = CMD_PanTilt_Tilt
            PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
            POS_PanTilt_Tilt = POS_PanTilt_Tilt - 5
          LOOP
        ENDIF
      ENDIF
 
    ENDIF
 
    'PULSOUT PIN_PanTilt_Pan, POS_PanTilt_Pan
    'PULSOUT PIN_PanTilt_Tilt, POS_PanTilt_Tilt
  LOOP