' www.crustcrawler.com
' Copyright 2006 CrustCrawler Inc.
'
'------- [ HexCrawlerPingLCDRc.bs2 ] ------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... HexCrawlerPingLCDRc.bs2
'   Purpose....
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   Help....... http://forum.crustcrawler.com/
'   Started.... 05/03/2006
'   Updated....
'   Version.... 2.0
'
'   Hardware
'   (1) CrustCrawler HexCrawler Robotic Kit
'   (1) CrustCrawler S3 tilt pan
'   (1) Parallax BOE
'   (1) Parallax Basic Stamp II
'   (1) Parallax PSC
'   (1) Parallax PING))) Ultra Sonic Sensor
'   (1) Parallax LCD AppMod
'
'=========================================================================
' Overview
'=========================================================================
' Run the HexCrawler in autonomous or RC mode.  Autonomous mode uses an
' S3/PING))) combination to avoid obstacles.  RC mode overrides
' autonomous mode.
'
' Please read all documentation and review demo/base code for the
' HexCrawler, PSC, BOE/BS2, and PING))).  The code below is a combination
' the HexCrawlerLCD.bs2 and HexCrawlerPing.bs2.  You can find detailed code
' explaination in the files mentioned
'
'=========================================================================
' Getting Started
'=========================================================================
' On first load, the CrustCrawler logo is displayed on the LCD AppMod.
' After 1/2 second the limiter menu is displayed. If the LCD is blank,
' adjust the contrast POT.
'
' Use the  LCD AppMod buttons to interface with the robot's gait
' parameters.
'
' BUTTONS
' A = Mode - Not used
' B = Increment
' C = Decrement
' D = Enter
'
' Button B increments the limiter value displayed.  Button C decrements
' the value.  Press D to exit setup.
'
' The LCD will display distance reading from the PING)))
'
' Distance Detected          Action
' -----------------          -------
' 00 to 10 cm                Backup
' 11 to 15 cm                Spin
' 16 to 30 cm                Gradual turn
' 31 to ...                  Forward
'
'=========================================================================
' Connections
'=========================================================================
'  S3      to      PSC
' -----           -----
' S3Y Servo        ch8
' S3X Servo        ch9
'
' PING     to      BOE
'------           -----
' GND              Vss
' 5V               Vdd
' SIG              Pin 14
'
' You will need 4 wires to connect the receiver
' to the BOE's X5 servo port (see your BOE Rev B 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
'
'=========================================================================
' Program Operation Overview
'=========================================================================
' Please read all documentation and review demo/base code for the
' HexCrawler, PSC, BOE/BS2, and PING))).  The code below is a combination
' the HexCrawlerLCD.bs2 and HexCrawlerPing.bs2.
'
'=========================================================================
' Sub Routines
'=========================================================================
' Please refer to HexCrawlerBase.bs2 and Ping_Demo.bs2 for details.
'
'-------------------------------------------------------------------------

' -----[ I/O Definitions ]------------------------------------------------
PSC             PIN     15              ' PSC module
Ping            PIN     14              ' PING)))
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

' -----[ LCD I/O Definitions ]--------------------------------------------
E               PIN     1                   ' LCD Enable (1 = enabled)
RW              PIN     2                   ' Read/Write
RS              PIN     3                   ' Reg Select (1 = char)
LcdDirs         VAR     DIRB                ' dirs for I/O redirection
LcdBusOut       VAR     OUTB
LcdBusIn        VAR     INB

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

' -----[ LCD Constants ]--------------------------------------------------
#DEFINE _lcdReady = ($stamp = BS2SX) OR ($stamp = BS2P)

LcdCls          CON     $01                 ' clear the LCD
LcdHome         CON     $02                 ' move cursor home
LcdCrsrL        CON     $10                 ' move cursor left
LcdCrsrR        CON     $14                 ' move cursor right
LcdDispL        CON     $18                 ' shift chars left
LcdDispR        CON     $1C                 ' shift chars right

LcdDDRam        CON     $80                 ' Display Data RAM control
LcdCGRam        CON     $40                 ' Character Generator RAM
LcdLine1        CON     $80                 ' DDRAM address of line 1
LcdLine2        CON     $C0                 ' DDRAM address of line 2

LcdScrollTm     CON     250                 ' LCD scroll timing (ms)

'---- [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
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
value           VAR     Byte                ' Increment Decr. value

dir             VAR     Nib                 ' Direction
menu            VAR     Nib                 ' Current menu
steps           VAR     Nib                 ' Numbers of steps

buttons         VAR     Nib
btnA            VAR     buttons.BIT0        ' left-most button
btnB            VAR     buttons.BIT1
btnC            VAR     buttons.BIT2
btnD            VAR     buttons.BIT3        ' right-most

RcMode          VAR     Bit

'-----[ 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     750
Center6         CON     750


'-----[ 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     $0                    ' Vertical servo ramp
EOD             CON     $FF                   ' End of data block

'----- [Adjustable vertical servo positions] --------------------------------
RaiseRight      CON     300                   ' Raised and lowered **
LowerRight      CON     1200                  ' vertical servo values **
RaiseLeft       CON     1200
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

'---[S3 constants]--------------------------------------------------------
S3Y             CON     12
S3X             CON     13
S3Ramp          CON     $7
LookLeft90      CON     400
LookRight90     CON     1100
LookLeft45      CON     600
LookRight45     CON     900

TooClose        CON     10
Far             CON     50
NumOfSteps      CON     2

Yes             CON     1
No              CON     0

'----- [EEPROM DATA ] ----------------------------------------------------
'LCD EEPROM Data
'Smiley     DATA $00, $0A, $0A, $00, $11, $0E, $06, $00  ' 0 smiley
DownArrow  DATA $04, $04, $04, $04, $1F, $0E, $04, $00  ' 1 Down Arrow
UpArrow    DATA $04, $0E, $1F, $04, $04, $04, $04, $00  ' 2 Up Arrow
C_Gear     DATA $06, $0F, $09, $18, $18, $09, $0F, $06  ' 3 char1 C Gear
Enter      DATA $01, $01, $01, $05, $0D, $1F, $0C, $04  ' 4 Enter Arrow
EndCG      DATA EOD

Msg1       DATA  "Crust   ",EOD
Msg2       DATA  " ",2,"rawler",EOD
Msg3       DATA  "PING))) ",EOD
Msg4       DATA  "RC      ",EOD
Direction  DATA  "Dir: ",EOD
Speed      DATA  "Spd: ",EOD
Limit      DATA  "Lim: ",EOD
Distance   DATA  "Dist:",EOD
GUI        DATA  "m ", 1, "  ",0," ", 3, EOD
'_________________________________________________________________________


' 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,
             EOD

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,
             EOD

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,
             EOD
             '(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,
             EOD
             '(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, EOD
'_________________________________________________________________________


Pre_Init:
' 0 = normal mode
' 1 = debug mode
#DEFINE debugMode = 0
GOSUB Initialize_LCD                      ' Initialize LCD
PAUSE 500                                 ' Wait 1/2 sec

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

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

  GOSUB Equal

' Parse X and assign turn
  SELECT stickXPos
    CASE 500 TO 700
      IF vel = 64 THEN
        ptrEEPROM = LSpin
      ELSE
        GOSUB SlowLeft
      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
        ptrEEPROM = RSpin
      ELSE
        GOSUB SlowRight
      ENDIF
  ENDSELECT
  GOTO SetDelay

'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
'Clear Screen
' Write UI
' Loop until button D is pressed
' Increment/Decrement value
' Convert value to ASCII by adding 48
'
'Parameters ()
'Return ()
'/////////////////////////////////////////////////////////////////////////

Menus:
  vel = 3
  char = LcdCls                             ' Clear command
  GOSUB LCD_Command                         ' Write clear screen command
  GOSUB Write_GUI                           ' Write Graphic Interface
Get_Menu:
  char = LcdHome                            ' Cursor to home position
  GOSUB LCD_Command
Limiter_Menu:
  ptrEEPROM = Limit                         ' Point to Speed string
  GOSUB LCD_Put_String                      ' Write String
  PAUSE 100                                 ' Wait
  value = Limiter                           ' Input param ramp

Incr_Decr:
  DO WHILE btnD = 0                         ' Do while D not pressed
    GOSUB LCD_Get_Buttons                   ' Check button pressed
    IF btnB THEN                            ' B pressed?
      value = (value + 1) // 256
    ENDIF
    IF btnC THEN                            ' C Pressed
      value = (value + 255) // 256
    ENDIF
    GOSUB Set_Value                         ' Assign ramp or direction
    GOSUB Write_Num
    PAUSE 75                                ' Write number to LCD
  LOOP

  GOSUB Write_Dist
  GOSUB Write_Ping
'________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' 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
      GOSUB Equal
    CASE ForwardRight
      ptrEEPROM = Forward
      GOSUB SlowRight
    CASE RightSpin
      ptrEEPROM = RSpin
      GOSUB Equal
    CASE BackRight
      ptrEEPROM = Back
      GOSUB SlowRight
     CASE Backward
      ptrEEPROM = Back
      GOSUB Equal
    CASE BackLeft
      ptrEEPROM = Back
      GOSUB SlowLeft
    CASE LeftSpin
      ptrEEPROM = LSpin
      GOSUB Equal
     CASE ForwardLeft
      ptrEEPROM = Forward
      GOSUB SlowLeft
  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
  'PAUSE 2000
'#ENDIF

'/////////////////////////////////////////////////////////////////////////
' 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
    limiter = 75                            ' Set limiter
    GOTO Menus                              ' first load
  ENDIF

  GOSUB LCD_Get_Buttons                     ' Check for button press
  IF btnA THEN                              ' Button A pressed?
    GOTO Menus                              ' Display menus
  ENDIF

RC_Signal_Check:
  GOSUB Get_Stick
  IF (stickXPos & stickYPos) < 500 THEN
    RcMode = No
    GOSUB Write_Ping
  ELSE
    RcMode = Yes
      GOSUB Write_RC
  ENDIF

Display_Distance:
  GOSUB Get_Sonar                           ' Get raw PING reading
  GOSUB C_Centimeter                        ' Convert to centimeter
  value = dist                              ' Display distance
  GOSUB Write_Num

Control_Source:
  IF RCMode = Yes THEN                      ' RC mode?
    GOTO Parse_Stick_Position               ' Read stick
  ELSE                                      ' Autonomous mode
    GOSUB Determine_Direction               ' Find path
  ENDIF

  #IF debugMode #THEN
    GOSUB Trace
  #ENDIF
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
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' 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
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' 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
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Position LCD cursor
' Conver the number in value to ASCII and write to the LCD AppMod
'
' Parameters ()
' Return ()
'/////////////////////////////////////////////////////////////////////////
Write_Num:
  char = LcdLine1 + 5                       ' Goto line 1 position 4
  GOSUB LCD_Command
C_ASCII:                                    ' convert to ASCII value
  char = (value / 100)                      ' Get 100s position
  value = (value //100)                     ' Get Mod
  char = char + 48                          ' Convert to ASCII
  GOSUB Write_LCD_Char                      ' Write value
  char = (value / 10)                       ' Get 10s position
  value = (value // 10)                     ' Get Mod
  char = char + 48                          ' Convert to ASCII
  GOSUB Write_LCD_Char                      ' Write value
  char = value + 48                         ' Convert to ASCII
  GOSUB Write_LCD_Char                      ' Write value
  GOSUB Get_Value
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Write interface screen to LCD AppMod
'
' Parameters ()
' Return ()
'/////////////////////////////////////////////////////////////////////////
Write_GUI:                                  ' Write GUI items on line 2
  char = LcdLine2                           ' Goto Line 2
  GOSUB LCD_Command                         ' LCD Command
  ptrEEPROM = GUI                           ' Write CG chars (menu items)
  GOSUB LCD_Put_String                      ' Write characters
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' Write PING))) screen to LCD AppMod
'
' Parameters ()
' Return ()
'/////////////////////////////////////////////////////////////////////////
Write_Dist:
  char = LcdCls                             ' Clear command
  GOSUB LCD_Command                         ' Write clear screen command
  ptrEEPROM = Distance                      ' Point to Direction string
  GOSUB LCD_Put_String                      ' Write String
RETURN

Write_Ping:
  char = LcdLine2                           ' Line 2 command
  GOSUB LCD_Command                         ' Write command
  ptrEEPROM = Msg3                          ' Point to PING)))
  GOSUB LCD_Put_String                      ' Write PING)))
RETURN

Write_RC:
  char = LcdLine2                           ' Line 2 command
  GOSUB LCD_Command                         ' Write command
  ptrEEPROM = Msg4                          ' Point to PING)))
  GOSUB LCD_Put_String                      ' Write PING)))
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' Sets velocity, direction, and liniter values
'
' Parameters (ramp)
' Return (rightRamp, leftRamp)
'/////////////////////////////////////////////////////////////////////////
Set_Value:
  limiter = value                       ' Assign limiter
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' Gets velocity, direction, and liniter values
'
'Parameters (menu)
'Return (vel, dir, limiter)
'/////////////////////////////////////////////////////////////////////////
Get_Value:
  value = limiter                       ' Assign limiter
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 + $5
RETURN
'_________________________________________________________________________

'/////////////////////////////////////////////////////////////////////////
' Slow left side
'
' Parameters (vel)
' Return (rightRamp, leftRamp)
'/////////////////////////////////////////////////////////////////////////
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 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
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Scan buttons
'
' Parameters ()
' Return (buttons)
'/////////////////////////////////////////////////////////////////////////
LCD_Get_Buttons:
  LcdDirs = %0000                           ' make LCD bus inputs
  buttons = %1111                           ' assume all pressed
  FOR counter = 1 TO 10
     buttons = buttons & LcdBusIn           ' make sure button held
    PAUSE 5                                 ' debounce 10 x 5 ms
  NEXT
  LcdDirs = %1111                           ' return bus to outputs
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Read EEPROM data and write it to the LCD
'
' Parameters (ptrEEPROM)
' Return ()
'/////////////////////////////////////////////////////////////////////////
LCD_Put_String:
  DO
    READ ptrEEPROM, char                    ' Read data at EEPROM address
    IF (char = EOD) THEN EXIT               ' 0 = End of EEPROM String
    GOSUB Write_LCD_Char                    ' Write character to LCD
    ptrEEPROM = ptrEEPROM + 1               ' Increment EEPROM pointer
  LOOP
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Write LCD command/Write Character
'
' Parameters (char)
' Return ()
'/////////////////////////////////////////////////////////////////////////
LCD_Command:                                ' write command to LCD
  #IF _LcdReady #THEN
    LCDCMD E, char
    RETURN
  #ELSE
    LOW RS
    GOTO Write_LCD_Char
  #ENDIF

Write_LCD_Char:                             ' write character to LCD
  #IF _LcdReady #THEN
    LCDOUT E, 0, [char]
  #ELSE
    LcdBusOut = char.HIGHNIB                ' output high nibble
    PULSOUT E, 3                            ' strobe the Enable line
    LcdBusOut = char.LOWNIB                 ' output low nibble
    PULSOUT E, 3
    HIGH RS                                 ' return to character mode
  #ENDIF
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Initialize LCD
' Write custom cahracters to the LCD
' Write CrustCrawler Banner
'
' Parameters ()
' Return ()
'/////////////////////////////////////////////////////////////////////////
Initialize_LCD:
  #IF _LcdReady #THEN
    LCDCMD E, %00110000 : PAUSE 5           ' 8-bit mode
    LCDCMD E, %00110000 : PAUSE 0
    LCDCMD E, %00110000 : PAUSE 0
    LCDCMD E, %00100000 : PAUSE 0           ' 4-bit mode
    LCDCMD E, %00101000 : PAUSE 0           ' 2-line mode
    LCDCMD E, %00001100 : PAUSE 0           ' no crsr, no blink
    LCDCMD E, %00000110                     ' inc crsr, no disp shift
  #ELSE
    NAP 5                                   ' let LCD self-initialize
    DIRL = %11111110                        ' setup pins for LCD
    LcdBusOut = %0011                       ' 8-bit mode
    PULSOUT E, 3 : PAUSE 5
    PULSOUT E, 3 : PAUSE 0
    PULSOUT E, 3 : PAUSE 0
    LcdBusOut = %0010                       ' 4-bit mode
    PULSOUT E, 3
    char = %00101000                        ' 2-line mode
    GOSUB LCD_Command
    char = %00001100                        ' on, no crsr, no blink
    GOSUB LCD_Command
    char = %00000110                        ' inc crsr, no disp shift
  #ENDIF
ClearScreen:
  char = LcdCls                             ' Clear screen
  GOSUB LCD_Command
Download_Chars:                             ' download custom chars
  char = LcdCGRam                           ' point to CG RAM
  GOSUB LCD_Command                         ' prepare to write CG data
  FOR counter = DownArrow TO EndCG-1           ' build custom chars
    READ counter, char                      ' get byte from EEPROM
    GOSUB Write_LCD_Char                    ' put into LCD CG RAM
  NEXT

CrustCrawler_Logo:                          ' Dispaly CC logo
  char = LcdLine1                           ' Line 1
  GOSUB LCD_Command
  ptrEEPROM = Msg1                          ' Message 1 pointer
  GOSUB LCD_Put_String                      ' Write string
  char = LcdLine2                           ' Go to Line 2
  GOSUB LCD_Command                         ' Write command
  ptrEEPROM = Msg2                          ' Point to message 2
  GOSUB LCD_Put_String                      ' Write string
RETURN

'/////////////////////////////////////////////////////////////////////////
' Determine which direction is most clear.
'
' Distance                  Action
'----------                --------
' < 15 cm                   Backup 3 steps
' 16 to 29                  Look far left/Ping look far right/Ping
' 30 to 59                  Look left/Ping look right/Ping
'
' When looking left and right, the PING is triggered and the distances
' are compared. The robot will turn in the direction of the largest
' distance reading.
'
' Parameters (dist)
' Return (dist)
'/////////////////////////////////////////////////////////////////////////

Determine_Direction:
  SELECT dist
    CASE < 15
      ptrEEPROM = Back
      steps = NumOfSteps
    CASE < 30
      servoPosition = LookLeft90
      GOSUB Look
      temp = dist
      servoPosition = lookRight90
      GOSUB Look
      IF temp > dist THEN
        ptrEEPROM = LSpin
        'steps = NumOfSteps
      ELSE
        ptrEEPROM = RSpin
        'steps = NumOfSteps
      ENDIF
    CASE < 60
      servoPosition = LookLeft45
      GOSUB Look
      temp = dist
      servoPosition = lookRight45
      GOSUB Look
      IF temp > dist THEN
        ptrEEPROM = Forward
        'steps = NumOfSteps
        GOSUB SlowLeft
      ELSE
        ptrEEPROM = Forward
        'steps = NumOfSteps
        GOSUB SlowRight
      ENDIF
    CASE ELSE
      ptrEEPROM = Forward
      GOSUB Equal
    ENDSELECT

    GOSUB Center_S3
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Turn the S3 left or right
'
' Parameters (servoPosition)
' Return ()
'/////////////////////////////////////////////////////////////////////////
Look:
  SEROUT PSC,Baud,["!SC", S3X, S3Ramp, servoPosition.LOWBYTE, servoPosition.HIGHBYTE, CR]
  PAUSE 1000
  GOSUB Get_Sonar
RETURN
'_________________________________________________________________________


'/////////////////////////////////////////////////////////////////////////
' Center the S3 but look slightly downward
'
' Parameters ()
' Return ()
'/////////////////////////////////////////////////////////////////////////
Center_S3:
  servoPosition = 650
  SEROUT PSC,Baud,["!SC",S3Y, S3Ramp,servoPosition.LOWBYTE, servoPosition.HIGHBYTE, CR]
  servoPosition = 750
  SEROUT PSC,Baud,["!SC",S3X, S3Ramp,servoPosition.LOWBYTE, servoPosition.HIGHBYTE, CR]
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, "Buttons: ", BIN buttons, CR,
          ?leftramp,
          ?rightramp,
          ?dir,
          ?vel,
          ?limiter,
          ?dist
  RETURN
#ENDIF
'_________________________________________________________________________