'------- [Quad_RC.bs2 1.0] -----------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... Quad_RC.bs2.BS2
'   Purpose.... Control the QuadCrawler with a Tower Hobbies
'               Tx/Rx.
'   Author..... CustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 20 August 2004
'   Updated.... 19 November 2004
'
'
'   Hardware
'   (1) QuadCrawler Robotic Kit
'   (1) Parallax BOE
'   (1) Parallax Basic Stamp II
'   (1) Parallax PSC
'   (1) Tower Hobbies 6 channel FM Radio Control System
'
' Quad_RC.BS2 is a combination of
' * QuadWalking_Engine_Select_Gait.bs2
' * RC_Test.BS2
'
' http://www.crustcrawler.com/code/index.php?id=20#quad
'
'=========================================================================
' Getting Started
'=========================================================================
' 1. Change the default leg centers in this program with the legs
'    centers found in TuneHex.BS2.
'    http://www.crustcrawler.com/code/quad/TuneQuad.bs2
'
' Find the section of code below
'-----[ Horizontal Leg Constants ]----------------------------------------
' Center1         CON     750     ' Leg1 horizontal servo
' Center2         CON     750     ' Leg2 horizontal servo
' Center3         CON     750     ' Leg3 horizontal servo
' Center4         CON     750     ' Leg4 horizontal servo
'
' Replace these constants with the constants
' you found using the TuneHex.bs2 program.
'
' Example QuadCrawler center constants (yours will vary)
'-----[ Horizontal Leg Constants ]----------------------------------------
'Center1         CON     725
'Center2         CON     800
'Center3         CON     785
'Center4         CON     750
'
' 2. Read the comments in RC_Test.BS2.
'    http://www.crustcrawler.com/code/interface/RC_Test.bs2
'
' 3. Run the RC_Test.BS2 program to verify your Receiver to
'    BOE connections are functioning.
'
'=========================================================================
' How to use this program
'=========================================================================
' Move the joystick to control the robot.
'
' Stick position vs. gait
'
'             Left        Straight      Right
'          ______________________________________
'         |            |            |            |
'         |  Gradual   |  Fast      |  Gradual   |
'         |  Left      |  Forward   |  Right     | Fast
'         |------------|------------|------------|
'         |  Gradual   |  Slow      |  Gradual   |
'         |  Left      |  Forward   |  Right     | Slow
'         |------------|------------|------------|
'         |  Spin      |   Stop     |  Spin      |
'         |  Left      |            |  Right     | Center
'         |------------|------------|------------|
'         |  Gradual   |  Slow      |  Gradual   |
'         |  Left      |  Backward  |  Right     | Slow
'         |------------|------------|------------|
'         |  Gradual   |  Fast      |  Gradual   |
'         |  Left      |  Backward  |  Right     | Fast
'         |____________|____________|____________|
'
'-------------------------------------------------------------------------
' -----[ I/O Definitions ]------------------------------------------------
PSC             PIN     15              ' PSC module
#IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN
  Baud          CON     1021            ' 2400 baud
#ELSE
  baud          CON     33164           ' 2400 baud
#ENDIF
StickXPin       PIN     13              ' Left/Right(X) joystick Rx Ch4
StickYPin       PIN     12              ' Up/Down(Y) joystick Rx Ch3

'---- [Walking Variables] ------------------------------------------------
servoAddr       VAR     Byte            ' Servo addresses
ptrEEPROM       VAR     Word            ' Gait select
servoPosition   VAR     Word            ' Servo Position
temp            VAR     Byte
ramp            VAR     Byte            ' Ramp used in SEROUT
rightRamp       VAR     Byte            ' Right side ramp values
leftRamp        VAR     Byte            ' Left side ramp values
gaitCode        VAR     Byte            ' Selected gaitCode
'---- [RC Vars] ----------------------------------------------------------
stickXPos       VAR     Word            ' Left/Right (X) joystick posn
stickYPos       VAR     Word            ' Up/Down(Y) joystick position

'----- [Adjustable Ramp Values] ------------------------------------------
'Off  fast >>>>> slow
' 0    $1  >>>>> $3F(63)
LiftRamp        CON     $1              ' Vertical servo ramp
VeryFast        CON     $8              ' Walk very fast
Fast            CON     $A              ' Walk fast

'-----[ Horizontal Leg Constants ]----------------------------------------
'Servo center
Center1         CON     750
Center2         CON     750
Center3         CON     750
Center4         CON     750

'-----[ Stride Calculations ]---------------------------------------------
Stride          CON     150             ' Stride units
delay           CON     Stride/5        ' Delay as a function of Stride

Leg1Center      CON     Center1
Leg1Forward     CON     Center1+Stride  ' Stride + Leg1 Center
Leg1Back        CON     Center1-Stride

Leg2Center      CON     Center2
Leg2Forward     CON     Center2+Stride
Leg2Back        CON     Center2-Stride

Leg3Center      CON     Center3
Leg3Forward     CON     Center3-Stride
Leg3Back        CON     Center3+Stride

Leg4Center      CON     Center4
Leg4Forward     CON     Center4-Stride
Leg4Back        CON     Center4+Stride

'----- [Adjustable vertical servo positions] --------------------------------
RaiseRight      CON     300             ' Raised and lowered
LowerRight      CON     1200            ' vertical servo values
RaiseLeft       CON     1200
LowerLeft       CON     300

'---- [Servo Address and Position EEPROM data] -----------------
' Walk Forward
Forward DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Back,
             $04,Word Leg3Center,         $06,Word Leg4Center,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Center,         $02,Word Leg2Center,
             $04,Word Leg3Forward,        $06,Word Leg4Back,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF ' end of forward

