' www.crustcrawler.com
' Copyright 2006 CrustCrawler Inc.
'
'------- [ HexRodPing.bs2 ] ----------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... HexRodPing.bs2
'   Purpose.... Simple obstacle avoidance with the QuadRod and PING)))
'               ultra sonic sensor.
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   Help....... http://forum.crustcrawler.com/
'   Started.... 05/3/2006
'   Updated....
'   Version.... 2.0
'
'   Hardware
'   (1) HexRod Robotic Kit
'   (1) Parallax BOE
'   (1) Parallax Basic Stamp II
'   (1) Parallax PSC
'   (1) Parallax PING))) Ultra Sonic Sensor
'
'=========================================================================
' Overview
'=========================================================================
' Demo obstacle avoidance using a Parallax PING))) ultra sonic
' distance detector attached to the front of a HexRod.
'
' Distance Detected          Action
' 00 to 10 cm                Backup
' 11 to 15 cm                Spin
' 16 to 30 cm                Gradual turn
' 31 to ...                  Forward
'=========================================================================
' Getting Started
'=========================================================================
' PING Connections:
' PING            BOE
'------          -----
' GND             Vss
' 5V              Vdd
' SIG             Pin 14
'
' Please read all documentation and review demo/base code for the
' HexRod, PSC, BOE/BS2, and PING))).  The code below is a combination
' the HexRodBase.bs2 and the PING demo application.
'
'=========================================================================
' Sub Routines
'=========================================================================
' Please refer to HexRodBase.bs2 and Ping_Demo.bs2 for details.
'
'
' -----[ I/O Definitions ]------------------------------------------------
PSC             PIN     15              ' PSC module

' 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

' -----[ PING))) I/O Definitions ]----------------------------------------
Ping            PIN     14

' -----[ PING))) Constants ]----------------------------------------------
#SELECT $STAMP
  #CASE BS2, BS2E
    Trigger     CON     5                   ' trigger pulse = 10 uS
    Scale       CON     $200                ' raw x 2.00 = uS
  #CASE BS2SX, BS2P, BS2PX
    Trigger     CON     13
    Scale       CON     $0CD                ' raw x 0.80 = uS
  #CASE BS2PE
    Trigger     CON     5
    Scale       CON     $1E1                ' raw x 1.88 = uS
#ENDSELECT

RawToIn         CON     889                 ' 65536 / 73.746 (with **)
RawToCm         CON     2257                ' 65536 / 29.034 (with **)

IsHigh          CON     1                   ' for PULSOUT
IsLow           CON     0

RefreshConv     CON    50                   ' Pause = refresh*RefreshConv

'---- [Walking Variables] ------------------------------------------------
ptrEEPROM       VAR     Word                ' Gait select
servoPosition   VAR     Word                ' Servo Position
temp            VAR     Word                ' Workspace
servoAddr       VAR     Byte                ' Servo addresses
ramp            VAR     Byte                ' Ramp used in SEROUT
vel             VAR     Byte                ' Value from speed screen
rightRamp       VAR     Byte                ' Right side ramp values
leftRamp        VAR     Byte                ' Left side ramp values
limiter         VAR     Byte                ' Reduce stride
delay           VAR     Byte                ' Wait

' -----[ Variables ]-------------------------------------------------------
dist            VAR     Word                ' distance measurement
counter         VAR     Byte                ' Loop counter
char            VAR     Byte                ' Character sent to LCD
'dir             VAR     Nib                 ' Direction

'-----[ Horizontal Leg Constants ]----------------------------------------
' Adjustable leg centers
' Left Side
' Increase value to move leg forward
Center1         CON     750
Center2         CON     750
Center3         CON     750
' Right Side
' Increase value to move leg back
Center4         CON     750
Center5         CON     780
Center6         CON     700

'-----[ 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

Leg5Center      CON     Center5
Leg5Forward     CON     Center5-Stride
Leg5Back        CON     Center5+Stride

Leg6Center      CON     Center6
Leg6Forward     CON     Center6-Stride
Leg6Back        CON     Center6+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

'----- [EEPROM DATA ] ----------------------------------------------------
' Walk Forward
Forward DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft,
             $02,Word Leg2Back,   $06,Word Leg4Back,   $0A,Word Leg6Back,
             $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Forward,
             $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft,
             $03,Word RaiseRight, $07,Word RaiseLeft,  $0B,Word RaiseLeft,
             $00,Word Leg1Back,   $04,Word Leg3Back,   $08,Word Leg5Back,
             $02,Word Leg2Forward,$06,Word Leg4Forward,$0A,Word Leg6Forward,
             $03,Word LowerRight, $07,Word LowerLeft,  $0B,Word LowerLeft,
             $FF

Back    DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft,
             $02,Word Leg2Forward,$06,Word Leg4Forward,$0A,Word Leg6Forward,
             $00,Word Leg1Back,   $04,Word Leg3Back,   $08,Word Leg5Back,
             $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft,
             $03,Word RaiseRight, $07,Word RaiseLeft,  $0B,Word RaiseLeft,
             $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Forward,
             $02,Word Leg2Back,   $06,Word Leg4Back,   $0A,Word Leg6Back,
             $03,Word LowerRight, $07,Word LowerLeft,  $0B,Word LowerLeft,
             $FF

LSpin   DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft,
             $02,Word Leg2Back,   $06,Word Leg4Forward,$0A,Word Leg6Forward,
             $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Back,
             $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft,
             $03,Word RaiseRight, $07,Word RaiseLeft,  $0B,Word RaiseLeft,
             $00,Word Leg1Back,   $04,Word Leg3Back,   $08,Word Leg5Forward,
             $02,Word Leg2Forward,$06,Word Leg4Back,   $0A,Word Leg6BAck,
             $03,Word LowerRight, $07,Word LowerLeft,  $0B,Word LowerLeft,
             $FF
             '(Address 8, 6, and A positions are opposite of forwared gait)

