' www.crustcrawler.com
' Copyright 2006 CrustCrawler Inc.
'
'------- [ QuadRodRc.bs2 ] ----------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... QuadRodRc.bs2
'   Purpose.... Investigate program parameters.
'               Create and test custom walking gaits.
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   Help....... http://forum.crustcrawler.com/
'   Started.... 05/03/2006
'   Updated.... 03/28/2007
'   Version.... 2.0
'
'   Hardware
'   (1) HexRod Robotic Kit
'   (1) Parallax BOE
'   (1) Parallax Basic Stamp II
'   (1) Parallax PSC
'   (1) Standard Radio Controller
'
'=========================================================================
' Updates 03/28/2007
'=========================================================================
' * Set delay to fire after reading each row in the EEPROM tables
'   and NOT after reading each column.
'
'=========================================================================
' Overview
'=========================================================================
' Control the Rod with a standard radio controller system.
'
'=========================================================================
' Getting Started
'=========================================================================
' Please read and run RC_Test.bs2 to test your BOE to Rx connections
' before running this program
'
'=========================================================================
' Connections
'=========================================================================
' You will need 4 wires to connect the receiver
' to the BOE's X5 servo port (see your BOE documentation).
' Soldering 8 High Density Female RS232 Sockets to the ends
' of four 20 gage wires works great.
'
' Connect the receiver's:
' 1. Throttle signal line (ch3)to X5 Pin 12 (outside pin) on the BOE.
' 2. Throttle ground (ch3)to X5 Pin 12 (inside pin) on the BOE
' 3. Rudder signal line (ch4) to X5 Pin 13 (outside pin)
' 4. Rudder ground (ch4) to X5 Pin 13 (inside pin) on the BOE
'
'            X5       X4
'        |--------|--------|  |------|
' Ground |  c  c  |  x  x  |  | 5V   |----
' N/C    |  x  x  |  x  x  |  | Reg  |----
' Signal |  c  c  |  x  x  |  |      |----
'        |--------|--------|  |------|
'           12 13    14 15
'-------------------------------------------------------------------------

' -----[ I/O Definitions ]------------------------------------------------
PSC             PIN     15          ' PSC module
StickXPin       PIN     13          ' Left/Right(X) joystick Rx Ch4
StickYPin       PIN     12          ' Up/Down(Y) joystick Rx Ch3

' ----[ Set baud for all STAMP versions ]---------------------------------
#SELECT $STAMP
  #CASE BS2SX, BS2P
    Baud        CON     1021 + $8000
  #CASE BS2PX
    Baud        CON     1646 + $8000
  #CASE #ELSE
    Baud        CON     396 + $8000
#ENDSELECT

'---- [Walking Variables] ------------------------------------------------
stickXPos       VAR     Word                ' X joystick position
stickYPos       VAR     Word                ' Y joystick position
ptrEEPROM       VAR     Word                ' Gait select
servoPosition   VAR     Word                ' Servo Position
temp            VAR     Word                ' Workspace
vel             VAR     Byte                ' Velocity
dir             VAR     Nib                 ' Direction
servoAddr       VAR     Byte                ' Servo addresses
ramp            VAR     Byte                ' Ramp used in SEROUT
rightRamp       VAR     Byte                ' Right side ramp values
leftRamp        VAR     Byte                ' Left side ramp values
limiter         VAR     Byte                ' Reduce stride
delay           VAR     Byte                ' Wait
rowCounter      VAR     Nib                 ' Count EERROM rows

'-----[ Horizontal Leg Constants ]----------------------------------------
' Adjustable leg centers
' Right Side - Increase value to move leg forward
Center1         CON     900  'Front
Center2         CON     700  'Back
'Left Side
Center3         CON     600  'Front
Center4         CON     800  'Back

'-----[ Stride Calculations ]---------------------------------------------
'/////////////////////////////////////////////////////////////////////////
' The stride value determines the maximum value to move a horizontal
' servo from center.  So a stride value of 200 means move 200 units
' plus servo center and 200 units minus servo center or 200 + 200
' units.
' 5.5556 units = 1 degree
' a Stride of 200 = 72 degrees or
' (200*2)/5.556 = 72
'/////////////////////////////////////////////////////////////////////////
Stride          CON     200

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

LiftRamp        CON     $1                    ' Vertical servo ramp

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

'----- [Direction Constants] ---------------------------------------------
Ahead           CON     0
ForwardRight    CON     1
RightSpin       CON     2
BackRight       CON     3
Backward        CON     4
BackLeft        CON     5
LeftSpin        CON     6
ForwardLeft     CON     7

'----- [ App Constants ]---------------------------------------------------
EOD             CON     $FF                 ' End of data block

'----- [EEPROM DATA ] ----------------------------------------------------
'                  'Right                 'Left
Forward DATA $01,Word RaiseRight,    $07,Word RaiseLeft,
             $00,Word Leg1Forward,   $06,Word Leg4Forward,  'Reset
             $02,Word Leg2Back,      $04,Word Leg3Back,     'Thrust
             $01,Word LowerRight,    $07,Word LowerLeft,

             $03,Word RaiseRight,    $05,Word RaiseLeft,
             $00,Word Leg1Back,      $06,Word Leg4Back,     'Thrust
             $02,Word Leg2Forward,   $04,Word Leg3Forward,  'Reset
             $03,Word LowerRight,    $05,Word LowerLeft,
             EOD ' end of Forward

