' TANK.BS1
' Drive the tank around.
' {$STAMP BS1}
' {$PBASIC 1.0}

' Pin Definitions
' LM293 Motor Controller from HVW Technology
SYMBOL Motor0Fwd = 0
SYMBOL Motor0Rev = 1
SYMBOL Motor1Fwd = 2
SYMBOL Motor1Rev = 3
' The Ping))) Ultrasonic Transducer from Parallax
SYMBOL Ping = 7

' LM293 Constants
SYMBOL FwdBits   = %1010
SYMBOL RevBits   = %0101
SYMBOL RightBits = %1001
SYMBOL LeftBits  = %0110
SYMBOL StopBits  = %1111

' Ping Constants
SYMBOL Trigger = 1  ' 10 uS trigger pulse
SYMBOL Scale = 10   ' raw x 10.00 = uS
SYMBOL RawToIn = 889  ' 1 / 73.746 (with **)
SYMBOL RawToCm = 2257 ' 1 / 29.034 (with **)
SYMBOL IsHigh = 1     ' for PULSOUT
SYMBOL IsLow  = 0

' Variables
SYMBOL rawDist = W1   ' raw measurement
SYMBOL inches = W2
SYMBOL cm = W3
SYMBOL i = B8
SYMBOL randNumber = W5

OUTPUT Motor0Fwd
OUTPUT Motor0Rev
OUTPUT Motor1Fwd
OUTPUT Motor1Rev
GOSUB All_Stop  ' Make sure we initialize stopped

DEBUG "Tank v1.0",CR,"(c)2006 Eric Lundquist",CR
GOSUB Get_Sonar
randNumber = rawDist  'set random number seed

Main:
  GOSUB Get_Sonar             ' Get sensor value
  IF inches > 36 THEN Full_Speed_Ahead
  IF inches < 6 THEN Back_Off
  RANDOM randNumber
  IF randNumber > 20000 THEN Kick_Right
Kick_Left:
  GOSUB Turn_Left
  FOR i = 0 TO 1000
  GOSUB Get_Sonar
  IF inches > 36 THEN Main
  NEXT i
Kick_Right:
  GOSUB Turn_Right
  FOR i = 0 TO 1000
  GOSUB Get_Sonar
  IF inches > 36 THEN Main
  NEXT i
  FOR i = 0 TO 1000
  GOSUB Back_Off
  GOSUB Get_Sonar
  IF inches > 6 THEN Main
  NEXT i
Full_Speed_Ahead:
  GOSUB Go_Fwd
  GOTO Main
Back_Off:
  GOSUB Go_Rev
  GOTO Main

Get_Sonar:
  LOW Ping                  'make trigger 0-1-0
  PULSOUT Ping, Trigger     'activate sensor
  PULSIN Ping, IsHigh, rawDist  ' measure echo pulse
  IF rawDist > 0 THEN Convert_Ping
  rawDist = 65535  ' set max distance
  inches = 65535
  cm = 65535
  RETURN
Convert_Ping:
  rawDist = rawDist * Scale 'convert to uS
  rawDist = rawDist / 2     'remove return trip
  inches = rawDist ** RawToIn ' convert to inches
  cm = rawDist ** RawToCm     ' convert to cm
  RETURN

All_Stop:
  PINS = StopBits
  RETURN

Go_Fwd:
  PINS = FwdBits
  RETURN

Go_Rev:
   PINS = RevBits
   RETURN

Turn_Right:
   PINS = RightBits
   RETURN

Turn_Left:
   PINS = LeftBits
   RETURN

   END