'----[Nomad_Walking_Demo - RobotServos.bsp]-----------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... Nomad_Walking_Demo.bsp
'   Purpose.... Nomad RC and/or LCD AppMod control.
'   Author..... CustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 10 February 2005
'   Updated....
'
'   Modifications for 18 Robot Servos
'   Author: John W. Raffensperger, Jr. (Beaver Dam, Wisconsin)
'   Released: 23 December 2006
'   Description of Changes:
'       Adjusted Raise and Lower Parameters
'       Re-wrote D2 Data Section
'
' Hardware
' (1) Nomad HDATS Robotic Kit
' (1) Parallax Board of Education
' (1) Parallax BS2p Module
' (1) Parallax Servo Controller (PSC)
' (1) Parallax LCD AppMod
' (1) Radio controller Tx/Rx System (optional)
'
'=========================================================================
'Getting Started
'=========================================================================
' 1. Physically Tune the Nomad
' 2. Run the RC_Test.bs2 program (optional - RC unit sold separatly)
' 3. Run the Nomad Demo program
'
'=========================================================================
' First Time Use
'=========================================================================
' Anytime the Nomad Demo program is run for the first time, reset, or powered up
' the Nomad will lower and center its leg joints.
'
' Use the LCD AppMod to:
' 1. Select Speed
' 2. Select
' 3. Run selection
'
' If you are using a radio controller.
'
' 1. Select a speed using button B on the LCD AppMod
' 2. Power the transmitter and receiver
' 3. Press button D (run) on the LCD AppMod
' 4. Move the Transmitter stick
'
' Please read the radio control section below for receiver to BOE
' connection and operation instructions.
'
'=========================================================================
' Parallax LCD AppMod Control
'=========================================================================
' LCD AppMod Buttons
' A ...... Stop/Enter selection mode
' B ...... Select speed (0-15)
' C ...... Select direction (0-11)
' D ...... Run (enter selection)
'
' BUTTON A
' Press and hold button A until the robot stops.  The LCD will the
' show current speed and direction settings.
'
' ----------
' |spd  Dir|
' |0    00 |
' ----------
' A  B  C  D
'
' Button B
' Press button B to change speeds from 0 to 15
' 0 ....... Fastest
' 8 ....... Default
' 15 ...... Slowest
'
' Button C
' Button C controls direction from 0 to 11 where 0 is 12 O’clock.
' If you followed the assembly guide 12 O’clock is the leg connected
' to PSC1 channels 0, 1, and 16.

' Button D
' Press button D to accept/run the selected speed and direction settings.
'
'=========================================================================
' LCD AppMod display
'=========================================================================
' The LCD displays the current speed and direction settings in
' selection mode.
' ----------
' |spd  Dir|
' |0    00 |
' ----------
' A  B  C  D
'
' The LCD shows the current gait setting on line 2 and "LCD Mode" on line 1 while
' the robot is walking.
' ----------
' |LCD Mode|
' |0    00 |
' ----------
' A  B  C  D
'
' The LCD shows the current gait setting on line 2 and "RC Mode" on line 1 while
' the robot is walking under RC control.
' ----------
' |RC Mode |
' |0    00 |
' ----------
' A  B  C  D
'
'=========================================================================
' Radio Control
'=========================================================================
' The Nomad is RC ready.  Simply Plug your RC receiver into the BS2p and
' away you go.  The instructions below are for connecting a
' Tower Hobbies 6 channel radio control system to the BOE.  Your
' receiver channels might differ.  Please refer to your receiver's
' instruction manual.
'
' We suggest running the Nomad_RC_Test.bs2 program before running the
' Nomad in radio control mode.  The Nomad_RC_Test.bs2 program verifies
' that the receiver is connected correctly to the BOE.
' http://www.crustcrawler.com/products/nomad.php?id=3

' Connections
' 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
'
'=========================================================================
' BOE X5 and X4 servo ports
'=========================================================================
'            X5       X4
'        |--------|--------|  |------|
' Ground |  c  c  |  x  x  |  | 5V   |----
' N/C    |  x  x  |  x  x  |  | Reg  |----
' Signal |  c  c  |  x  x  |  |      |----
'        |--------|--------|  |------|
'           12 13    14 15
'
' The Demo program is designed to detect the presence of a receiver
' on Pins 14 and 15.  Once detected simply move the transmitter stick.
' The Nomad should respond.  If not check your connections.
'---- [Programming Notes] ------------------------------------------------
'
'
'-----[ PSC I/O Definitions ]---------------------------------------------
PSC             PIN     15             ' PSC module