Back    DATA $01,Word RaiseRight,    $07,Word RaiseLeft,
             $00,Word Leg1Back,      $06,Word Leg4Back,
             $02,Word Leg2Forward,   $04,Word Leg3Forward,
             $01,Word LowerRight,    $07,Word LowerLeft,

             $03,Word RaiseRight,    $05,Word RaiseLeft,
             $00,Word Leg1Forward,   $06,Word Leg4Forward,  '
             $02,Word Leg2Back,      $04,Word Leg3Back,     '
             $03,Word LowerRight,    $05,Word LowerLeft,
             EOD ' end of Back

' Left Turn EEPROM values
LSpin DATA   $01,Word RaiseRight,    $07,Word RaiseLeft,
             $00,Word Leg1Forward,   $06,Word Leg4Back,     '
             $02,Word Leg2Back,      $04,Word Leg3Forward,  '
             $01,Word LowerRight,    $07,Word LowerLeft,

             $03,Word RaiseRight,    $05,Word RaiseLeft,
             $00,Word Leg1Back,      $06,Word Leg4Forward,  '
             $02,Word Leg2Forward,   $04,Word Leg3Back,     '
             $03,Word LowerRight,    $05,Word LowerLeft,
             EOD ' end of LSpin

' Right turn EEPROM Values
RSpin  DATA  $01,Word RaiseRight,    $07,Word RaiseLeft,
             $00,Word Leg1Back,      $06,Word Leg4Forward,  '
             $02,Word Leg2Forward,   $04,Word Leg3Back,     '
             $01,Word LowerRight,    $07,Word LowerLeft,

             $03,Word RaiseRight,    $05,Word RaiseLeft,
             $00,Word Leg1Forward,   $06,Word Leg4Back,     '
             $02,Word Leg2Back,      $04,Word Leg3Forward,  '
             $03,Word LowerRight,    $05,Word LowerLeft,
             EOD ' end of RSpin

'                Raise                Center           Lower
Neutral 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,
        EOD ' end of adjust legs
'_________________________________________________________________________

' 0 = normal mode
' 1 = debug mode
#DEFINE debugMode = 0

'Set the Quad to neutral
Reset_PSC:                                  ' Reset the Rod legs
  ptrEEPROM = Neutral
  limiter = 0
  GOTO Main
Initialize:                                 ' Init values
  ' Init limiter
  limiter = 75
  ' Inti velocity
  vel = 3
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
'Set Speed and assign EEPROM gait block
'
' Parameters (StickXPin, StickYPin)
' Return (vel, ptrEEPROM, delay, leftRamp, rightRamp)
'/////////////////////////////////////////////////////////////////////////
Parse_Stick_Position:
' Parse Y stick
'  assign direction and velocity
  SELECT stickYPos
    CASE 580 TO 650
      vel = 3
      dir = ahead
    CASE 651 TO 730
      vel = 7
      dir = ahead
    CASE 731 TO 800
      vel = 64
      'ptrEEPROM = neutral
    CASE 801 TO 870
      vel = 7
      dir = Backward
    CASE 871 TO 960
      vel = 3
      dir = Backward
  ENDSELECT
  'Assing ramp
  GOSUB Equal

' Parse X and assign turning parameters
  SELECT stickXPos
    CASE 500 TO 700
      IF vel = 64 THEN
        dir = RightSpin
        'DEBUG CLS, "SPinRight"
      ELSE
        GOSUB SlowRight
        'DEBUG CLS, "SlowRight"
      ENDIF
    CASE 701 TO 820
    'If the sick in the neutral position, check for for RC signal
      IF vel = 64 THEN GOTO RC_Signal_Check
    CASE 821 TO 999
      IF vel = 64 THEN
        dir = LeftSpin
        'DEBUG CLS, "SPinLeft"
      ELSE
        GOSUB SlowLeft
        'DEBUG CLS, "SlowLeft"
      ENDIF
  ENDSELECT

'/////////////////////////////////////////////////////////////////////////
' Set direction gait properties
'   Set ptrEEPROM to EEPROM block
'   Set left and right ramp values
'
' Parameters (dir)
' Return ()
'/////////////////////////////////////////////////////////////////////////
Set_Direction:
  SELECT dir
    CASE Ahead
      ptrEEPROM = Forward
    CASE ForwardRight
      ptrEEPROM = Forward
    CASE RightSpin
      vel = 7
      GOSUB Equal
      ptrEEPROM = RSpin
    CASE BackRight
      ptrEEPROM = Back
     CASE Backward
      ptrEEPROM = Back
    CASE BackLeft
      ptrEEPROM = Back
    CASE LeftSpin
      vel = 7
      GOSUB Equal
      ptrEEPROM = LSpin
     CASE ForwardLeft
      ptrEEPROM = Forward
  ENDSELECT

