' ==============================================================================
'   {$PORT COM4}
'
'   File...... NomadGaitPICwithRC.bsp
'   Purpose... Demostrate Integration of RC and GaitPIC commands to control Nomad
'   Author.... Dan Lynch
'   Date...... 12/01/2006
'   Utilized the following programs from Crust Crawlers:
'              GaitPIC-Template.bsp (Author: Steven R.Norris)
'              Nomad_RC_Test.bsp (Author: Mike Gebhard)
'
'
'   {$STAMP BS2p}
'   {$PBASIC 2.5}
'
' ==============================================================================

' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------
' Secrets to Success:
'   1) Download Nomad_Walking_Engine_gaitCode.bsp to confirm you
'       can change the code in the Nomad.
'   2) Confirm that Transmitter and receiver (with 4.8V battery pack) are working independent of installation on Nomad
'       a) attach servo into throttle and rudder ports on receiver to identify and confirm
'          proper channels.
'   3) Connect proper channel from receiver into Nomad's proper ports
'   4) Download Nomad_RC_Test.bsp program and run it with PC tethered.
'   5) Turn on Transmitter and confirm left transmitter stick readings with grid below.
'      Example 1: Left transmitter stick at full up   should display a "0" in the debug window on your PC
'      Example 2: Left transmitter stick at full down should display a "6" in the debug window on your PC
'         a) if your values are inversed then you need to "reverse" the channel setting on the trasmitter.
'   6) Download this program and your Nomad will respond to your transmitter as described below.
'   7) This is to get you started, you can easily change the speed or display commands on the Nomad's LCD
'      by just using the documentations OR program examples provided by CrustCrawler.
'
' Transmitter stick position grid - move the stick and it should give you the appropriate value.
' |-----|-----|-----|-----|-----|
' |  B  |  0  |  0  |  0  |  1  |
' |-----|-----|-----|-----|-----|
' |  A  |  F  |  F  |  F  |  2  |
' |-----|-----|-----|-----|-----|
' |  9  |  F  |  F  |  F  |  3  |
' |-----|-----|-----|-----|-----|
' |  8  |  F  |  F  |  F  |  4  |
' |-----|-----|-----|-----|-----|
' |  7  |  6  |  6  |  6  |  5  |
' |-----|-----|-----|-----|-----|
'
'
'Transmitter stick position and Nomad reaction:
'   F   = stop
'   0   = Forward
'   6   = Backward
'   9   = Counter Clockwise
'   3   = Clockwise
'   A,B = Left Diagonal Crawl Forward
'   1,2 = Right Diagonal Crawl Forward
'   7,6 = Left Diagonal Crawl Backward
'   4,5 = Right Diagonal Crawl Backward
'
' ------------------------------------------------------------------------------
' I/O Definitions
' ------------------------------------------------------------------------------

Piezo           PIN     0               ' Piezo Speaker

' GaitPIC interface
Pin_GpBusy      PIN     8               ' Busy
Pin_GpStop      PIN     9               ' All stop
Pin_GP          PIN     10              ' Serial commands

' ------------------------------------------------------------------------------
' Constants
' ------------------------------------------------------------------------------

' GaitPIC baud rate
Gpbaud          CON     45               ' 38400 baud

' Servo positions
Center          CON     750
Raise           CON     600
Lower           CON     300

LiftRamp        CON     7
HorzRamp        CON     8
FootRamp        CON     8

' ------------------------------------------------------------------------------
' Variables
' ------------------------------------------------------------------------------

Gait            VAR     Byte
Direction       VAR     Byte
Speed           VAR     Byte
GaitCount       VAR     Byte

Servo           VAR     Byte
ServoAddr       VAR     Byte
Position        VAR     Word
Ramp            VAR     Byte


StickXPin       PIN     13          ' Left/Right(X) joystick Rx Ch4
StickYPin       PIN     12          ' Up/Down(Y) joystick Rx Ch3

'---- [ Variables ] ------------------------------------------------------
stickXPos       VAR     Word        ' Left/Right (X) joystick posn
stickYPos       VAR     Word        ' Up/Down(Y) joystick position
gaitCode        VAR     Byte        ' Mode selected
displayCode     VAR     Bit

' ------------------------------------------------------------------------------
' EEPROM Data
' ------------------------------------------------------------------------------

' ------------------------------------------------------------------------------
' Initialization
' ------------------------------------------------------------------------------

  ' Define initial state of all pins
  OUTS=%0000000000000000
  '     FEDCBA9876543210
  DIRS=%0000011000000001    '1=Output

  HIGH Pin_GpStop
  HIGH Pin_GP
  PAUSE 2000

  ' Flush the GaitPIC's receive buffer
  GOSUB FlushIt

  ' Home
  LOW Pin_GpStop
  GOSUB HomeIt

  DEBUG CLREOL,  "Nomad RC Interface Program"
  Gait = 0
  Direction =0
  Speed = 0
  GaitCount = 254   'Continous number of steps

