'  File:   TRP3B.BAS
'  Author: Tom Dickens
'  Date:   May 30, 1995
'  This is an SBASIC version of the programs in TRP3.ASM
'  The detailed hardware comments are in the TRP3.ASM file.

include  "regs11.lib"
'  Numeric constants
const DIRECTION_DELAY        =  $08
const LIGHT_SEEKING_DELAY    =  $18
'  Variables
declare number
declare delay_time

main:
   gosub     Init_Servos     ' Initialize the servo-motors
   '  Gosub to the desired program.  Put the program you
   '  want to run as the first gosub below.  The second gosub
   '  will not be used.
   gosub     Light_Seeking_Pgm
   gosub     Line_Following_Pgm
end

'  ============================================== 
'  Line Following Program Logic
Line_Following_Pgm:
   '  Configure port C, bits 0, 2, 4 as input, bits 1, 3, 5 as output.
   '    With the DDRC register, 0=input, 1=output.
   pokeb ddrc, %00101010
   
   delay_time = DIRECTION_DELAY
   do
      if peekb( porta ) and 5 <> 0 ' Check the bumper switches
         gosub Calibrate
      endif

      gosub Set_Oputput_LEDs
      '  Read the current line-follower sensor
      number = peekb( portc ) and %00010101
      '  If all bits set, all detectors see white
      if number <> %00010101
         if number and 1 = 0
            gosub Arc_Left
            gosub DelayA
         else
            if number and 16 = 0
               gosub Arc_Right
               gosub DelayA
            else
               if number and 4 = 0
                  gosub Set_Forward
               endif
            endif
         endif
      endif
   loop
return

Calibrate:
   interrupts off
   pokeb tctl1, %10100000 'set timers 2 & 3 low at compare equal
   pokeb cforc, $60 'force timer 2 & 3 compare action
   pokeb tctl1, 0 'disable timers 2 & 3
   do
      gosub Set_Oputput_LEDs
   loop    ' loop forever
return

Set_Oputput_LEDs:
   number = (peekb(portc ) xor $ff)
   pokeb portc, number + number
return

'  ============================================== 
'  Light Seeking Program Logic
Light_Seeking_Pgm:
   pokeb option, %10010000  ' Power up A/D with clock delay
   do
      '   Read A2D for bits 0 to 3.
      pokeb adctl, %00010000  ' single scan, multi-mode, pins e0-3
      do while peekb( adctl ) and $80 = 0
      loop  ' Wait for A/D to finish
   
      if peekb( adr1 ) <* peekb( adr2 )
         gosub Arc_Left
      else
         gosub Arc_Right
      endif
      delay_time = LIGHT_SEEKING_DELAY
      gosub DelayA
      gosub Set_Forward
      gosub DelayA
   loop   ' Loop Forever
return

'  Delay based on the value in delay_time
DelayA:
   declare i
   declare j
   for i=0 to* delay_time
      for j=1 to 1200
      next
   next
return

'  ============================================== 
'  The robot device-driver code is from here on down...
'  ============================================== 
'     Numeric constants for the robot driver:
const SERVO_RIGHT_OFF         =     $0C40
const SERVO_LEFT_OFF          =     $0C40
const SERVO_RIGHT_FORWARD     =     $07D0
const SERVO_LEFT_FORWARD      =     $0FA0

Set_Forward:
   poke toc2, SERVO_RIGHT_FORWARD
   poke toc3, SERVO_LEFT_FORWARD
return

Arc_Right:
   poke toc2, SERVO_RIGHT_OFF
   poke toc3, SERVO_LEFT_FORWARD
return

Arc_Left:
   poke toc2, SERVO_RIGHT_FORWARD
   poke toc3, SERVO_LEFT_OFF
return

Init_Servos:
   pokeb tctl1, $50
   pokeb oc1m,  $60
   pokeb oc1d,  $60
   gosub Set_Forward
return
' end-of-file