include  "regs11.lib"
declare i
declare  n
declare  t
declare  x
declare z
declare y
declare n2
declare Fob
declare FobAdjust
declare Tmp
' Ground sensor values
declare LeftCenterG
declare RightG
declare LeftG
declare RightCenterG
declare TimeIt
declare wait
declare RollIt
declare Number2
declare TimeCount
declare TimeCountH
declare iTmp
declare Analog(8)
declare SenseMax(8)
declare SenseMin(8)
declare SenseAverage(8)
declare SenseTotalL(8)
declare SenseTotalH(8)
declare SenseThreshold(8)
declare SenseDiff(8)
const LeftStop = $9AA
const LeftFF   = $2000
const LeftFR   = $200
const RightStop =$9EB
const RightFF   =$200
const RightFR   =$2000
const LeftSpeed = TOC2
const RightSpeed = TOC3
const L = 2
const LC = 6
const RC = 3
const R = 7
wait = 0
TimeIt = 0
RollIt = 0
y = 0
n = 14
z = 1
n2 = 0
Number2 = 50
'
'  The RTI interrupt service routine
'
interrupt $fff0 
'RTI Interrupt
  if peekb(tflg2) and %01000000 <> 0
    if wait <> 0
      wait = wait - 1
    endif
    TimeIt = TimeIt + 1
    pokeb  tflg2, %01000000
  endif
' keep track of PWM timer roll over
  if peekb(tflg2) and %10000000 <> 0
    RollIt = RollIt + 1
    pokeb  tflg2, %10000000
  endif
' I don't think this is used at the moment
' but in case it is I'll leave it in for the Encoder article
  if peekb(tflg1) and %01000000 <> 0
    Number2 = Number2 + 1
    pokeb  tflg1, %01000000
  endif
end
'This subroutine blinks an LED attached to any of the 
'output lines on port c. It will be removed at some time in the future
'currently it's never called. To call it simply add the line
'call Alive 
Alive:
y = 0
for n2 = 0 to 10
    for n = 0 to 1
        for x = 0 to 6300
        next 
    next
 
    ' Alternate between all on and all off
    if y = 0    
 y = $ff
    else
 y = 0
    endif
    pokeb portc, y 'Change port C to either all low or all high
next
pokeb portc, 0
return
'This subroutine reads all of the Analog To Digital channels when it's called
'It stores the results in the array "Analog".
'It also stores the difference from the average value which was calculated
'earlier. This difference value is meaningless until after the Calibrate 
'subroutine is called. 
sense:
pokeb adctl, $10    ' multi-chnl, 1-4
waituntil adctl, $80    ' loop until conversion complete
Analog(0) = peekb(adr1)    
Analog(1) = peekb(adr2)    
Analog(2) = peekb(adr3)
Analog(3) = peekb(adr4)                       ' Analog 4
pokeb adctl, $14                                ' multi-chnl, 5-8
waituntil adctl, $80
Analog(4) = peekb(adr1)    
Analog(5) = peekb(adr2)    
Analog(6) = peekb(adr3)    ' Analog 7
Analog(7) = peekb(adr4)     ' Analog 8
for iTmp = 0 to 7
  SenseDiff(iTmp) = Analog(iTmp) - SenseAverage(iTmp)
next
Fob = Analog(0)
return
'Calibrate
'Does software calibration of the analog channels. The code
'runs for all the analog channels even though currently only
'4 of them have line sensors attatched. This keeps the code simple
'but if memory constraints become a problem it will need to be 
'optimized to only read the line sensors.
'It works by spinning the robot around for awhile and 
'tracking the largest and smallest readings for each sensor
'It also averages all of the readings for each sensor.
'Since SBasic doesn't have 32 bit integers it's necessary
'to calculate an average on 128 samples at a time. Then average
'each of those averages at the end of the calibration time.
'While this is inconvient, it works just fine. It is possible to
'loose up to 127 samples at the end, but on average we'll only
'loose half that many (64) and compared to the total samples
'I measured of approximately 3500 that's an insignifigant
'number of samples. 
'Since the majority of the paper doesn't have any lines on it
'the average value is very close to the value of the paper
'without a line. Providing a very good basis for determining
'if a line is present or not.
'Lastly this function calculates a sense threshold. This threshold is
'used to determin if a sensor is over a black line. It's calculated by
'picking a value exactly half way between the average and the maximum
'value for that sensor. The goal is to privide a threshold with lots
'of margin for error.
Calibrate:
  'Spin in place counter clockwise
  poke RightSpeed, RightFF
  poke LeftSpeed, LeftFR
 
  ' Initialize with values to be replaced
  for i = 0 to 7     ' Loop through all 8 analog channels
    SenseMin(i) = 255
    SenseMax(i) = 0
    SenseTotalL(i) = 0
    SenseTotalH(i) = 0
  next
  ' Calculate min and max and average values while spinning in place
  RollIt = 0
  TimeCount = 0
  TimeCountH = 0
  while RollIt < 400  'arbitrarily wait until the PWM generator has rolled over 400 times
    gosub Sense
    TimeCount = TimeCount + 1
    for i = 0 to 7
      SenseMin(i) = Min(SenseMin(i), Analog(i))
      SenseMax(i) = Max(SenseMax(i), Analog(i))
      SenseTotalL(i) = SenseTotalL(i) + Analog(i) 'Total the readings for the Low average
    next
 
    'If it's time to calculate the low average, then do it
    if TimeCount = 128 
      TimeCount = 0   'Prepare to add up another 128 samples
      TimeCountH = TimeCountH + 1 'Count the number of low averages we're adding to High average total
      for i = 0 to 7
        x = SenseTotalL(i) / 128
        SenseTotalL(i) = 0
        SenseTotalH(i) = SenseTotalH(i) + x
      next      
    endif
  wend
  ' print the results for debugging
  for i = 0 to 7
    SenseAverage(i) = SenseTotalH(i) / TimeCountH
    x = SenseAverage(i) + SenseMax(i)
    SenseThreshold(i) = x / 2
    printx "Index:"; i;"  Min:"; SenseMin(i); "   Max:"; SenseMax(i); "   Average:"; SenseAverage(i); "  Thresh:"; SenseThreshold(i)
  next 
  printx "TimeCount:"; TimeCount
  printx "TimeCountH:"; TimeCountH
  'Time to stop spinning in place
  poke RightSpeed, RightStop
  poke LeftSpeed, LeftStop