#SELECT $STAMP
  #CASE BS2SX, BS2P
    Baud        CON     1021 + $8000
  #CASE BS2PX
    Baud        CON     1646 + $8000
  #CASE #ELSE
    Baud        CON     396 + $8000
#ENDSELECT
'-----[ RC I/O Definitions ]----------------------------------------------
StickXPin       PIN     13             ' Left/Right(X) joystick Rx Ch4
StickYPin       PIN     12             ' Up/Down(Y) joystick Rx Ch3

EEPROM_Block    CON     108            ' EEPROM Block size
' -----[ 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

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)

'-----[ Leg Constants ]---------------------------------------------------
Raise           CON     750
Lower           CON     550
Center          CON     750

'-----[ Speed settings ] -------------------------------------------------
TopSpeed        CON     $A            ' Starting Ramp speed
IncrSpeed       CON     TopSpeed-$2   ' Leg Lift speed
speeds          CON     $10           ' Number of speeds settings (16)

'-----[ Walking Variables ]-----------------------------------------------
stickXPos       VAR     Word           ' Left/Right (X) joystick posn
stickYPos       VAR     Word           ' Up/Down(Y) joystick position
ptrEEPROM       VAR     Word           ' Gait select
char            VAR     Byte           ' character sent to LCD
scan            VAR     Byte           ' loop counter
servoPosition   VAR     Byte(6)        ' Servo Position
servoAddr       VAR     Byte           ' Servo addresses
ramp            VAR     Byte           ' Ramp used in SEROUT
pointer         VAR     Byte           ' pointer to scatchpad
gaitCode        VAR     Byte
temp            VAR     Byte
direction       VAR     gaitCode.LOWNIB
speed           VAR     gaitCode.HIGHNIB
counter         VAR     Nib            ' Count 0 to 5
idx             VAR     Nib            ' loop counter

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

'---- [EEPROM Data] ------------------------------------------------------
Msg1       DATA  "Crust",0
Msg2       DATA  " Crawler",0
Msg3       DATA  "Robotics",0
Msg4       DATA  "Presents"
Msg5       DATA  "The",0
Msg6       DATA  "  NOMAD ",0
Nav        DATA  "Spd  Dir",0
Msg7       DATA  "LCD Mode",0
Msg8       DATA  "RC Mode ",0

'---- [EEPROM Data] ------------------------------------------------------
'Direction inline with a leg
D1 DATA       Word Center,    Word Lower, Word Center,    ' Start Stroke A
              Word Center+50, Word Lower, Word Center-50,
              Word Center-70, Word Lower, Word Center-50,
              Word Center,    Word Raise, Word Center-80, ' Mid Reset B
              Word Center-70, Word Raise, Word Center+10,
              Word Center+50, Word Raise, Word Center+10,

              Word Center,    Word Lower, Word Center-50, ' End Stroke A
              Word Center-70, Word Lower, Word Center+10,
              Word Center+50, Word Lower, Word Center+10,
              Word Center,    Word Lower, Word Center-50, ' Start Stroke B
              Word Center-70, Word Lower, Word Center+10,
              Word Center+50, Word Lower, Word Center+10,

              Word Center,    Word Raise, Word Center,    ' Mid Reset A
              Word Center+50, Word Raise, Word Center+50,
              Word Center-70, Word Raise, Word Center-50,
              Word Center,    Word Lower, Word Center,    ' End Stroke B
              Word Center,    Word Lower, Word Center-20,
              Word Center,    Word Lower, Word Center-20,
              $FF


'Direction "between" legs
D2 DATA       Word Center,    Word Lower, Word Center+24, ' Start Stroke A (Plant)
              Word Center,    Word Lower, Word Center,
              Word Center-20, Word Lower, Word Center-24,
              Word Center+20, Word Raise, Word Center-24, ' Mid Reset B (Lift)
              Word Center,    Word Raise, Word Center,
              Word Center,    Word Raise, Word Center+24,

              Word Center+20, Word Lower, Word Center-24, ' End Stroke A (Propel)
              Word Center-25, Word Lower, Word Center+10,
              Word Center,    Word Lower, Word Center+24,
              Word Center+20, Word Lower, Word Center-24, ' Start Stroke B (Plant)
              Word Center,    Word Lower, Word Center,
              Word Center,    Word Lower, Word Center+24,

              Word Center,    Word Raise, Word Center+24, ' Mid Reset A (Lift)
              Word Center,    Word Raise, Word Center,
              Word Center-20, Word Raise, Word Center-24,
              Word Center,    Word Lower, Word Center+24, ' End Stroke B (Propel)
              Word Center+25, Word Lower, Word Center+10,
              Word Center-20, Word Lower, Word Center-24,
              $FF