' Walk Backward
Back    DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Center,         $02,Word Leg2Center,
             $04,Word Leg3Forward,        $06,Word Leg4Back,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Back,
             $04,Word Leg3Center,         $06,Word Leg4Center,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF 'end of back

' Left Turn EEPROM values
LTurn   DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Forward+Stride, $06,Word Leg4Back+Stride,
             $04,Word Leg3Forward,        $02,Word Leg2Back,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Center+Stride,  $06,Word Leg4Center+Stride,
             $04,Word Leg3Forward,        $02,Word Leg2Back,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF ' end of left turn

' Right turn EEPROM Values
RTurn   DATA $01,Word RaiseRight,         $07,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Center-Stride,
             $04,Word Leg3Center-Stride,  $06,Word Leg4Back,
             $01,Word LowerRight,         $07,Word LowerLeft,
             $03,Word RaiseRight,         $05,Word RaiseLeft,
             $00,Word Leg1Forward,        $02,Word Leg2Back-Stride,
             $04,Word Leg3Forward-Stride, $06,Word Leg4Back,
             $03,Word LowerRight,         $05,Word LowerLeft,
             $FF  ' end of right turn

'                Raise                Center           Lower
Adjust  DATA $01,Word RaiseRight, $00,Word Center1, $01,Word LowerRight
        DATA $03,Word RaiseRight, $02,Word Center2, $03,Word LowerRight
        DATA $05,Word RaiseLeft,  $04,Word Center3, $05,Word LowerLeft
        DATA $07,Word RaiseLeft,  $06,Word Center4, $07,Word LowerLeft,
        $FF ' end of adjust legs

gaitCode = $03

Parse_GaitCode:                         ' Assign gait parameters
  SELECT gaitCode
    CASE $03                            ' Adjust Legs
      GOTO Get_Stick
    CASE $01
      ptrEEPROM = LTurn                 ' Spin left
      rightRamp = VeryFast              ' Assign ramp to
      leftRamp = VeryFast               ' left and right sides
      GOTO Walking_Engine
    CASE $02
      ptrEEPROM = RTurn                 ' Spin right
      rightRamp = VeryFast
      leftRamp = VeryFast
      GOTO Walking_Engine
  ENDSELECT

Assign_Direction:
  IF gaitCode.HIGHNIB <= $2 THEN
    ptrEEPROM = Forward
  ELSE
    ptrEEPROM = Back
  ENDIF

Assign_Ramp:
  IF (gaitCode.HIGHNIB = $1) OR (gaitCode.HIGHNIB = $4) THEN
    ramp = VeryFast
  ELSE
    ramp = Fast
  ENDIF

Assign_Left_Right_Leg_Speed:
  SELECT gaitCode.LOWNIB
    CASE $0
      rightRamp = Ramp
      leftRamp = RightRamp
    CASE $1
      rightRamp = Ramp
      leftRamp = (Ramp+$2)
    CASE $2
      leftRamp = Ramp
      rightRamp = (Ramp+$2)
  ENDSELECT

'-----[ Main Walking Routine ]------------------------------------------
Walking_Engine:
  READ ptrEEPROM,servoAddr,servoPosition.LOWBYTE,servoPosition.HIGHBYTE
  DO WHILE servoAddr <> $FF
    IF (servoAddr // 2) = 1 THEN
      Ramp = LiftRamp
    ELSE
      IF (servoAddr = $00) OR (servoAddr = $02) THEN
        Ramp = RightRamp
      ENDIF
      IF(servoAddr = $04) OR (servoAddr = $06) THEN
        Ramp = LeftRamp
      ENDIF
    ENDIF
    GOSUB Write_PSC
  LOOP
Check_RC:
  GOTO Get_Stick

'----- [Serial Out EEPROM values to PSC] -------------------------------
Write_PSC:
  SEROUT PSC,Baud,["!SC",ServoAddr,Ramp,servoPosition.LOWBYTE,
         servoPosition.HIGHBYTE, CR]
  ptrEEPROM = ptrEEPROM + 3
  READ ptrEEPROM,servoAddr,servoPosition.LOWBYTE,servoPosition.HIGHBYTE
  PAUSE delay
RETURN

' Convert Stick positions to gaitcode
Get_Stick:
  temp = gaitCode
  PULSIN StickXPin, 1, stickXPos        ' Read joystick positions
  PULSIN StickYPin, 1, stickYPos        ' from transmitter P13, P122

  IF (stickXPos & stickYPos) < 500 THEN
    GOTO Get_Stick
    'DEBUG CLS, "No RC Signal Detected"
  ELSE
    SELECT stickYPos
      CASE 580 TO 650
        gaitCode = $10
      CASE 651 TO 730
        gaitCode = $20
      CASE 731 TO 800
        gaitCode = $00
      CASE 801 TO 870
        gaitCode = $30
      CASE 871 TO 960
        gaitCode = $40
    ENDSELECT
    SELECT stickXPos
      CASE 500 TO 700
        gaitCode = gaitCode | $01
      CASE 701 TO 820
        gaitCode = gaitCode | $00
        IF gaitCode = 0 THEN
          gaitCode = $03
        ENDIF
      CASE 821 TO 999
        gaitCode = gaitCode | $02
    ENDSELECT
  ENDIF

  SELECT gaitCode
    CASE $03
      GOTO Get_Stick
    CASE temp
      ptrEEPROM = ptrEEPROM-72
      GOTO Walking_Engine
    CASE ELSE
      GOTO Parse_GaitCode
  ENDSELECT