return
main:
'Initialize the serial port
pokeb  baud, $30
pokeb  sccr1, $00  'critical line that was missing from the examples I used
pokeb  sccr2, $0c
'Set Port C is all outputs and set all the pins low
pokeb  ddrc, $ff
pokeb  portc, $00
'Overflow and RTI
pokeb   tflg2, %11000000
pokeb   tmsk2, %11000000
'Initialize A/D
pokeb  option, $80  ' turn on A/D system
' Set up for PWM
pokeb OC1M,  %11111000    ' Out Cmp 1 affects PA7-PA3
pokeb OC1D, %11111000     ' Sending them high
pokeb TCTL1, %10100000 ' OC2 turns off PA6 and OC3 turns off PA5
pokeb TMSK1, %00000000 'no interrupt
pokeb TFLG1, %00000000 
'Stop the servos by sending the correct PWM as determined through experimentation
poke  TOC2, LeftStop      'Stop for left servo
poke  TOC3, RightStop
interrupts on
'Gosub Alive   'don't call the subroutine alive anymore
print "Hello There"
print
'Z never equals 35 so this code isn't used anymore. I used it while figuring out the 
'PWM values to use for stop. Fob is an analog channel that I hung a potentiometer off of. 
'The potentiometer was wired as a voltage divider to provide a number between 0-255 depending
'on it's position. 
if z = 35
  gosub Sense
  Tmp = peek(TOC2)
  FobAdjust = Fob - 8
  if FobAdjust < 0
     FobAdjust = 0
  endif
  FobAdjust = $950 + FobAdjust
  Tmp = FobAdjust
  printx "Fob: "; Fob; "  NewVal:"; Tmp
  
  poke TOC3, Tmp
 for x = 0 to 2000
 next
endif

'Calibrate the analog line sensors
GoSub Calibrate
' Spin counter clockwise
poke RightSpeed, RightFF
poke LeftSpeed, LeftFR
' Keep spinning until we find the line by watching the Right Center (RC) 
' Analog Sensor. When we find it the line should be between the left center and 
' right center sensors. At which point it's time to simply follow the line.
n = 0
while n = 0
  Gosub Sense
  if Analog(RC) > SenseThreshold(RC)
    n = 1
  endif
wend
'Follow the line by adjusting the pulse width modulation (PWM) 
'each time the PWM clock generator rolls over (starting the pulse)
RollIt = 0
while z<> 0 ' Loop forever
  if RollIt > 0  ' Wait for the PWM counter to roll over
     RollIt = 0  ' Reset PWM rollover flag for next time.
     gosub Sense  'Read the line sensors
       x = SenseDiff(RC) - SenseDiff(LC)  'Get the difference in the readings
       'Convert the difference to an absolute value
       if x < 0
          x = 0 - x
       endif
       'If the difference is small enough run both wheels forward
       'Note: This isn't working well and needs work. x is rarely, if ever under 8
       if x < 8
   poke RightSpeed, RightFF 
          poke LeftSpeed, LeftFF
       else
  'Turn which ever way is necessary to center the line.
         if SenseDiff(RC) > SenseDiff(LC)
            'too far right
     poke RightSpeed, RightStop 
            poke LeftSpeed, LeftFF
         else
     poke RightSpeed, RightFF
            poke LeftSpeed, LeftStop
         endif
       endif
  endif
wend