' ==============================================================================
'
'   File...... GaitPIC-Test.bsp
'   Purpose... Set the horizontal center positions using the GaitPIC
'   Author.... (C) 2006 Steven R. Norris -- All Rights Reserved
'   E-mail.... norris56@comcast.net
'   Started... 06/02/2006
'   Updated...
'
'   {$STAMP BS2p}
'   {$PBASIC 2.5}
'
' ==============================================================================

' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------

' ------------------------------------------------------------------------------
' Revision History
' ------------------------------------------------------------------------------

' 0622a - First version

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

Piezo           PIN     4               ' Piezo Speaker

' GaitPIC interface
Pin_GpBusy      PIN     7               ' Busy
Pin_GpStop      PIN     9               ' All stop
Pin_GP          PIN     11              ' Serial commands

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

' GaitPIC baud rate
' Set for the type of Basic Stamp you are using
Gpbaud          CON     1021            ' 2400 baud

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

Gait            VAR     Byte
GaitCount       VAR     Byte

counter         VAR     Byte

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

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

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

  HIGH Pin_GpStop
  HIGH Pin_GP
  PAUSE 500

  FREQOUT Piezo, 2000, 3000               ' Signal Program Start/Reset

  ' Home
  LOW Pin_GpStop
  Gait = 0
  GaitCount = 1
  GOSUB SetGaitWait

  PAUSE 2000

' ------------------------------------------------------------------------------
' Program Code
' ------------------------------------------------------------------------------
Main:
  DO
    ' Forward
    FOR counter = 1 TO 12
      Gait = 3
      GaitCount = 1
      GOSUB SetGaitWait
    NEXT

    ' Back
    FOR counter = 1 TO 4
      Gait = 12
      GaitCount = 1
      GOSUB SetGaitWait
    NEXT

    ' Spin Left 180
    FOR counter = 1 TO 15
      Gait = 1
      GaitCount = 1
      GOSUB SetGaitWait
    NEXT

    ' Forward
    FOR counter = 1 TO 8
      Gait = 3
      GaitCount = 1
      GOSUB SetGaitWait
    NEXT

    ' Spin Right 180
    FOR counter = 1 TO 15
      Gait = 2
      GaitCount = 1
      GOSUB SetGaitWait
    NEXT
  LOOP

  STOP
  END

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

SetGaitWait:
  SEROUT Pin_GP, GpBaud, ["!w",Gait,GaitCount,$FF]
  DO WHILE Pin_GpBusy = 0
  LOOP
  PAUSE 1
  DO WHILE Pin_GpBusy = 1
  LOOP
  RETURN