'----[RC_Test.BS2]-----------------------------------------
' {$STAMP BS2}
' {$PBASIC 2.5}
'
'   File....... RC_Test.BS2
'   Purpose.... Test Radio Controller connections
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 16 July 2004
'   Updated.... 23 December 2004
'
' Hardware:
' (1) Parallax Basic Stamp Module
' (1) Parallas BOE
' (1) Tower Hobbies 6 channel FM Radio Control System
'
' Use this code to troubleshoot connecting a Radio
' Control unit to a BS2.  It accepts readings from
' a radio control receiver and converts the reading
' to a byte value. This code also detects if the
' transmitter is turn off or on.  This code is
' designed to be used with all Crawler walking code.
'
'=========================================================================
' Getting Started
'=========================================================================
' 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
'
'=========================================================================
' Program Operation Overview
'=========================================================================
' 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 two results are ORed together and
' the value is stored in gaitCode variable.
' gaitcode results are displayed on the debug screen.
'
' 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
'         |____________|____________|____________|
'
'
' The Get_Stick routine is designed to plug into all CrustCrawler
' robot programs.
'
'=========================================================================
' How to use this program
'=========================================================================
' Connect the Rx to the the BOE and download the code.  Power the BOE, Rx,
' and Tx.  the debug screen will display "RC On".  Move the joystick to
' the top left.  You should see 11 on the debug screen.  If not check your
' connections.
'
' Now you can run any of the CrustCrawler RC programs.
'-------------------------------------------------------------------------

' -----[ I/O Definitions ]------------------------------------------------
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

DEBUG "==== RC Test ====",CR

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

'---- [Troubleshoot] -----------------------------------------------------
' Un-comment the lines below to display
' raw PULSIN values on pins 12 and 13

'DEBUG CLS, "Raw PULSIN values", CR, ?stickYPos, ?stickXPos
'PAUSE 1000
'GOTO Get_Stick
'-------------------------------------------------------------------------

RC_Signal_Check:
  IF (stickXPos & stickYPos) < 500 THEN
    DEBUG CRSRXY, 0, 1, "RC Off ",CR
    displayCode = 0
  ELSE
    DEBUG CRSRXY, 0, 1, "RC On  ",CR
    displayCode = 1
  ENDIF

Get_High_Nib:
  SELECT stickYPos
    CASE 580 TO 650
      gaitCode = $10
    CASE 651 TO 730
      gaitCode = $20
    CASE 731 TO 800
      gaitCode = $00
    CASE 801 TO 870
      gaitCode = $30
    CASE 871 TO 960
      gaitCode = $40
  ENDSELECT

Get_Low_Nib:
  SELECT stickXPos
    CASE 500 TO 700
      gaitCode = gaitCode | $01
    CASE 701 TO 820
      gaitCode = gaitCode | $00
    CASE 821 TO 999
      gaitCode = gaitCode | $02
  ENDSELECT

Display_gaitCode:
  IF displayCode = 0 THEN
    DEBUG CRSRXY, 0, 2, CLREOL, "**"
  ELSE
    DEBUG CRSRXY, 0, 2, CLREOL, HEX gaitCode
  ENDIF
GOTO Get_Stick                      ' loop forever