RSpin   DATA $01,Word RaiseRight, $05,Word RaiseRight, $09,Word RaiseLeft,
             $02,Word Leg2Forward,$06,Word Leg4Back,   $0A,Word Leg6Back,
             $00,Word Leg1Back,   $04,Word Leg3Back,   $08,Word Leg5Forward,
             $01,Word LowerRight, $05,Word LowerRight, $09,Word LowerLeft,
             $03,Word RaiseRight, $07,Word RaiseLeft,  $0B,Word RaiseLeft,
             $00,Word Leg1Forward,$04,Word Leg3Forward,$08,Word Leg5Back,
             $02,Word Leg2Back,   $06,Word Leg4Forward,$0A,Word Leg6Forward,
             $03,Word LowerRight, $07,Word LowerLeft,  $0B,Word LowerLeft,
             $FF
             '(Address 2, 0, and 4 positions are opposite of forwared gait)
'Lower and Center all legs
Neutral DATA $00,Word Center1,
             $02,Word Center2,
             $04,Word Center3,
             $06,Word Center4,
             $08,Word Center5,
             $0A,Word Center6,
             $01,Word LowerRight,
             $03,Word LowerRight,
             $05,Word LowerRight,
             $07,Word LowerLeft,
             $09,Word LowerLeft,
             $0B,Word LowerLeft, $FF
'_________________________________________________________________________


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

Initialize:
  limiter = 0                               ' Initialize limiter to 0
  vel = 3                                   ' Set vel to 1
  ramp = 0                                  ' Init ramp
  ptrEEPROM = Neutral                       ' Center and lower all legs
  delay = 30                                ' Delay
  GOTO Main                                 ' Center legs
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Walk backward if an object is within 10cm
' Spin left if an object is withing 10-15cm
' Make a gradual left turn if an object is detected between 15 and 30cm
' Walk forward if no object is detected within 30cm
'
'Parameters (distance reading)
'Return (dir)
'/////////////////////////////////////////////////////////////////////////
Determine_Gait_From_Distance:
  SELECT dist
    CASE < 10
      ptrEEPROM = Back
      GOSUB Equal
    CASE < 15
      ptrEEPROM = LSpin
      GOSUB Equal
    CASE < 30
      ptrEEPROM = Forward
      GOSUB SlowLeft
    CASE ELSE
      ptrEEPROM = Forward
      GOSUB Equal
  ENDSELECT
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Main walking engine
'
' Parameters ()
' Return ()
'/////////////////////////////////////////////////////////////////////////
Main:
   'Read 3 bytes from EEPROM
  READ ptrEEPROM, ServoAddr, servoPosition.LOWBYTE, servoPosition.HIGHBYTE
  DO WHILE servoAddr <> $FF
  'Vertical servo are odd numbers
    IF (servoAddr // 2) = 1 THEN
      ramp = LiftRamp
    ELSE
      'Set right side ramp (servo speed)
      IF (servoAddr = $00) OR (servoAddr = $02) OR (servoAddr = $04) THEN
        ramp = rightRamp
      ENDIF
      'Set left side ramp
      IF(servoAddr = $06) OR (servoAddr = $08) OR (servoAddr = $0A) THEN
        ramp = leftRamp
      ENDIF
      'Limit horizontal swing if limiter variable 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
  LOOP

  IF ptrEEPROM = Neutral + 36 THEN            ' First run centers legs
    limiter = 75                              ' Set limiter
    PAUSE 2000                                ' Pause
  ENDIF

Get_Ping_Reading:
  GOSUB Get_Sonar                           ' Get raw PING reading
  GOSUB C_Centimeter                        ' Convert to centimeter
  #IF debugMode #THEN
    GOSUB Trace
  #ENDIF
  GOTO Determine_Gait_From_Distance

GOTO Main 'Loop forever
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' 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]
  PAUSE delay
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' This subroutine triggers the Ping sonar sensor and measures
' the echo pulse.  The raw value from the sensor is converted to
' microseconds based on the Stamp module in use.  This value is
' divided by two to remove the return trip -- the result value is
' the distance from the sensor to the target in microseconds.
' --John Williams "Servo October 2005"
'/////////////////////////////////////////////////////////////////////////

Get_Sonar:
  Ping = IsLow                              ' make trigger 0-1-0
  PULSOUT Ping, Trigger                     ' activate sensor
  PULSIN  Ping, IsHigh, dist                ' measure echo pulse
  dist = dist */ Scale                      ' convert to uS
  dist = dist / 2                           ' remove return trip
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' Convert distance reading to centimeter
'
' Parameters (dist)
' Return (dist)
'/////////////////////////////////////////////////////////////////////////

C_Centimeter:
  dist = dist ** RawToCm                    ' convert to centimeters
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' Convert distance reading to inches
'
'Parameters (dist)
'Return (dist)
'/////////////////////////////////////////////////////////////////////////
C_Inches:
  dist = dist ** RawToIn                    ' convert to inches
RETURN
'_________________________________________________________________________


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

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

'/////////////////////////////////////////////////////////////////////////
'Parameters (vel)
'Return (rightRamp, leftRamp)
'
' Slow left side
'/////////////////////////////////////////////////////////////////////////
SlowRight:
  rightRamp = vel + $5
  leftRamp = vel
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 $08
      temp = Center5
    CASE $0A
      temp = Center6
    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
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' 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, ?vel, ?limiter, ?ptrEEPROM, ?dist
  RETURN
#ENDIF