' ==============================================================================
'
'   File...... GaitPIC-Demo.bsp
'   Purpose... Nomad GaitPIC Demo Program
'   Author.... (C) 2006 Steven R. Norris -- All Rights Reserved
'              Based on a program by Mike Gebhard of CrustCrawler
'   E-mail.... norris56@comcast.net
'   Started... 06/29/2006
'   Updated... 07/03/2006
'
'   {$STAMP BS2p}
'   {$PBASIC 2.5}
'
' ==============================================================================

' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------
' This is a demostration of the Nomad GaitPIC. It is based on the original
' Nomad walking demo program written by Mike Gebhard. Most of the button and LCD
' routines were borrowed from his program. All the walking routines have been replaced
' by commands going out to the GaitPIC. The GaitPIC is an external microcontroller
' (16F688) that provides all the servo coordination needed for walking.
' The Basic Stamp communicates to the GaitPIC via a serial line at 38400 baud.

' Note: The R/C interface in not in this version.

' The User Interface:
' LCD AppMod Buttons
' A ...... Select Gait (0-2)
' B ...... Select speed (0-3)
' C ...... Select direction (0-5)
' D ...... Run/Stop
'
' ----------
' |Gt Sp Dr|
' |00 01 00|
' ----------
' A  B  C  D
'
' Button A
' Press button A to change gait from 0 to 2
' 0 ....... Walk
' 1 ....... Spin CW
' 2 ....... Spin CCW
'
' Button B
' Press button B to change speeds from 0 to 3
' 0 ....... Fastest
' 1 ....... Default
' 2 ....... Slower
' 3 ....... Slower with high stepping
'
' Button C
' Button C controls direction from 0 to 5
' 0 ....... 0 degrees
' 1 ....... 60 degrees
' 2 ....... 120 degrees
' 3 ....... 180 degrees (reverse)
' 4 ....... 240 degrees
' 5 ....... 300 degrees

' Button D
' Press button D to run/stop the selected speed and direction settings.
'
' Button A and B together
' Press button A and B at the same time to "home" the legs
'
'=========================================================================
' LCD AppMod display
'=========================================================================
' The LCD displays the current gait, speed and direction settings in
' selection mode.
' ----------
' |Gt Sp Dr|
' |00 01 00|
' ----------
' A  B  C  D
'
' The LCD shows the current gait settings on line 2 and "Walking" on line 1 while
' the robot is walking.
' ----------
' |Walking |
' |00 01 00|
' ----------
' A  B  C  D
'
' ------------------------------------------------------------------------------
' Revision History
' ------------------------------------------------------------------------------

' 0627a - First version

' ------------------------------------------------------------------------------
' I/O Definitions
' ------------------------------------------------------------------------------

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

' LCD
E               PIN     1              ' LCD Enable (1 = enabled)
RW              PIN     2              ' Read/Write
RS              PIN     3              ' Reg Select (1 = char)

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

' GaitPIC baud rate
Gpbaud          CON     45               ' 38400 baud

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

' GaitPIC parameters
Gait            VAR     Byte
Direction       VAR     Byte
Speed           VAR     Byte
GaitCount       VAR     Byte

' LCD
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)

ptrEEPROM       VAR     Word
char            VAR     Byte           ' character sent to LCD
scan            VAR     Byte           ' loop counter
temp            VAR     Byte

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

Nav        DATA  "Gt Sp Dr",0
Msg1       DATA  "Walking ",0
Msg2       DATA  "Stopped ",0

' ------------------------------------------------------------------------------
' 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 all the legs
  LOW Pin_GpStop
  GOSUB HomeIt

  ' Default speed
  Speed = 1

  ' Setup the user interface
  GOSUB Initialize_LCD                 ' Init LCD
  char = LcdCls                        ' Clear Screen
  GOSUB Write_LCD_Command
  GOSUB LCD_Write_Nav                  ' Write navigation string
  GOSUB LCD_Write_Gait                 ' Write Gait
  GOSUB LCD_Write_Speed                ' Write Speed
  GOSUB LCD_Write_Direction            ' Write Direction

  ' ------------------------------------------------------------------------------
' Program Code
' ------------------------------------------------------------------------------
Main:
  GOSUB Select_Speed_Direction

  DO
    GOSUB LCD_Get_Buttons
    IF btnD = 1 THEN
      DO WHILE btnD = 1
        GOSUB LCD_Get_Buttons
      LOOP

      GOSUB LCD_Write_Stop

      IF Pin_GpBusy = 1 THEN
        GOSUB StopIt
        GOSUB Select_Speed_Direction
      ENDIF
    ENDIF
  LOOP
  END

'-----[ 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 btnA = 1 AND btnB = 0 THEN      ' Gait setting
      Gait = (Gait+1)//3
      GOSUB LCD_Write_Gait             ' Write gait to LCD
    ENDIF

    IF btnB = 1 AND btnA = 0 THEN      ' Speed setting
      Speed = (Speed+1)//4
      GOSUB LCD_Write_Speed            ' Write speed to LCD
    ENDIF

    IF btnA = 1 AND btnB = 1 THEN      ' Home
      GOSUB HomeIt
    ENDIF

    IF btnC = 1 THEN                   ' Direction setting
      Direction = (Direction+1)//6
      GOSUB LCD_Write_Direction        ' Write direction to LCD
    ENDIF
  LOOP

  DO WHILE btnD = 1
    GOSUB LCD_Get_Buttons
  LOOP

  GOSUB LCD_Write_Walking              ' Write "Walking"
  GaitCount = 254                      ' Continuous
  GOSUB WalkIt
  RETURN

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 7 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_Gait:
  char = LcdLine2+0                    ' Write gait to LCD
  GOSUB Write_LCD_Command
  temp = gait/10                      ' Convert to ASCII
  char = temp+48
  GOSUB Write_LCD_Char

  char = LcdLine2+1
  GOSUB Write_LCD_Command

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

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

  char = LcdLine2+4
  GOSUB Write_LCD_Command

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

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

  char = LcdLine2+7
  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 = Msg1                     ' Point to message 1
  GOSUB LCD_Put_String                 ' Write string
  RETURN

LCD_Write_Stop:
  char = LcdLine1+0
  GOSUB Write_LCD_Command
  ptrEEPROM = Msg2                     ' Point to message 2
  GOSUB LCD_Put_String                 ' Write string
  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

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