'Sumo robot version 4.1 Using PWM speed control and only one edge sensor

oldsearchres  var  word   'decision from irod for location of the opponent
newsearchres  var  word   'new decision from irod used for remembering where the opponent was last
threshold     var  word   'Edge detector base level
lcds          var  word   'Variable to detect the edge of the ring 


slow          con  50     'speed constants
med           con  80     'speed constants
fast          con  125    'speed constants

backupdur     con  300    'how long to backup during edge escape
turndur       con  300    'how long to turn during edge escape
rundur        con  100    'how long to wait before doing something else

lmot          con   14    'left motor address
rmot          con   15    'right motor address
forward       con  129    'forward direction constant
backup        con  130    'reverse direction constant
spdflg        var  byte   'adjusts speed when target is in sight

N9600   con   84          'serial communication constant for speed control

newsearchres = 0          'initial setup for search constants
oldsearchres = 0          'initial setup for search constants

'Calibrate edge sensor
high 0
pause 1
rctime 0,1,lcds
threshold = lcds/2

pause 5200

'Initial scan for target
firstscan:
oldsearchres = (in11 * 1) + (in12 * 2)
branch oldsearchres,[slowforward,slowrightpiv,slowleftpiv,medforward]

spdflgrset:
spdflg = -1

'Checking for edge of ring
checkbar:
high 0
pause 1
rctime 0,1,lcds
if lcds < threshold then escape
goto newsrch

'New search for target
newsrch:
newsearchres = (in11 * 1) + (in12 * 2)

branch newsearchres,[trkbrnch,slowrightpiv,slowleftpiv,spdsel]

spdsel:
spdflg = spdflg + 1
if spdflg > 2 then fix
branch spdflg,[slowforward,medforward,fastforward]

'Turns toward direction of last sighting of target
trkbrnch:
branch oldsearchres,[slowforward,medrightpiv,medleftpiv,medforward]

fix:
spdflg = 2
goto fastforward

'Goes forward slowly searching for target
slowforward:
oldsearchres = newsearchres
   serout lmot,N9600,[forward]
   serout lmot,N9600,[slow]
   serout rmot,N9600,[forward]
   serout rmot,N9600,[slow]
 pause rundur
goto checkbar

'Goes forward fast toward target
fastforward:
oldsearchres = newsearchres
   serout lmot,N9600,[forward]
   serout lmot,N9600,[fast]
   serout rmot,N9600,[forward]
   serout rmot,N9600,[fast]
 pause rundur
goto checkbar

medforward:
oldsearchres = newsearchres
   serout lmot,N9600,[forward]
   serout lmot,N9600,[med]
   serout rmot,N9600,[forward]
   serout rmot,N9600,[med]
 pause rundur
goto checkbar

'Does a slowright pivot
slowrightpiv:
oldsearchres = newsearchres
   serout lmot,N9600,[forward]
   serout lmot,N9600,[slow]
   serout rmot,N9600,[backup]
   serout rmot,N9600,[slow]
 pause rundur
goto checkbar

'Does a med right pivot
medrightpiv:
oldsearchres = newsearchres
   serout lmot,N9600,[forward]
   serout lmot,N9600,[med]
   serout rmot,N9600,[backup]
   serout rmot,N9600,[med]
 pause rundur
goto spdflgrset

'Does a slowleft pivot
slowleftpiv:
oldsearchres = newsearchres
   serout lmot,N9600,[backup]
   serout lmot,N9600,[slow]
   serout rmot,N9600,[forward]
   serout rmot,N9600,[slow]
 pause rundur
goto checkbar

'Does a med left pivot
medleftpiv:
oldsearchres = newsearchres
   serout lmot,N9600,[backup]
   serout lmot,N9600,[med]
   serout rmot,N9600,[forward]
   serout rmot,N9600,[med]
 pause rundur
goto spdflgrset

'Escape move
escape:
oldsearchres = newsearchres
   serout lmot,N9600,[backup]
   serout lmot,N9600,[med]
   serout rmot,N9600,[backup]
   serout rmot,N9600,[med]

pause backupdur

   serout lmot,N9600,[forward]
   serout lmot,N9600,[med]
   serout rmot,N9600,[backup]
   serout rmot,N9600,[med]

pause turndur
goto slowforward