'----[Five_Channel_RC.bsp]-----------------------------------------
' {$STAMP BS2p}
' {$PBASIC 2.5}
'
'   File....... Five_Channel_RC.bsp
'   Purpose.... Radio control code used in the
'               RC SG5-UT arm
'   Author..... CrustCrawler Inc. (Mike Gebhard)
'   E-mail..... support@crustcrawler.com
'   Started.... 23 May 2005
'   Updated.... 23 May 2005
'
' Hardware:
' (1) Parallax Basic Stamp P Module
' (1) Parallax BOE
' (1) Tower Hobbies System 3000 6 channel FM Radio Control System
' (1) Tower Hobbies System 3000 7 channel receiver
'
'=========================================================================
' Getting Started
'=========================================================================
' CONNECTIONS
' You will need 10 wires to connect the receiver
' to the BOE's X5 servo port (see your BOE Rev B documentation)
' and/or I/O pins.
'
' Soldering High Density Female RS232 sockets to the ends
' of 20 gage wires works great.
'
' Don't forget that each signal (pin) connection
' requires a ground connection (Vss)
' on the BOE/BS2p.
'
' BS2p Pin to Tower Hobbies Rx connections:
' Pin 10.........ch6 (flaps)
' Pin 11.........ch2 (elevator)
' Pin 12.........ch3 (throttle)
' Pin 13.........ch4 (rudder)
' Pin 14.........ch1 (aileron)
'
'=========================================================================
' BOE X5 and X4 servo ports
'=========================================================================
'            X5       X4
'        |--------|--------|  |------|
' Ground |  3  4  |  1  x  |  | 5V   |----
' N/C    |  x  x  |  x  x  |  | Reg  |----
' Signal |  3  4  |  1  x  |  |      |----
'        |--------|--------|  |------|
'           12 13    14 15
'
'=========================================================================
' Program Overview
'=========================================================================
' Use this code to troubleshoot connecting a radio control unit to a BS2p.
' It accepts readings from an RC receiver then updates an array used to
' position the SG5-UT arm joints.  This code also detects If the receiver
' is powered and the transmitter is sending a signal.
'
'=========================================================================
' Controlling the SG5-UT arm with an RC unit
'=========================================================================
' This code is not connected directly to the SG5-UT arm but it is linked
' to the variables that are passed to the arm control program.
'
' This code reads 5 receiver channels connected to BS2p PINS
' with the PULSIN command.
' Pin 10.........ch6 (flaps)
' Pin 11.........ch2 (elevator)
' Pin 12.........ch3 (throttle)
' Pin 13.........ch4 (rudder)
' Pin 14.........ch1 (aileron)
'
' Action is taken if any of the transmitter sticks are moved from center.
'
'=========================================================================
' Left Stick positions vs BS2p PULSIN readings
'=========================================================================
'
' StickXPos PUSLIN Values
' Left     Mid     Right
' 1465 --> 2037--> 2610
'
' StickYPos PUSLIN Values
' Top  1580
'        |
'      2052
'        |
' Down 2525
'
'=========================================================================
' The next sections describe in detail each sub routine.
'=========================================================================
' Each sub routine can take input parameters and return or update
' variables.
'
' Example:
' sub(input parameter<data type>) return variable<data type>.
'
' The next several sections describe parameters, return values, and how
' the routine function.
'
'=========================================================================
' Get_Receiver_Channel_Values
'=========================================================================
' sub() return ctrlByte<byte>
'
' - ctrlByte This byte controls which SG5-UT joint to move.  It is made
'            up of the aliases below.
'
'  baseBit             VAR     ctrlByte.BIT0   ' 0 = no change
'  biceptBit           VAR     ctrlByte.BIT1   ' 1 = joint value changed
'  elbowBit            VAR     ctrlByte.BIT2
'  wristBit            VAR     ctrlByte.BIT3
'  gripperBit          VAR     ctrlByte.BIT5
'
' This routine loops through the BS2p pins that are connected to the
' RC receiver (10...14).  It checks if any of the receiver outputs
' indicate that the transmitter sticks moved from center.  In other
' words if a transmitter stick is pushed up, down, left, or right.
' The BS2p detects transmitter stick position by measuring the pulse width
' (PULSIN) on a pin. For the BS2p anything between (2800 and 2200) or
' (400 and 1800) indicates that a transmitter control is not centered
'
' Pin 10.........ch6 (flaps)
' Pin 11.........ch2 (elevator)
' Pin 12.........ch3 (throttle)
' Pin 13.........ch4 (rudder)
' Pin 14.........ch1 (aileron)
'
' The 5 transmitter controls (flaps, elevator, throttle, rudder, and
' aileron) have a related SG5-UT arm joint.
'
' If any transmitter control is not centered then the control bit related
' to the selected transmitter control is set to 1.  This tells the
' program which arm joint to move.  Setting baseBit = 1 means move the
' base servo.
'
' Not only is a bit in the ctrlByte set but the array() value that
' controls that particular joint position is also updated.  The array is
' indexed by make the pscChannel variable equal to the current joint
' number For example, pscChannel = base | array(pscChannel) references
' the base servo position.  This process also aligns the PSC channel
' to the array element.
'
'=========================================================================
' Incr_Decr_Servo_Position_Array
'=========================================================================
' sub(pscChannel<nib> channelValue<word>) return array(pscChannel)<byte>
'
' - pscChannel This variable holds the PSC channel related to the current
'              joint to move and the array element that holds the joint
'              position.
'
' - channelValue Pulse width measured by the PULSIN command
'
' Incr_Decr_Servo_Position_Array increments or decrements the position
' array(pscChannel) by the constant degrees which equals 5.  If
' channelValue is greater than 2200 array(pscChannel) is incremented by 5
' If channelValue is less than 1800 array(pscChannel) is decremented by 5
'
' Incr_Decr_Servo_Position_Array also checks to see if the bicept servo
' was just updated.  If so, it does the opposite math to the eblow array
' element.
'
' Therefore, incrementing the bicept position cause the elbow position
' to decrement.
'
' Note: The elbow servo is also controlled by the trim knob.
'
'=========================================================================
' Receiver_Power_Check
'=========================================================================
' sub() return RC<bit>
'
' - RC RC mode flag
'
' Receiver_Power_Check checks pin 10 for a signal.  If a signal is
' detected RC=1 else RC=0.  The RC bit can be used to make a decision like
' if the receiver is off go to autonomous mode or go to home position.
'
'=========================================================================
' Servo_Range_Constraint
'=========================================================================
' sub() return array()<byte>
'
' - array(pscChannel) Array that holds servo positions.
'
' Since the servos used in the arm have a max rotation of 180 degrees,
' this sub routine verifies that all array() values fall between 0 and
' 180 degrees.
'
'=========================================================================
' Display
'=========================================================================
' sub(<all variables>)
'
' This sub displays all the variables and values used in this code to
' the debug screen. If
'
' #DEFINE debugMode = 1
'
' This is a handy troubleshooting technique.
'
'=========================================================================
' Troubleshooting
'=========================================================================
' Find the code section below.
'---------------------------
' #DEFINE debugMode = 1
'---------------------------
'
' Set debugMode = 1 to display all program variable on the debug screen
' for troubleshooting.  Default is debugMode = 1.
'
' Use the debug window to verify that your connections are working
' properly.
'
'=========================================================================
' Main
'=========================================================================
' This section of code is used to give the programmer a way to
' interact with the code.
'
'
'-------------------------------------------------------------------------