'             Horizontal     Vertical       Ankle
Tune DATA     Word 750,      Word 550,      Word 750,
              Word 750,      Word 550,      Word 750,
              Word 750,      Word 550,      Word 750,
              Word 750,      Word 550,      Word 750,
              Word 750,      Word 550,      Word 750,
              Word 750,      Word 550,      Word 750,
              $FF

PUT $0, $0,$4,$8,$6,$A,$2              ' Horizontal leg addresses
PUT $6, $2,$6,$A,$8,$0,$4              ' in scratchpad ram
PUT $C, $4,$8,$0,$A,$2,$6
PUT $12,$6,$A,$2,$0,$4,$8
PUT $18,$8,$0,$4,$2,$6,$A
PUT $1E,$A,$2,$6,$4,$8,$0

Initialize:
  GOSUB Initialize_LCD                 ' Init LCD
  GOSUB CrustCrawler_Logo              ' Write CC Logo

  char = LcdCls                        ' Clear Screen
  GOSUB Write_LCD_Command
  GOSUB LCD_Write_Nav                  ' Write navigation string

  gaitCode = $80                       ' Init gaitCode
  counter = 0                          ' Init counter
  GOSUB LCD_Write_Speed                ' Write Speed
  GOSUB LCD_Write_Direction            ' Write Direction

  ramp = $C                            ' Init ramp
  ptrEEPROM = Tune                     ' Center and lower legs
  GOTO Walking_Engine                  ' Position legs

