' line follow program with two CdS cell sensors

' author: kerwin lumpkins
' this file calls an init_adc sub that initializes the adc module
' then it runs a loop that gets the adc value on channel 7 and 6

' it then compares these values to a threshold level and if it
' is greater then that sensor is on black.  if not then on white.
' the two are combined into a single variable called cells which
' is a 2 bit value where bit 1 is the left sensor black (1) or
' white (0) value and bit 0 is the right sensor.  this single variable
' called cells is then used to decide how to drive the motors

' put in the cal value for your part here (marked on the 8535 case
' most are hex 9F, some 91's and some in between
Osccal = &H9F
$regfile = "m8535.dat"

' items for the ADC module
Declare Sub Init_adc
Declare Function Get_darc_adc(byval Channel As Byte) As Integer
Dim Left_value As Integer , Right_value As Integer
Dim Left_cell As Byte , Right_cell As Byte
Dim Cells As Byte
Dim Channel As Byte

' items for the servo motors
Declare Sub Servos12_init
Declare Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)
Declare Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)

Const Black = 1
Const White = 0
Const Threshold_left = 750
Const Threshold_right = 750


Declare Sub Forward()
Declare Sub Right_turn()
Declare Sub Left_turn()
Declare Sub Stop_servos()
Declare Sub Lost()


Main:

Call Init_adc

' run in continuous loop
Do
 Wait 1
 Left_value = Get_darc_adc(7)                               'get adc value on the sensor
 Print ""
 Print ""
 Print " left value " ; Left_value                          'print to serial port
 Print " "                                                  'print blank line

 Right_value = Get_darc_adc(6)                              'get adc value on the sensor
 Print " right value " ; Right_value                        'print to serial port
 Print " "                                                  'print blank line


  ' this might be backwards
 If Left_value > Threshold_left Then Left_cell = Black Else Left_cell = White
 If Right_value > Threshold_right Then Right_cell = Black Else Right_cell = White

 Shift Left_cell , Left , 1                                 ' take the one bit value of left_cell and shift it
      ' to the left one place

 Cells = Left_cell Or Right_cell                            ' combine the cell values into a single 2 bit value
 Print "cells value is " ; Cells
 Print ""
 Select Case Cells
    Case 0 : Call Lost
    Case 1 : Call Right_turn
    Case 2 : Call Left_turn
    Case 3 : Call Forward
    Case Else : Call Stop_servos
  End Select

Loop

' subroutine to initialize the adc module
Sub Init_adc
   ' set for AVCC reference voltage,Right justified,channel 0
   Admux = &H40

   'set ad status and control register for conversion disabled, conversion
   'not started,auto trigger disabled,interrupts disabled,and divide
   'by 64 prescaler
   Adcsra = &H06

End Sub Init_adc

' function to get adc value and return it
Function Get_darc_adc(byval Channel As Byte) As Integer
   Dim Flag As Byte
   Dim Adcnotdone As Bit
   'init the admux for channel 0
   Admux = Admux And &HE0
   Admux = Admux + Channel

   'enable the adc
   Adcsra = &H86
   'start the conversion
   Adcsra = &HC6

   Adcnotdone = 1

   While Adcnotdone = 1
     Flag = Adcsra And &H40

     'wait until the adc interrupt flag is set
     If Flag = 0 Then Adcnotdone = 0
   Wend

   Dim Adcvalh As Integer
   Dim Adcvall As Integer
   Dim Adcval As Integer
   Adcvalh = 0
   Adcvall = 0
   Adcval = 0

   'get the 2 adc hi and lo bytes and combine into one integer value
   Adcvall = Adcl
   Adcvalh = Adch
   Shift Adcvalh , Left , 8
   Adcval = Adcvalh + Adcvall
   'Print "adcvalh:" ; Adcvalh ; " adcvall: " ; Adcvall ; "   adcval = " ; Adcval


   'disable conversion
   Adcsra = &H06

   Get_darc_adc = Adcval

End Function Get_darc_adc(byval Channel As Byte)

Sub Forward()
  ' up to you guys
  Print "in forward sub"
  Print ""
End Sub

Sub Right_turn()
  ' up to you guys
  Print "in right turn sub"
  Print ""
End Sub

Sub Left_turn()
  ' up to you guys
  Print "in left turn sub"
  Print ""
End Sub

Sub Lost()
  ' up to you guys
  Print "in lost sub"
  Print ""
End Sub

Sub Stop_servos()
  ' up to you guys
  Print "servos stopped"
  Print ""
End Sub


Sub Servos12_init:

' set bits 5 and 4 for output
Ddrd = Ddrd Or &H30

' set timer 1 for pwm output, prescaler 64
' these settings are for 1 MHz clock
Tccr1a = &HA3
Tccr1b = &H0C

'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

' Here the servos are init'd to be at an "OFF" setting
' hex 03FF gives a 100 % duty cycle pulse.
Ocr1ah = &H03
Ocr1al = &HFF

Ocr1bh = &H03
Ocr1bl = &HFF

End Sub Servos12_init


Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)
'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

   Ocr1bh = Hset
   Ocr1bl = Lset
End Sub Servo1_set(byval Hset As Byte , Byval Lset As Byte)


Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)
'OCR1 is a 16 bit register that sets the on time or duty
'cycle for servos 1 and 2. The setting is a 16 bit value that
'consists of the Hset being the hi byte, and LSet the low.
'The servos12 device is a 10 bit resolution PWM output so only
'the low 2 bits of Hset are valid.  The Highest value for Hset = hex 3.
'Pulse width = (setting + 1) * 35 us.  So a setting of
'0 will give a 35 us pulse.  Setting = 1 gives a 70 us
'pulse.  A setting of 46 will give about 1.5 ms pulse.
'Hset = &H03 and Lset = &HFF is the OFF setting.
' Note that OCR1A sets servo 2 and B sets servo1

   Ocr1ah = Hset
   Ocr1al = Lset
End Sub Servo2_set(byval Hset As Byte , Byval Lset As Byte)