'-----[ I/O ]-------------------------------------------------------------
#IF ($stamp = BS2SX) OR ($stamp = BS2P) #THEN
  Baud              CON     1021            ' BS2p 2400 baud
#ELSE
  Baud              CON     33164           ' BS2 2400 baud
#ENDIF
PSC                 CON     15              ' PSC Module
degrees             CON     5
Base                CON     0               ' Rotating base
Bicep               CON     1               ' Left bicept
Elbow               CON     2               ' Elbow
Wrist               CON     3               ' Wrist
Gripper             CON     4               ' Gripper
RightBicep          CON     5               ' Right bicept

'=================================================================
' Constant declaration statement found from
' running Adjust_Bicept.bsp during assembly.
' This code will not download until you declare
' the RightBicepOffset constant.
' Chapter #5: Adjusting the Biceps Servos.
RightBicepOffset    CON     -10             ' Right Bicep offset
'=================================================================

RC_On               CON     1               ' Receiver ON
RC_Off              CON     0               ' Receiver OFF

'---- [ Variables ] ------------------------------------------------------
channelValue        VAR     Word            ' Rx channel value
array               VAR     Byte(6)         ' Servo positions (degrees)
pscChannel          VAR     Nib             ' PSC channel
RxChannels          VAR     Nib             ' Receiver channel
RC                  VAR     Bit             ' Rx On/Off