'/////////////////////////////////////////////////////////////////////////
' Set delay
' Delay provides time for the servos to reach a position and is based
' on ramp (vel).
' This is an adjustable value.
'
' Parameters (vel)
' Return (delay)
'/////////////////////////////////////////////////////////////////////////
SetDelay:
  IF vel < 9 THEN
    delay = 30
  ELSE
    delay = 10
  ENDIF
'_________________________________________________________________________

'#IF debugMode #THEN
  'GOSUB Trace
'#ENDIF
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Main walking engine
'
' Parameters ()
' Return Nothing
'/////////////////////////////////////////////////////////////////////////
Main:
  'Decrease stride if spinning
  IF (ptrEEPROM = LSpin) OR (ptrEEPROM = RSpin) THEN
    limiter = 100
  ENDIF
  'Read 3 bytes from EEPROM
  READ ptrEEPROM, ServoAddr, servoPosition.LOWBYTE, servoPosition.HIGHBYTE
  DO WHILE servoAddr <> $FF
  'Determine vertical or horizontal servo
    IF (servoAddr // 2) = 1 THEN
      ramp = LiftRamp
    ELSE
      'Set right side ramp
      IF (servoAddr = $00) OR (servoAddr = $02) THEN
        ramp = rightRamp
      ENDIF
      'Set left side ramp
      IF(servoAddr = $04) OR (servoAddr = $06) THEN
        ramp = leftRamp
      ENDIF
      'Limit horizontal swing if limiter has a value greater than zero
      IF (limiter > 0)  THEN
        GOSUB Limit_Stride
      ENDIF
    ENDIF
    GOSUB Write_PSC
    'Increment pointer and get the next 3 EEPROM values
    ptrEEPROM = ptrEEPROM + 3
    READ ptrEEPROM, ServoAddr, servoPosition.LOWBYTE, servoPosition.HIGHBYTE
    #IF debugMode #THEN
      GOSUB Trace
    #ENDIF
  LOOP

RC_Signal_Check:
  GOSUB Get_Stick
  IF (stickXPos & stickYPos) < 500 THEN
    GOTO RC_Signal_Check                    ' Keep checking for a signal
  ENDIF

  GOTO Parse_Stick_Position                 ' Assign direction and speed

GOTO Main 'Loop forever - should never reach this position
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Write position commands to the PSC
'
' Parameters (ServoAddr, servoPosition, delay)
' Return Nothing
'/////////////////////////////////////////////////////////////////////////
Write_PSC:
  SEROUT PSC,Baud,["!SC",ServoAddr,
         Ramp,servoPosition.LOWBYTE,
         servoPosition.HIGHBYTE, CR]

  IF rowCounter = 1 THEN
    PAUSE delay
  ENDIF

RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Read stick postion on pins 12 and 13
'
' Parameters ()
' Return (StickXPin, StickYPin)
'/////////////////////////////////////////////////////////////////////////
Get_Stick:
  PULSIN StickXPin, 1, stickXPos    ' Read joystick positions
  PULSIN StickYPin, 1, stickYPos    ' from transmitter P13, P12
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Reduce leg travel by the value in limiter.  This allows for run-time
' horizontal leg control.
'
' Parameters (ServoAddr, servoPosition)
' Return (servoPosition)
'/////////////////////////////////////////////////////////////////////////
Limit_Stride:
  'Get joint center value and put it in temp
  SELECT ServoAddr
    CASE $00
      temp = Center1
    CASE $02
      temp = Center2
    CASE $04
      temp = Center3
    CASE $06
      temp = Center4
    CASE ELSE
    'Do Noting
  ENDSELECT
    'Reduce swing from center by the value in limiter
    IF servoPosition > temp THEN
      servoPosition = servoPosition - limiter
    ELSE
      servoPosition = servoPosition + limiter
    ENDIF
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Equal
'
'Parameters (vel)
'Return (rightRamp, leftRamp)
'/////////////////////////////////////////////////////////////////////////
Equal:
  rightRamp = vel
  leftRamp = vel
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Slow right side
'
'Parameters (vel)
'Return (rightRamp, leftRamp)
'/////////////////////////////////////////////////////////////////////////
SlowLeft:
  rightRamp = vel
  leftRamp = vel + $7
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Slow left side
'
'Parameters (vel)
'Return (rightRamp, leftRamp)
'/////////////////////////////////////////////////////////////////////////
SlowRight:
  rightRamp = vel + $7
  leftRamp = vel
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Writes variables to the debug terminal when DebugMode = 1
'
' Copy and paste the Debug tool to strategic location in the code.  This
' allow you to see what is in each variable in the Trace sub.
'/////////////////////////////////////////////////////////////////////////
'DEBUG Tool
#IF debugMode #THEN
  GOSUB Trace
#ENDIF

' Write trace information
'   to debug terminal
#IF debugMode #THEN
  Trace:
    DEBUG CLS, ?ramp, ?leftramp, ?rightramp, ?limiter, ?delay, ?ptrEEPROM, ?Neutral,
               ?stickXPos, ?stickYPos
  RETURN
#ENDIF
'_________________________________________________________________________