Parse_GaitCode:                        ' Get speed and direction
  counter = 0                          ' from gaitCode
  IF (direction//2) = 0 THEN
    ptrEEPROM = D1                     'Direction is inline with a leg
  ELSE
    ptrEEPROM = D2                     'Direction is "between" legs
  ENDIF
  pointer = (direction/2)*6            ' pointer to scratch pad ram
  ramp = speed + TopSpeed              ' Set ramp speed
  'DEBUG HEX ?gaitCode, CR, HEX ?pointer, CR, HEX ?ramp

Walking_Engine:
  READ ptrEEPROM, servoPosition(0), servoPosition(1),
                  servoPosition(2), servoPosition(3),
                  servoPosition(4), servoPosition(5)
  DO WHILE (servoPosition(0) <> $FF)   ' loop while not $FF
    GET counter+pointer, servoAddr     ' get horizontal address
    GOSUB Write_PSC                    ' write to psc
    counter = (counter+1)//6           ' incr counter
  LOOP

'-----[ Decisions ]-------------------------------------------------------
Check_First_Run:                       ' First run
  IF (ptrEEPROM - 36) = Tune THEN
    GOTO Select_Speed_Direction
  ENDIF

Check_ButtonA_Press:                   ' Check for button a press
  GOSUB LCD_Get_Buttons
  IF btnA = 1 THEN
    GOTO Select_Speed_Direction
  ENDIF

Check_RC_Power:                        ' Check for Rx signal
  GOSUB Get_Stick
  IF stickXPos = 0  THEN
    'GOTO Check_RC_Power               ' Un-comment for RC only mode
    GOSUB LCD_Write_Walking            ' Write "Walking" on LCD
    GOTO Parse_GaitCode
  ELSE
    GOTO Get_gaitCode_LowNib           ' Convert Rx data to gaitCode
  ENDIF
END

'-------------------------------------------------------------------------
Write_PSC:
  'Horizontal
  SEROUT PSC,Baud,["!SC",servoAddr, ramp,
                   servoPosition(0), servoPosition(1), CR]
  'Vertical
  ramp = ramp - TopSpeed
  servoAddr = (servoAddr+1)
  PAUSE 2
  SEROUT PSC,Baud,["!SC",servoAddr, ramp, servoPosition(2),
                   servoPosition(3), CR]
  'Ankle
  ramp = ramp + TopSpeed
  servoAddr = ((servoAddr-1)/2)+16
  SEROUT PSC,Baud,["!SC",servoAddr, ramp, servoPosition(4),
                   servoPosition(5), CR]
  PAUSE 2
  'Setup for Next Leg
  ptrEEPROM = ptrEEPROM + 6
  READ ptrEEPROM, servoPosition(0), servoPosition(1),
                  servoPosition(2), servoPosition(3),
                  servoPosition(4), servoPosition(5)
  PAUSE 2
RETURN

'-----[ LCD Subroutines ]-------------------------------------------------
Select_Speed_Direction:                ' Select speed and direction
  GOSUB LCD_Write_Nav
  DO WHILE btnD = 0                    ' Do while D not pressed
    GOSUB LCD_Get_Buttons
    IF btnB = 1 THEN                   ' Speed setting
      speed = (speed+1)//speeds
      GOSUB LCD_Write_Speed            ' Write speed to LCD
    ENDIF
    IF btnC = 1 THEN                   ' Direction setting
      direction = (direction+1)//12
      GOSUB LCD_Write_Direction        ' Write direction to LCD
    ENDIF
  LOOP
  GOSUB LCD_Write_Walking              'Write "Walking"
GOTO Parse_GaitCode

  LCD_Get_Buttons:
    LcdDirs = %0000                    ' make LCD bus inputs
    buttons = %1111                    ' assume all pressed
    FOR scan = 1 TO 10
      buttons = buttons & LcdBusIn     ' make sure button held
      PAUSE 7                          ' debounce 10 x 5 ms
    NEXT
    LcdDirs = %1111                    ' return bus to outputs
  RETURN

LCD_Put_String:
  DO
    READ ptrEEPROM, char               ' Read data at EEPROM address
    IF (char = 0) 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:                     ' Low RS = LCD directive like
    LCDCMD E, char
  RETURN

  Write_LCD_Char:                      ' HIGH RS = Write a character
    LCDOUT E, 0, [char]
  RETURN

LCD_Write_Speed:
  char = LcdLine2+0                    ' Write speed to LCD
  GOSUB Write_LCD_Command
  temp = speed/10                      ' Convert to ASCII
  char = temp+48
  GOSUB Write_LCD_Char

  char = LcdLine2+1
  GOSUB Write_LCD_Command

  temp = speed//10
  char = temp+48
  GOSUB Write_LCD_Char
RETURN

LCD_Write_Direction:                   ' Write Direction to LCD
    char = LcdLine2+5
    GOSUB Write_LCD_Command
    temp = direction/10                ' Convert to ASCII
    char = temp+48
    GOSUB Write_LCD_Char

    char = LcdLine2+6
    GOSUB Write_LCD_Command

    temp = direction//10
    char = temp+48
    GOSUB Write_LCD_Char
RETURN


LCD_Write_Nav:
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Nav                      ' Point to Nav
  GOSUB LCD_Put_String                 ' Write string
RETURN

LCD_Write_Walking:
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Msg7                     ' Point to message 7
  GOSUB LCD_Put_String                 ' Write string
RETURN

LCD_Write_RC_Mode:
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Msg8                     ' Point to message 8
  GOSUB LCD_Put_String                 ' Write string
RETURN

CrustCrawler_Logo:                     ' Dispaly CC logo
    ptrEEPROM = Msg1                   ' Point to message 1
    GOSUB LCD_Put_String               ' Write string
    char = LcdLine2+0                  ' Go to Line 2
    GOSUB Write_LCD_Command            ' Write command
    ptrEEPROM = Msg2                   ' Point to message 2
    GOSUB LCD_Put_String               ' Write string
    PAUSE 2000                         ' wait 2 secs

    'char = LcdCls                      ' Clear Screen
    'GOSUB Write_LCD_Command

    'ptrEEPROM = Msg3                   ' Point to message 3
    'GOSUB LCD_Put_String               ' Write string
    'char = LcdLine2+0                  ' Go to Line 2
    'GOSUB Write_LCD_Command            ' Write command
    'ptrEEPROM = Msg4                   ' Point to message 4
    'GOSUB LCD_Put_String               ' Write string
    'PAUSE 2000

    char = LcdCls
    GOSUB Write_LCD_Command

    ptrEEPROM = Msg5                   ' Point to message 5
    GOSUB LCD_Put_String               ' Write string
    char = LcdLine2+0                  ' Go to Line 2
    GOSUB Write_LCD_Command            ' Write command
    ptrEEPROM = Msg6                   ' Point to message 6
    GOSUB LCD_Put_String               ' Write string
    PAUSE 2000
  RETURN

Initialize_LCD:
  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
  char = LcdCls                        ' Clear screen
  GOSUB Write_LCD_Command
RETURN
'-----[ RC ]--------------------------------------------------------------
Get_gaitCode_LowNib:
GOSUB LCD_Write_RC_Mode                ' Write "RC Mode" to LCD
GOSUB Get_Stick                        ' Convert stick positions to
  SELECT stickYpos                     ' 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

  IF gaitCode.LOWNIB = $F THEN         ' Neutral
    GOTO Get_gaitCode_LowNib
  ENDIF

  GOSUB LCD_Write_Direction            ' Write direction to LCD
  GOTO Parse_GaitCode

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