' =========================================================================
'
'   File....... wc-bot (wheels and chain robot)
'   Purpose.... Mine Sweeper Contest
'   Author..... Daniel and Zachary McGlothin
'   Started.... 26-JUN-2005
'
'   {$STAMP BS2}
'   {$PBASIC 2.5}
'
' =========================================================================


' -----[ Program Description ]---------------------------------------------
' Run a pattered behavior to attempt to cover the
' Mine Field for the PennBot Mine Contest.
'
' The pattern is to run forward about 4 feet,
' shift left about 1 foot while moving backwards,
' and repeat until 4 forward passes have occured.
' Next turn 90 degrees right.
' Then repeat the sweeping algorithm
' shifting to the right each time.
' Then turn 90 degrees left.
' Finally, repeat from the beginning.
'
' -----[ Revision History ]------------------------------------------------


' -----[ I/O Definitions ]-------------------------------------------------
LeftWheel    PIN     15
RightWheel   PIN     11

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

#SELECT $STAMP
  #CASE BS2, BS2E, BS2PE
    T1200       CON     813
    T2400       CON     396
    T4800       CON     188
    T9600       CON     84
    T19K2       CON     32
    TMidi       CON     12
    T38K4       CON     6
  #CASE BS2SX, BS2P
    T1200       CON     2063
    T2400       CON     1021
    T4800       CON     500
    T9600       CON     240
    T19K2       CON     110
    TMidi       CON     60
    T38K4       CON     45
  #CASE BS2PX
    T1200       CON     3313
    T2400       CON     1646
    T4800       CON     813
    T9600       CON     396
    T19K2       CON     188
    TMidi       CON     108
    T38K4       CON     84
#ENDSELECT

SevenBit        CON     $2000
Inverted        CON     $4000
Open            CON     $8000

Baud            CON     T9600


' -----[ Variables ]-------------------------------------------------------
i         VAR      Byte
speed     VAR      Byte
vear      VAR      Byte

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


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

Reset:
  PAUSE 2500

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

Main:
  DO
    GOSUB FirstSweep
    GOSUB TurnRight90
    GOSUB SecondSweep
    GOSUB TurnLeft90
  LOOP
  END

' -----[ Subroutines ]-----------------------------------------------------
FirstSweep:
    'pass 1
    GOSUB Forward
    GOSUB ShiftLeft
    GOSUB BackTrack
    'pass 2
    GOSUB Forward
    GOSUB ShiftLeft
    GOSUB BackTrack
    'pass 3
    GOSUB Forward
    GOSUB ShiftLeft
    GOSUB BackTrack
    'pass 4
    GOSUB Forward
  RETURN


SecondSweep:
    'pass 1
    GOSUB Forward
    GOSUB ShiftRight
    GOSUB BackTrack
    'pass 2
    GOSUB Forward
    GOSUB ShiftRight
    GOSUB BackTrack
    'pass 3
    GOSUB Forward
    GOSUB ShiftRight
    GOSUB BackTrack
    'pass 4
    GOSUB Forward
  RETURN


TurnLeft90:
  FOR i = 1 TO 30
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  FOR i = 1 TO 45
    PULSOUT LeftWheel,  750
    PULSOUT RightWheel, 750-speed
    PAUSE 20
  NEXT
  FOR i = 1 TO 30
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  RETURN

TurnRight90:
  FOR i = 1 TO 30
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  FOR i = 1 TO 75
    PULSOUT LeftWheel,  750+speed
    PULSOUT RightWheel, 750
    PAUSE 20
  NEXT
  FOR i = 1 TO 30
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  RETURN

Forward:
  speed = 100
  '25 iterations is about 9" on a battery > 8.5 volts
  FOR i = 1 TO 125
    PULSOUT LeftWheel,  750+speed
    PULSOUT RightWheel, 750-speed
    PAUSE 20
  NEXT
  RETURN

ShiftLeft:      'done in reverse
  speed = 100
  vear  = speed / 33
  FOR i = 1 TO 50
    PULSOUT LeftWheel,  750-vear
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  FOR i = 1 TO 20
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  FOR i = 1 TO 20
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+vear
    PAUSE 20
  NEXT
  RETURN

ShiftRight:      'done in reverse
  speed = 100
  vear  = speed / 33
  FOR i = 1 TO 20
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+vear
    PAUSE 20
  NEXT
  FOR i = 1 TO 20
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  FOR i = 1 TO 30
    PULSOUT LeftWheel,  750-vear
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  RETURN

BackTrack:
  speed = 100
  FOR i = 1 TO 105
    PULSOUT LeftWheel,  750-speed
    PULSOUT RightWheel, 750+speed
    PAUSE 20
  NEXT
  RETURN

'     --------------------------     '
'    /                          \    '
' --<   End of Program Listing   >-- '
'    \                          /    '
'     --------------------------     '