'------- [QuadWalking_Engine_Base_Program ] ------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... QuadWalking_Engine_Base_Program.BS2
'   Purpose.... Investigate QuadWalking_Engine program parameters.
'               Create and test custom walking gaits.
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 20 August 2004
'   Updated.... 27 December 2004
'   Version.... 1.2
'
'   Hardware
'   (1) QuadCrawler Robotic Kit
'   (1) Parallax BOE
'   (1) Parallax Basic Stamp II
'   (1) Parallax PSC
'
' QuadWalking_Engine_Base_program.BS2 allows you to FULLY
' control leg joint parameters like speed, rotation, and sequence
' programmatically .  This control allows you to create and
' test custom walking gaits.
'
'=========================================================================
' Getting Started
'=========================================================================
' 1. Change the default leg centers in this program with the legs
'    centers found in TuneQuad.BS2.
'    http://www.crustcrawler.com/downloads/code.php?id=20
'
' 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 TuneQuad.bs2 program.
'
' Example QuadCrawler center constants (yours will vary)
'-----[ Horizontal Leg Constants ]----------------------------------------
'Center1         CON     725
'Center2         CON     800
'Center3         CON     785
'Center4         CON     750
'
'=========================================================================
' Program Operation Overview
'=========================================================================
' This program is broken into 3 sections
' Variables and Constants
' EEPROM Data
' Walking engine
'
' Variables and Constants:
' All constants and variable that control joint position and speed are
' adjustable.
' Stride is the distance +/- a horizontal servo moves from center.
' Therefore, a stride of 150 is 150 + center and 150 - center for a
' total of 300 units or about 60 degrees.
' Delay gives the servo time to reach a position.  Delay is expressed
' as a ratio of stride (stride/5).
' leftRamp and rightRamp control left side and right side servo speeds.
' LiftRamp controls vertical servo speed.
' ptrEEPROM pointer to EEPROM.
'
' EEPROM Data
' EEPROM data is written in four 49 byte blocks; Forward, Back,
' LTurn, and RTurn.  Each block of EEPROM data is broken into 3
' byte sections; servo address, LOWBYTE of servo position, and
' HIGHBYTE of servo position.  EEPORM data is READ 3 bytes at a time
' until an $FF (end of data 49th byte) is found.
'
' Walking Engine:
' This is the work horse of all QuadCrawler programs. This sub routine
' determines if the servo address (read from EEPROM) is a vertical servo,
' left horizontal servo, or right horizontal servo.  Speed (Ramp) is
' assigned depending on servo address. Servo address, Servo postion,
' and ramp are passed to the Write_PSC sub routine. Write_PSC writes
' this data to the PSC.
'
'=========================================================================
' Sub Routines
'=========================================================================
' ptrEEPROM is a pointer to EEPROM addresses. Assigning
' ptrEEPROM to "Forward" (ptrEEPROM = Forawrd) places
' the pointer at the starting EEPROM address for
' forward motion.
'
' rightRamp and leftRamp control servo speeds (ramp) on
' the left or right side of the QuadCrawler.
' Slowing the servos on one side of the robot causes
' the QuadCralwer to make gradual turns.
'
' Walking_Engine:
' The "Walking_Engine" sub routine initializes ptrEEPROM to point to a
' block of EEPROM data.  3 byte values are read and stored
' in servoAddr, HIGHBYTE.servoPosition, and LOWBYTE.servoPosition
' variables. Walking_Engine first evaluates the value in servoAddr.
'
' Walking_Engine loops through the EEPROM data until an $FF is found.
' An $FF indicates the end of an EEPROM data block. ptrEEPROM is reset
' and the process repeats.
'
' If servoAddr is an odd number a vertical servo is addressed. Vertical
' servo are assigned a very fast ramp value to lift and lower legs
' quickly.
'
' If servoAddr is even Walking_Engine assigns ramp values (speed)
' depending on leg location, left or right side of the robot. Slowing
' leg speed on one side of the robot cause the robot to make gradual turn
' toward the slow side.
'
' Walking_Engine then passes, ptrEEPROM, servoAddr, and servoPosition
' variables to the Write_PSC sub routine.
'
' Write_PSC:
' The "Write_PSC" sub routine writes servoAddr, ramp, and servoPosition
' to the PSC.  ptrEEPROM is incremented to point to the next 3 bytes of
' EEPROM data. The three bytes of EEPROM data are read into servoAddr,
' HIGHBYTE.servoPosition, and LOWBYTE.servoPosition.  Control is returned
' to Walking_Engine.
'
'=========================================================================
' Walk Forward Diagram
'=========================================================================
'
'      **Right Side**        |
'                            |
' F    \          /          |        /      \
' O    _\_      _/_          |      _/_      _\_
' R   / 1 \____/ 2 |         |     / 1 \____/ 2 |
' W  |             |         |    |             |
' A  |     ____    |         |    |     ____    |
' R   \_3_/    \_4_|         |     \_3_/    \_4_|
' D     \        /           |       /        \
'        \      /            |      /          \
'
' If you are really adventurous you can change
' or add EEPROM data and create virtually any walking
' sequence you can imagine.
'-------------------------------------------------------------------------
' -----[ 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

'---- [Walking Variables] ------------------------------------------------
servoAddr       VAR     Byte                  ' Servo addresses
ptrEEPROM       VAR     Word                  ' Gait select
servoPosition   VAR     Word                  ' Servo Position
ramp            VAR     Byte                  ' Ramp used in SEROUT
rightRamp       VAR     Byte                  ' Right side ramp values
leftRamp        VAR     Byte                  ' Left side ramp values

'----- [Adjustable Ramp Values] ------------------------------------------
LiftRamp        CON     $1                    ' Vertical servo ramp

'-----[ Horizontal Leg Constants ]----------------------------------------
'Default Servo center
Center1         CON     750                   ' Leg1 horizontal servo
Center2         CON     750                   ' Leg2 horizontal servo
Center3         CON     750                   ' Leg3 horizontal servo
Center4         CON     750                   ' Leg4 horizontal servo

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

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

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

'---- [Byte Servo Address and Word 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
'---- [End EEPROM Data] --------------------------------------------------

'-----[ Main Walking Routine ]--------------------------------------------
' To change gaits comment the line
' ptrEEPROM = Forward and remove
' the comment from another line
Walking_Engine:
  ptrEEPROM = Forward
  'ptrEEPROM = Back
  'ptrEEPROM = LTurn
  'ptrEEPROM = RTurn
  rightRamp = $8
  leftRamp = $8
  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
GOTO Walking_Engine

'----- [Write to the 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

'=====[ Updates ]=========================================================
' Updates:
' 1. Updated EEPROM table for more aggressive
'    Stride
' 2. Programmatic horizontal leg adjustment
'=========================================================================