'Kronos Crawler IR control program
'Use the CH+ and CH- for forward and reverse movement
'Use the Vol keys for the left and right movement
'Use the Key pad to set the speed 2-9
'Sonar device is connected to ports 0 and 1
'IR is connected to port 0
func main()
   print "reset"
   pause 300 ' Give CoProc time to resync
   dim cmd
   dim lcmd,range
   clear

   CPinit()
   CPMotorReset(0)

   CPMotor1speed(0,200)
   CPMotor2speed(0,200)
   pause 100

   CPMotorDamperoff(0) 'Turn off damper so we can slam off breaks.

loop: 
   cmd = IRreadverify(0,1,10000)

   if cmd = 0 then
     CPMotorBotstop(0)
     goto loop
   endif

   'Test the Sonar here
   if lcmd = 17 or cmd = 17 then
     sonar 1,2,range 
     range = range / 64
     if range <16 then
         CPMotorBotrev(0)
         pause 20 'Go in revers for short period
         CPMotorBotstop(0)
         lcmd = 0
      goto loop
      endif
  endif 

'Forward
   if cmd = 17 then
      CPMotorBotfwd(0)
      lcmd = 17
      goto loop
    endif

'Reverse
    if cmd = 18 then 
      CPMotorBotrev(0)
     goto loop
    endif

'Left
   if cmd = 20 then
      CPMotorBotleft(0)
     goto loop
   endif

'Right
   if cmd = 19 then
     CPMotorBotright(0)
     goto loop
   endif

   goto loop

endfunc

include \lib\DiosCoproc.lib
include \lib\DiosCPMotor.lib
include \lib\DiosIR.lib