'mini bot
 'Athena servo sumo bot
'Several of the bot movement routines are not called and could
' be removed

   dim cmd,device,Lservo,Rservo,range,cyclecount,state
   
   'Edge sensor LED
   output 2
   high 2

   const Lservoport 4
   const Rservoport 5

   'Wait 5.25 seconds before we start
   longpause 250,21


'----------------------------------------------------
'Search
'This will have the bot spin until it sees an object
'It uses a Sharp GP2D120 with the miniad command.
'----------------------------------------------------
dosearch:
  gosub right
  cyclecount = 0
searchloop:

   if cyclecount > 200 then
    gosub fwd
   endif

   'Check edge here
   if inp10 = 0 then
     gosub edgedetect
     goto dosearch
   endif

   gosub procservos
   miniad 0,range
   
   if range > 1 then 
     goto dodestroy
   endif
  pause 15

  cyclecount = cyclecount + 1
  goto searchloop


'----------------------------------------------------
'destroy
'Once the search routine spots an object this routine
'will move the bot forward.  If enemy detection is lost
'the search routine will be executed  
'----------------------------------------------------
dodestroy:
  gosub fwd
  cyclecount = 0
destroyloop:
  
   'Check edge here
   if inp10 = 0 then
     gosub edgedetect
     goto dosearch
   endif

   gosub procservos
   miniad 0,range
   
   'here we will wait 20 cycles before checking range
   if cyclecount > 20 then
    if range = 1 then
     goto dosearch
    endif
   endif

   pause 15

   cyclecount = cyclecount + 1
   goto destroyloop   

'---------------------------------------------------
'Edge detect
'---------------------------------------------------
edgedetect:
  state = 0
  gosub procstate
edgedetectloop:
  
  gosub procservos
  pause 15
  cyclecount = cyclecount - 1
  if cyclecount = 0 then
    gosub procstate
    if state = 3 then 
      return
    endif
  endif

  goto edgedetectloop

procstate:
  branch state,dostate0,dostate1,dostate2
  
statecont:
  state = state + 1
  return

dostate0:
    gosub bkwd
    cyclecount = 25
    goto statecont

dostate1:
    gosub left
    cyclecount = 35
    goto statecont

dostate2:
    gosub fwd
    cyclecount = 200
    goto statecont

'-----------------------------------------------------
'Bot servo routines
'-----------------------------------------------------
procservos:
   servo Lservoport,Lservo
   servo Rservoport,Rservo

   return

fwd:
   gosub servoson
   Lservo = 200
   Rservo = 100
   return

bkwd:
   gosub servoson
   Lservo = 100
   Rservo = 200
   return

right:
   gosub servoson
   Lservo = 200
   Rservo = 200
   return

left:
   gosub servoson
   Lservo = 100
   Rservo = 100
   return


stop:
   gosub servosoff
   Lservo = 150
   Rservo = 150
   return

servoson:
   output Lservoport
   output Rservoport
   return

servosoff:
   input Lservoport
   input Rservoport
   return