' ------------------------------------------------------------------------------
' Program Code
' ------------------------------------------------------------------------------
Main:
  'Default values for stick direction. High Nib is previous, Low Nib is current
  gaitCode.HIGHNIB=$C
  gaitCode.LOWNIB=$F

Get_Stick:
  PULSIN StickXPin, 1, stickXPos    ' Read joystick positions
  PULSIN StickYPin, 1, stickYPos    ' from transmitter P13, P12

Get_gaitCode_LowNib:
  SELECT stickYpos                  ' Convert to gaitCode
    CASE < 1700
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $1
        CASE < 1800
          gaitCode.LOWNIB = $B
        CASE ELSE
             gaitCode.LOWNIB = $0
      ENDSELECT
    CASE < 1950
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $2
        CASE < 1800
          gaitCode.LOWNIB = $A
        CASE ELSE
         gaitCode.LOWNIB = $F
       ENDSELECT
    CASE < 2200
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $3
        CASE < 1800
          gaitCode.LOWNIB = $9
        CASE ELSE
         gaitCode.LOWNIB = $F
       ENDSELECT
    CASE < 2450
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $4
        CASE < 1800
          gaitCode.LOWNIB = $8
        CASE ELSE
         gaitCode.LOWNIB = $F
       ENDSELECT
    CASE < 2700
      SELECT stickXPos
        CASE > 2450
          gaitCode.LOWNIB = $5
        CASE < 1950
          gaitCode.LOWNIB = $7
        CASE ELSE
          gaitCode.LOWNIB = $6
       ENDSELECT
    CASE ELSE
      gaitCode.LOWNIB = $F
  ENDSELECT


  'Translate Transmitter stick position to an action on the Nomad
  DEBUG CRSRXY, 1, 2, CLREOL, "New gaitCode = " , HEX gaitcode.LOWNIB , "  Previous gaitCode = " , HEX gaitcode.HIGHNIB, CR
  IF NOT (gaitCode.HIGHNIB = gaitCode.LOWNIB) THEN    'Did stick change from last action
    gaitCode.HIGHNIB = gaitcode.LOWNIB      'yes it did, set high nib with new stick position
    GOSUB StopIt
    GOSUB FlushIt
    Direction = 0
    gait = 0
    speed = 0
    IF gaitCode.LOWNIB = $F THEN
      DEBUG CLREOL, "Stop it"
    ELSE
      SELECT  gaitCode.LOWNIB
        CASE $0
          DEBUG CLREOL, "Walk Forward - Straight"
        CASE $B,$A
          DEBUG CLREOL, "Walk Forward - Left Diagonal - 300 degrees"
          Direction = 5
        CASE $1, $2
          DEBUG CLREOL, "Walk Forward - Right Diagonal - 60 degrees"
          Direction = 1
        CASE $7, $8
          DEBUG CLREOL, "Walk Backwards - Left Diagonal - 240 degrees"
          Direction = 4
        CASE $5, $4
          DEBUG CLREOL, "Walk Backwards - Right Diagonal - 120 degrees"
          Direction = 2
        CASE $3   ',$2,$4
          DEBUG CLREOL, "Rotate Clockwise"
          gait = 1
        CASE $9    ',$8,$A
          DEBUG CLREOL, "Rotate Counter Clockwise"
          gait = 2
        CASE $6
          DEBUG CLREOL, "Walk Backwards - Straight"
          Direction = 3
        CASE ELSE
          DEBUG CLREOL, "Ignore Command  ", HEX gaitcode.LOWNIB
      ENDSELECT

      GOSUB WalkIt
    ENDIF '-- Else not stop
  ENDIF
GOTO Get_Stick

STOP
END

' ------------------------------------------------------------------------------
' Subroutines
' ------------------------------------------------------------------------------

FlushIt:
  SEROUT Pin_GP, GpBaud, [0,0,0,0,0,0,0,0,$FF]
  RETURN

WalkIt:
  SEROUT Pin_GP, GpBaud, ["!w",Gait,Direction,Speed,GaitCount,$FF]
  DO WHILE Pin_GpBusy = 1
  LOOP
  RETURN

StopIt:
  SEROUT Pin_GP, GpBaud, ["!s",$FF]
  DO WHILE Pin_GpBusy = 1
  LOOP
  RETURN

HomeIt:
  SEROUT Pin_GP, GpBaud, ["!h",$FF]
  DO WHILE Pin_GpBusy = 1
  LOOP
  RETURN