'------- [Quad_LED_Button_RC ] -------------------------------------------
'{$STAMP BS2}
'{$PBASIC 2.5}
'
'   File....... Quad_LED_Button_RC.BS2
'   Purpose.... Use EEPROM tables, LED display, two button interface
'               and a Tower Hobbies Tx/Rx to produce 15 different gaits.
'   Author..... CustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 16 April 2004
'   Updated.... 28 November 2004
'   Version.... 1.1
'
'=====[ Updates ]=========================================================
' Updates:
' 1. Updated EEPROM tables.
'     a. Aggresive stride
'     b. Less servo strain
' 2. Programmatic horizontal leg adjustment
' 3. Updated comments
'
'=========================================================================
' Programmatic Adjustments
' Find the section of code below:
'=========================================================================
'-----[ Horizontal Leg Constants ]----------------------------------------
' Center1         CON     750     ' Leg1 horizontal servo
' Center2         CON     750     ' Leg2 horizontal servo
' Center3         CON     750     ' Leg3 horizontal servo
' Center4         CON     750     ' Leg4 horizontal servo
'
' Replace these constants with the constants
' you found using the HomeQuad.bs2 program.
'
' Example Quad center constants yours will vary
'-----[ Horizontal Leg Constants ]----------------------------------------
'Center1         CON     725
'Center2         CON     800
'Center3         CON     785
'Center4         CON     750
'
'=========================================================================
' Button operation:
'=========================================================================
' Press either button during program execution to enter selection mode.
' Press the up and down buttons to select gaits.
' Press both buttons to accept selection
'
' If zero is selected the robot will center and lower it's legs
' for adjustment according to the quadCrawler assembly guide.
' Press the BOE reset button to restart.
'
'=========================================================================
' Tower Hobbies 6 channel FM Radio Control System
'=========================================================================
' RC Operation:
' Left Stick positions
'                            Up
'          ______________________________________
'         |            |            |            |
' Fast -> |   Left     |  Forward   |  Right     | <- Top
'         |------------|------------|------------|
' Slow -> |   Left     |  Forward   |  Right     | <- Top Mid
'         |------------|------------|------------|
' Spin -> |   Left     |  Stop      |  Right     | <- Center
'         |------------|------------|------------|
' Slow -> |   Left     |  Backward  |  Right     | <- Bottom Mid
'         |------------|------------|------------|
' Fast -> |   Left     |  Backward  |  Right     | <- Bottom
'         |____________|____________|____________|
'                           Down
'
' Connections:
' You will need 2 female-female servo wires to connect the receiver
' to the BOE's X5 port (see your BOE Rev B documentation).
' You might have to make your own connectors.
' Connect the receiver's Throttle to X5 Pin 12 on the BOE.
' Connect the Rudder to X5 Pin 13.
'
' The Get_Stick routine uses the PULSIN command
' to measures the pulse width on pin 12(Y)StickYPin
' and pin 13(X)StickXPin. This data is stored in
' the stickYPos and stickXPos variables respectively.
'
' Left Stick positions vs pulse readings
'             Left        Center       Right
'          ______________________________________
'         |            |            |            |
'         |  580(Y)    |  580(Y)    |  580(Y)    |
'         |  545(X)    |  765(X)    |  974(X)    | Top
'         |------------|------------|------------|
'         |  685(Y)    |  685(Y)    |  685(Y)    |
'         |  545(X)    |  765(X)    |  974(X)    | Top Mid
'         |------------|------------|------------|
'         |  545(X)    |  765(X)    |  974(X)    |
'         |  765(Y)    |  765(Y)    |  765(Y)    | Center
'         |------------|------------|------------|
'         |  545(X)    |  765(X)    |  974(X)    |
'         |  865(Y)    |  865(Y)    |  865(Y)    | Bottom Mid
'         |------------|------------|------------|
'         |  545(X)    |  765(X)    |  974(X)    |
'         |  945(Y)    |  945(Y)    |  945(Y)    | Bottom
'         |____________|____________|____________|
'
' Next, Get_Stick uses a SELECT command to evaluate
' stickYPos and assign a hex byte to the gaitCode
' variable.
' 580 to 650 = $10
' 651 TO 730 = $20
' 731 TO 800 = $00
' 801 TO 870 = $30
' 871 TO 960 = $40
'
' Lastly, Get_Stick uses another SELECT command to
' evaluate stickXPos.
' 500 TO 700 = $01
' 701 TO 820 = $00
' 821 TO 999 = $02
' The results are ORed with gaitCode.
' The value is stored in gaitCode and
' displayed using the DEBUG command.
'
' Left Stick positions vs gaitCode value
'             Left        Center       Right
'          ______________________________________
'         |            |            |            |
'         |    $11     |    $10     |   $12      | Top
'         |------------|------------|------------|
'         |    $21     |    $20     |   $22      | Top Mid
'         |------------|------------|------------|
'         |    $01     |    $00     |   $02      | Center
'         |------------|------------|------------|
'         |    $21     |    $20     |   $22      | Bottom Mid
'         |------------|------------|------------|
'         |    $31     |    $30     |   $32      | Bottom
'         |____________|____________|____________|
'
'=========================================================================
' LED vs gaitCode table
'=========================================================================
' LED   gaitCode
' (0)   $00 - Home
' (1)   $01 - Spin Left
' (2)   $02 - Spin Right
'
' LED   gaitCode                    LED  gaitCode
' (3)   $10 - Forward Fast         (6)   $20 - Forward
' (4)   $11 - Fast Forward Left    (7)   $21 - Forward Left
' (5)   $12 - Fast Forward Right   (8)   $22 - Forward Right
'
' LED   gaitCode                   LED   gaitCode
' (9)   $30 - Backward             (C)   $40 - Fast Backward
' (A)   $31 - Backward Left        (d)   $41 - Fast Backward Left
' (b)   $32 - Backward Right       (E)   $42 - Fast Backward Right
'                                  (F)   Open
'
' 7 Segement LED Display and gaitCode EEPROM data:
' The 7 Segement LED data starts at the address labeled LED.
' gaitCode information starts at the address labeled Gait.
' btnIndx is a pointer to the LED EEPROM data.
' The two sets of EEPROM data form a parallel array.  If btnIndx
' is pointing to EEPROM address 3, the segment display value
' is $3D. Making the segments variable equal to $3D causes a 3 to
' show on the LED. Then, btnIndx + Gait is the gaitCode $10 or
' walk forward.
'
' The Get_GaitCode sub routine is responsible for aligning
' the LED display to the gaitCode.
'
'---- [7 Segment LED and gaitCode EEPROM Data] -----
' Hex #     0   1   2   3   4   5   6   7   8   9   A   B   C   D   E
'LED  DATA $7E,$18,$6D,$3D,$1B,$37,$77,$1C,$7F,$1F,$5F,$73,$66,$79,$67
'Gait DATA $00,$01,$02,$10,$11,$12,$20,$21,$22,$30,$31,$32,$40,$41,$42
'
' Gait EEPROM Data:
' Gait EEPROM data is stored in 3 byte sections.
' Servo address, LOWBYTE of servo position, and
' HIGHBYTE of servo position.  Data is READ from
' EEPROM 3 bytes at a time then sent to the PSC.
' Allowing control over a single leg servo.
'
' How does it work?
' ptrEEPROM, rightRamp, and leftRamp variables
' are all you need to control the robot.
'
' ptrEEPROM is a pointer to EEPROM addresses. Assigning
' ptrEEPROM to "Forward" (ptrEEPROM = Forawrd) places
' the pointer at the starting EEPROM address for
' forward motion.
'
' rightRamp and leftRamp control servo speeds (ramp) on
' the left or right side of the QuadCrawler.
' Slowing the servos on one side of the robot causes
' the QuadCralwer to make gradual turns.
'
' Sub Routines:
' Walking_Engine:
' The "Walking_Engine" sub routine READs the value stored in
' the address "ptrEEPROM" and assigns servo speed (ramp).
' Walking_Engine only READs servo addresses $00 to $0B (0-11).
' I'll explain this later.
'
' If Walking_Engine encounters an $FF it has reached the
' end of the Forward EEPROM data. Then ptrEEPROM is reset
' and the process repeats.
'
' If Walking_Engine READS an odd servo address it knows it is
' dealing with a vertical servo and assigns a very
' fast ramp value to lift a leg quickly.
'
' If the servo address is even Walking_Engine assigns ramp
' values depending on leg location, left or right
' side of the robot.
'
' Walking_Engine then passes control and the ptrEEPROM variable
' and the Write_PSC sub routine.
'
' Write_PSC:
' The "Write_PSC" sub uses "ptrEEPROM"
' to READ 3 consecutive bytes of data stored
' in EEPROM. Servo Address, LOWBYTE of servo position,
' and HIGHBYTE of servo position.  This data is
' written to the PSC with a SEROUT command.
' ptrEEPROM is updated to point to the next servo
' address in EEPROM and control is returned to
' Walking_Engine. This is why Walking_Engine only
' sees servo addresses.
'
' Adjustable values:
' [Adjustable Ramp Value section]
' Ramp is a prameter passed to the PSC with the SEROUT command.
' See your PSC instruction manual for more information.
' Ramp is the speed/time it takes for a servo
' to move to a new position.  Small values, 1-7,
' move fast while larger values, 8-15($F), move
' slow.
'
' Adjust the these values to increase/decrease
' servo speeds.  Experiment to find the values
' that work best for your robot.
'
'=========================================================================
' Walk Forward Diagram
'=========================================================================
'
'      **Right Side**        |
'                            |
' F    \          /          |        /      \
' O    _\_      _/_          |      _/_      _\_
' R   / 1 \____/ 2 |         |     / 1 \____/ 2 |
' W  |             |         |    |             |
' A  |     ____    |         |    |     ____    |
' R   \_3_/    \_4_|         |     \_3_/    \_4_|
' D     \        /           |       /        \
'        \      /            |      /          \
'
'
' 7 Segement LED Display:
'    Segment map:      .edc bafg  HEX      .edc bafg  HEX
'       (a)         0 %0111 1110  $7E   8 %0111 1111  $7F
'      -----        1 %0001 1000  $18   9 %0001 1111  $1F
' (f) |     | (b)   2 %0110 1101  $6D   A %0101 1111  $5F
'     | (g) |       3 %0011 1101  $3D   B %0111 0011  $73
'      -----        4 %0001 1011  $1B   C %0110 0110  $66
' (e) |     | (c)   5 %0011 0111  $37   D %0111 1001  $79
'     |     |       6 %0111 0111  $77   E %0110 0111  $67
'      -----        7 %0001 1100  $1C   F %0100 0111  $47 - Open
'
'-------------------------------------------------------------------------

' -----[ I/O Definitions ]------------------------------------------------
PSC             PIN     15              ' PSC module
ModeBtn         PIN     8               ' select robot mode up
StartBtn        PIN     12              ' select robot mode down
segments        VAR     OUTL            ' output on pins 0 - 7
Baud            CON     33164           ' 2400 baud
StickXPin       PIN     13              ' Left/Right(X) joystick Rx Ch4
StickYPin       PIN     12              ' Up/Down(Y) joystick Rx Ch3

'---- [Button Variables]--------------------------------------------------
btns            VAR     Nib             ' button holder
btn1            VAR     btns.BIT0       ' debounced button value
btn2            VAR     btns.BIT1       ' deboucned button value