ctrlByte            VAR     Byte            ' Servo joint(s) updated
baseBit             VAR     ctrlByte.BIT0   ' 0 = no change
biceptBit           VAR     ctrlByte.BIT1   ' 1 = joint value changed
elbowBit            VAR     ctrlByte.BIT2
wristBit            VAR     ctrlByte.BIT3
gripperBit          VAR     ctrlByte.BIT5

'=========================================================================
' debugMode = 1 --> Display variables (debug mode on)
' debugMode = 0 --> debug mode off
#DEFINE debugMode = 1
'=========================================================================

  array(Base) = 90
  array(Bicep) = 90
  array(Elbow) = 90
  array(Wrist) = 90
  array(4) = 0
  array(Gripper) = 135

Main:
  ctrlByte = %00000000
  GOSUB Receiver_Power_Check
  'RC = 1                                ' Set RC mode on all the time
  IF RC THEN                            ' Check for Rx on/off
    GOSUB Get_Receiver_Channel_Values   ' Get receiver channel pulse width
  ELSE
    #IF debugMode #THEN
      DEBUG CLS
      GOSUB Display
    #ENDIF
  ENDIF
  GOTO Main                             ' loop forever
END

'sub() return ctrlByte<byte>
Get_Receiver_Channel_Values:
' Loop through pins 10 to 14 (RxChannels)
  FOR RxChannels = 10 TO 14
  ' Read Rx value
    PULSIN RxChannels, 1, channelValue
    ' Do nothing if stick positions are centered or near center
    IF (((channelValue > 2200) AND (channelValue < 2800)) OR ((channelValue > 400) AND (channelValue < 1800))) THEN
      SELECT RxChannels
        CASE 10
          pscChannel = Elbow                ' Array element to change
          elbowBit = 1                      ' Joint affected
        CASE 11
          pscChannel = Wrist
          wristBit = 1
        CASE 12
          pscChannel = Bicep
          biceptBit = 1
        CASE 13
          pscChannel = Gripper
          gripperBit = 1
        CASE 14
          pscChannel = Base
          baseBit = 1
      ENDSELECT
      GOSUB Incr_Decr_Servo_Position_Array
    ENDIF
  #IF debugMode #THEN
    DEBUG CLS
    GOSUB Display
  #ENDIF
  NEXT
  GOSUB Servo_Range_Constraint
RETURN

'-------------------------------------------------------------------------
'sub(pscChannel<nib> channelValue<word>) return array(pscChannel)
Incr_Decr_Servo_Position_Array:
  SELECT channelValue
    CASE > 2200
      array(pscChannel) = array(pscChannel) + degrees
    CASE < 1800
      array(pscChannel) = array(pscChannel) - degrees
  ENDSELECT
'-------------------------------------------------------------------------
' This section updates elbow position if bicept was just updated
' Elbow always moves in the opposite direction as the bicept.
' The statements below can be removed if you do not want both
' the bicept and elbow to move at the same time. The elbow is
' also controlled by the trim channel
  IF pscChannel = bicep THEN
    pscChannel = Elbow
    SELECT channelValue
      CASE > 2200
        array(pscChannel) = array(pscChannel) - degrees
      CASE < 1800
        array(pscChannel) = array(pscChannel) + degrees
    ENDSELECT
  ENDIF
'-------------------------------------------------------------------------
RETURN

'sub() return RC<bit>
Receiver_Power_Check:
  PULSIN 10, 1, channelValue
  IF channelValue = 0 THEN
    RC = RC_Off
  ELSE
    RC = RC_On
  ENDIF
RETURN

'sub() array(pscChannel)<byte>
Servo_Range_Constraint:
' This section enforces that the
' array(x) position values are between
' 0 and 180.
  FOR pscChannel = 0 TO 5
    IF (array(pscChannel) > (180 + degrees)) THEN
      array(pscChannel) = 0
    ENDIF
    IF (array(pscChannel) = (180 + degrees)) THEN
      array(pscChannel) = 180
    ENDIF
  NEXT
RETURN

' Display all variables
#IF debugMode #THEN
  Display:
    DEBUG "-----------------------",CR
    IF RC THEN
      DEBUG "Receiver is ON!",CR
    ELSE
      DEBUG "Receiver is OFF!",CR
    ENDIF
    DEBUG  ?RxChannels,
           ?channelValue,
           DEC ?pscChannel,
           ?array(Base),
           ?array(Bicep),
           ?array(Elbow),
           ?array(Wrist),
           '?array(4), Empty for 6 axis arm
           ?array(Gripper),
           BIN ?ctrlByte
    PAUSE 1000
  RETURN
#ENDIF