'----[Nomad_RC_Test.bsp]-----------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... Nomad_RC_Test.bsp
'   Purpose.... Test Radio Controller connections
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 10 February 2005
'   Updated....
'
' Hardware:
' (1) Parallax Basic Stamp P Module
' (1) Parallax BOE
' (1) Tower Hobbies 6 channel FM Radio Control System
'
' Use this code to troubleshoot connecting a Radio Control unit
' to a Parallax BOE/Basic Stamp.  It accepts readings from a radio
' control receiver and converts the reading to a byte value (gaitCode).
' This program also detects if the transmitter is turn on or off
' and is designed to be used with all 3DOF 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.
'
' StickXPos Values
' Left     Right
' 1465 --> 2610
'
' StickYPos Values
' Top  1580
'        |
'        |
' Down 2525
'
' StickXPos and StickXPos variables are converted to a gaitCode.
' The results are displayed on the DEBUG screen where a gaitCode of
' 0 is 12 O'clock.  A gaitCode of F is neutral or stop.
'
'=========================================================================
' Debug Output vs. Stick Position
'=========================================================================
'
' |-----|-----|-----|-----|-----|
' |  7  |  0  |  0  |  0  |  1  |
' |-----|-----|-----|-----|-----|
' |  6  |  F  |  F  |  F  |  2  |
' |-----|-----|-----|-----|-----|
' |  6  |  F  |  F  |  F  |  2  |
' |-----|-----|-----|-----|-----|
' |  6  |  F  |  F  |  F  |  2  |
' |-----|-----|-----|-----|-----|
' |  5  |  4  |  4  |  4  |  3  |
' |-----|-----|-----|-----|-----|

'-------------------------------------------------------------------------

' -----[ 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] -----------------------------------------------------
' #DEFINE debugMode = 1 -> gaitCode display
' #DEFINE debugMode = 0 -> raw PULSIN values
' on pins 12 and 13
#DEFINE debugMode = 1

#IF debugMode = 0 #THEN
  DEBUG CLS, "Raw PULSIN values", CR, ?stickYPos, ?stickXPos
  PAUSE 300
  GOTO Get_Stick
#ENDIF
'-------------------------------------------------------------------------

gaitCode = $00

RC_Signal_Check:                    ' Check for signal
  IF stickXPos = 0  THEN
    DEBUG CRSRXY, 0, 1, "RC Off ",CR
    stickYPos = 0
    displayCode = 0
  ELSE
    DEBUG CRSRXY, 0, 1, "RC On  ",CR
    displayCode = 1
  ENDIF

Get_gaitCode_LowNib:
  SELECT stickYpos                  ' Convert to gaitCode
    CASE < 1800
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $1
        CASE < 1800
          gaitCode.LOWNIB = $7
        CASE ELSE
          gaitCode.LOWNIB = $0
      ENDSELECT
    CASE < 2200
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $2
        CASE < 1800
          gaitCode.LOWNIB = $6
        CASE ELSE
          gaitCode.LOWNIB = $F
       ENDSELECT
    CASE < 2600
      SELECT stickXPos
        CASE > 2200
          gaitCode.LOWNIB = $3
        CASE < 1800
          gaitCode.LOWNIB = $5
        CASE ELSE
          gaitCode.LOWNIB = $4
       ENDSELECT
    CASE ELSE
      gaitCode.LOWNIB = $F
  ENDSELECT

#IF debugMode #THEN
Display_gaitCode:                   ' Display results
  DEBUG CR
  IF displayCode = 0 THEN
    DEBUG CRSRXY, 0, 3, CLREOL, "No Signal"
  ELSE
    DEBUG CRSRXY, 0, 3, CLREOL, "gaitCode = ", HEX gaitCode
  ENDIF
#ENDIF
GOTO Get_Stick                      ' loop forever