' =========================================================================
'
'   File...... Advanced_Tank_Bot.BS2
'   Purpose... Tank Bot with Tilt Sensing
'   Author.... Parallax
'   E-mail.... support@parallax.com
'   Started...
'   Updated... 12 APRIL 2004
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
'
' This program incorporates the Memsic 2125 accelerometer into the BOE-Bot
' tank kit to allow the tank to exlore uneven terrain without tipping over.


' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------

LfIrOut         PIN     8                       ' left IR LED output
LfIrIn          PIN     9                       ' left IR sensor input
RtIrOut         PIN     1                       ' right IR LED output
RtIrIn          PIN     0                       ' right IR sensor input

LMotor          PIN     13                      ' left servo motor
RMotor          PIN     12                      ' right servo motor

TiltX           PIN     4                       ' X tilt input
TiltY           PIN     5                       ' Y tilt input


' -----[ Constants ]-------------------------------------------------------

LFwd            CON     1000                    ' left motor forward
LStop           CON      750                    ' left motor stop
LRev            CON      500                    ' left motor reverse

RFwd            CON      500                    ' right motor forward
RStop           CON      750                    ' right motor stop
RRev            CON     1000                    ' right motor reverse

IsHigh          CON     1                       ' high-going pulse
IsLow           CON     0                       ' low-going pulse

YThresh         CON     20                      ' Y tilt threshold


' -----[ Variables ]-------------------------------------------------------

pulses          VAR     Byte                    ' counter for servo control
irBits          VAR     Nib                     ' ir detection bits
irLeft          VAR     irBits.BIT1             ' left IR detection
irRight         VAR     irBits.BIT0             ' right IR detection
xAxis           VAR     Word                    ' x-axis tilt reading
yAxis           VAR     Word                    ' y-axis tilt reading


' -----[ EEPROM Data ]-----------------------------------------------------


' -----[ Initialization ]--------------------------------------------------

Reset:
  LOW LMotor                                    ' initialize motor outputs
  LOW RMotor


' -----[ Program Code ]----------------------------------------------------

Main:
  DO
    GOSUB Check_Level                           ' verify terrain okay

    ' navigate around obstacles

    GOSUB Read_IR_Sensors
    ON irBits GOSUB Forward, Go_Left, Go_Right, U_Turn
  LOOP

  END


' -----[ Subroutines ]-----------------------------------------------------

Check_Level:
  PULSIN TiltY, IsHigh, yAxis                   ' check longitudinal tilt
  yAxis = yAxis / 100                           ' filter reading
  IF (yAxis =< YThresh) THEN                    ' if terrain too steep
    GOSUB U_Turn                                ' ... and turn around
  ENDIF
  RETURN


Read_IR_Sensors:
  irBits = %00
  FREQOUT LfIrOut, 1, 38500                     ' modulate left IR LED
  irLeft = ~LfIrIn                              ' read input (1 = target)
  FREQOUT RtIrOut, 1, 38500                     ' modulate right IR LED
  irRight = ~RtIrIn                             ' read input (1 = target)
  RETURN


Forward:                                        ' move tank forward
  FOR pulses = 1 TO 5
    PULSOUT LMotor, LFwd
    PULSOUT RMotor, RFwd
    PAUSE 20
  NEXT
  RETURN


Go_Left:                                        ' turn to left
  FOR pulses = 1 TO 15
    PULSOUT LMotor, LStop
    PULSOUT RMotor, RFwd
    PAUSE 20
  NEXT
  RETURN


Go_Right:                                       ' turn to right
  FOR pulses = 1 TO 15
    PULSOUT LMotor, LFwd
    PULSOUT RMotor, RStop
    PAUSE 20
  NEXT
  RETURN


U_Turn:                                         ' turn around
  IF (yAxis > YThresh) THEN
    GOSUB Back_Up                               ' back-up if flat terrain
  ENDIF
  PULSIN TiltX, IsHigh, xAxis                   ' check longitudinal tilt
  xAxis = xAxis / 100                           ' filter reading
  IF (xAxis < 24) THEN                          ' turn left
    FOR pulses = 1 TO 60
      PULSOUT LMotor, LRev
      PULSOUT RMotor, RFwd
      PAUSE 20
    NEXT
  ELSE                                          ' turn right
    FOR pulses = 1 TO 60
      PULSOUT LMotor, LFwd
      PULSOUT RMotor, RRev
      PAUSE 20
    NEXT
  ENDIF
  RETURN


Back_Up:                                        ' back up a bit
  FOR pulses = 1 TO 10
    PULSOUT LMotor, LRev
    PULSOUT RMotor, RRev
    PAUSE 20
  NEXT
  RETURN