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