Datasheet

Page 126· Applied Robotics with the SumoBot
' -----[ Main Routine ]-------------------------------------------------------
DO ' DO...LOOP repeats indefinitely
GOSUB Read_Line_Sensors ' Look for lines
GOSUB Read_Object_Detectors ' Look for objects
GOSUB Read_Pushbutton ' Check pushbutton
DEBUG HOME, ' Display all sensors states
? irLS,
? irLF,
? irRF,
? irRS,
? qtiStateLeft,
? qtiStateRight,
? pushbutton
' Delay for slower PCs
PAUSE 100
LOOP
' -----[ Subroutine - Calibrate_Qtis ]----------------------------------------
Calibrate_Qtis:
HIGH qtiPwrLeft ' Turn left QTI on
HIGH qtiSigLeft ' Discharge capacitor
PAUSE 1
RCTIME qtiSigLeft, 1, qtiLeft ' Measure charge time
LOW qtiPwrLeft ' Turn left QTI off
HIGH qtiPwrRight ' Turn right QTI on
HIGH qtiSigRight ' Discharge capacitor
PAUSE 1
RCTIME qtiSigRight, 1, qtiRight ' Measure charge time
GOSUB Read_Line_Sensors ' Get reflection values
qtiThreshold = (qtiLeft + qtiRight) / 2 ' Calculate average
qtiThreshold = qtiThreshold / 4 ' Take 1/4 average
IF qtiThreshold > 220 THEN ' Account for code overhead
qtiThreshold = qtiThreshold - 220
ELSE
qtiThreshold = 0
ENDIF
RETURN
' -----[ Subroutine - Read_Line_Sensors ]-------------------------------------
Read_Line_Sensors: