' -----[ Title ]--------------------------------------------------------------
' Applied Robotics with the SumoBot - SumoWrestler.bs2
' SumoWrestler.bs2 modified so that each state is contained by a
' subroutine.
' {$STAMP BS2} ' Target = BASIC Stamp 2
' {$PBASIC 2.5} ' Language = PBASIC 2.5
' -----[ I/O Definitions ]---------------------------------------------------
ServoLeft PIN 13 ' Left servo connected to P13
ServoRight PIN 12 ' Right servo connected to P12
qtiPwrLeft PIN 10 ' Left QTI on/off pin P10
qtiSigLeft PIN 9 ' Left QTI signal pin P9
qtiPwrRight PIN 7 ' Right QTI on/off pin P7
qtiSigRight PIN 8 ' Right QTI signal pin P8
DummyPin PIN 6 ' I/O pin for pulse-decay P6
LedSpeaker PIN 5 ' LED & speaker connected to P5
IrLedLS PIN 2 ' Left IR LED connected to P2
IrSenseLS PIN 1 ' Left IR detector to P1
IrLedLF PIN 4 ' Left IR LED connected to P4
IrSenseLF PIN 11 ' Left IR detector to P11
IrLedRF PIN 15 ' Right IR LED connected to P15
IrSenseRF PIN 14 ' Right IR detector to P14
IrLedRS PIN 3 ' Right IR LED connected to P3
IrSenseRS PIN 0 ' Right IR detector to P0
' -----[ Constants ]----------------------------------------------------------
' SumoBot maneuvers
Forward CON 0 ' Forward
Backward CON 1 ' Backward
RotateLeft CON 2 ' RotateLeft
RotateRight CON 3 ' RotateRight
PivotLeft CON 4 ' Pivot to the left
PivotRight CON 5 ' Pivot to the right
CurveLeft CON 6 ' Curve to the left
CurveRight CON 7 ' Curve to the right
' Servo pulse width rotations
FS_CCW CON 850 ' Full speed counterclockwise
FS_CW CON 650 ' Full speed clockwise
NO_ROT CON 750 ' No rotation
LS_CCW CON 770 ' Low speed counterclockwise
LS_CW CON 730 ' Low speed clockwise
' IR object detectors
IrFreq CON 38500 ' IR LED frequency
' -----[ Variables ]----------------------------------------------------------
temp VAR Word ' Temporary variable
multi VAR Word ' Multipurpose variable
counter VAR Byte ' Loop counting variable.
maneuver VAR Nib ' SumoBot travel maneuver
sensors VAR Byte ' Sensor flags byte
qtiLF VAR sensors.BIT5 ' Stores snapshot of QtiSigLeft
qtiRF VAR sensors.BIT4 ' Stores snapshot of QtiSigRight
irLS VAR sensors.BIT3 ' State of Left Side IR
irLF VAR sensors.BIT2 ' State of Left Front IR
irRF VAR sensors.BIT1 ' State of Right Front IR
irRS VAR sensors.BIT0 ' State of Right Side IR
' -----[ EEPROM Data ]--------------------------------------------------------
RunStatus DATA 0 ' Run status EEPROM byte
QtiThresh DATA Word 0 ' Word for QTI threshold time
' -----[ Initialization ]-----------------------------------------------------
GOSUB Reset ' Wait for Reset press/release
GOSUB Start_Delay ' 5 Second delay
GOSUB Calibrate_Qtis ' Determine b/w threshold
GOSUB Look_About ' Was Goto Look_About
' -----[ Main Routine ]-------------------------------------------------------
DO
IF qtiLF = 1 THEN ' Left qti sees line?
GOSUB Avoid_Tawara_Left ' State = avoid left tawara
ELSEIF qtiRF = 1 THEN ' Right qti sees line?
GOSUB Avoid_Tawara_Right ' State = avoid right tawara
ELSEIF irLF = 1 AND irRF = 1 THEN ' Both? Lunge forward
GOSUB Go_Forward ' State = Go forward
ELSEIF irLF = 1 THEN ' Just left?
GOSUB Track_Front_Left_Object ' State = Track front left obj.
ELSEIF irRF = 1 THEN ' Just right?
GOSUB Track_Front_Right_Object ' State = Track front right obj.
ELSEIF irLS = 1 THEN ' Left side?
GOSUB Track_Side_Left_Object ' State = track side left obj.
ELSEIF irRS = 1 THEN ' Right side?
GOSUB Track_Side_Right_Object ' State = track side right obj.
ELSE ' Nothing sensed?
GOSUB Search_Pattern ' State = Search pattern
ENDIF
LOOP
' -----[ Subroutine - Reset ]-------------------------------------------------
Reset:
READ RunStatus, temp ' Byte @RunStatus -> temp
temp = temp + 1 ' Increment temp
WRITE RunStatus, temp ' Store new value for next time
IF (temp.BIT0 = 1) THEN ' Examine temp.BIT0
DEBUG CLS, "Press/release Reset", CR, ' 1 -> end, 0 -> keep going
"button..."
END
ELSE
DEBUG CR, "Program running..."
ENDIF
RETURN
' -----[ Subroutine - Start_Delay ]-------------------------------------------
Start_Delay:
FOR counter = 1 TO 5 ' 5 beeps, 1/second
PAUSE 900
FREQOUT LedSpeaker, 100, 3000
NEXT
RETURN
' -----[ Subroutine - Calibrate_Qtis ]----------------------------------------
Calibrate_Qtis:
HIGH qtiPwrLeft ' Turn left QTI on
HIGH qtiSigLeft ' Discharge capacitor
PAUSE 1
RCTIME qtiSigLeft, 1, temp ' Measure charge time
LOW qtiPwrLeft ' Turn left QTI off
multi = temp ' Free temp for another RCTIME
HIGH qtiPwrRight ' Turn right QTI on
HIGH qtiSigRight ' Discharge capacitor
PAUSE 1
RCTIME qtiSigRight, 1, temp ' Measure charge time
multi = (multi + temp) / 2 ' Calculate average
multi = multi / 4 ' Take 1/4 average
IF multi > 220 THEN ' Account for code overhead
multi = multi - 220
ELSE
multi = 0
ENDIF
WRITE QtiThresh, Word multi ' Threshold to EEPROM
RETURN
' -----[ Subroutine - Servos_And_Sensors ]------------------------------------
Servos_And_Sensors:
GOSUB Pulse_Servos ' Call Pulse_Servos subroutine
' Call sensor subroutine(s).
sensors = 0 ' Clear previous sensor values
GOSUB Read_Object_Detectors ' Call Read_Object_Detectors
GOSUB Read_Line_Sensors ' Look for lines
RETURN
' -----[ Subroutine - Pulse_Servos ]------------------------------------------
Pulse_Servos:
' Pulse to left servo
LOOKUP maneuver, [ FS_CCW, FS_CW, FS_CW, FS_CCW,
NO_ROT, FS_CCW, LS_CCW, FS_CCW ], temp
PULSOUT ServoLeft, temp
' Pulse to right servo
LOOKUP maneuver, [ FS_CW, FS_CCW, FS_CW, FS_CCW,
FS_CW, NO_ROT, FS_CW, LS_CW ], temp
PULSOUT ServoRight, temp
' Pause between pulses (remove when using IR object detectors + QTIs).
' PAUSE 20
RETURN
' -----[ Subroutine - Read_Object_Detectors ]---------------------------------
Read_Object_Detectors:
FREQOUT IrLedRS, 1, IrFreq ' Right side IR LED headlight
irRS = ~IrSenseRS ' Save right side IR receiver
FREQOUT IrLedRF, 1, IrFreq ' Repeat for right-front
irRF = ~IrSenseRF
FREQOUT IrLedLF, 1, IrFreq ' Repeat for left-front
irLF = ~IrSenseLF
FREQOUT IrLedLS, 1, IrFreq ' Repeat for left side
irLS = ~IrSenseLS
RETURN
' -----[ Subroutine - Read_Line_Sensors ]-------------------------------------
Read_Line_Sensors:
HIGH qtiPwrLeft ' Turn on QTIs
HIGH qtiPwrRight
HIGH qtiSigLeft ' Push signal voltages to 5 V
HIGH qtiSigRight
PAUSE 1 ' Wait 1 ms for capacitors
READ QtiThresh, Word temp ' Get threshold time
INPUT qtiSigLeft ' Start the decays
INPUT qtiSigRight
PULSOUT DummyPin, temp ' Wait threshold time
qtiLF = ~qtiSigLeft ' Snapshot of QTI signal states
qtiRF = ~qtiSigRight
LOW qtiPwrLeft ' Turn off QTIS
LOW qtiPwrRight
RETURN
' -----[ Subroutine - Avoid_Tawara_Left ]-------------------------------------
Avoid_Tawara_Left:
FOR counter = 1 TO 15 ' Back up
maneuver = Backward
GOSUB Servos_And_Sensors
NEXT
FOR counter = 1 TO 15 ' Turn right
maneuver = RotateRight
GOSUB Servos_And_Sensors
NEXT
RETURN
' -----[ Subroutine - Avoid_Tawara_Right ]------------------------------------
Avoid_Tawara_Right:
FOR counter = 1 TO 15 ' Back up
maneuver = Backward
GOSUB Servos_And_Sensors
NEXT
FOR counter = 1 TO 15 ' Turn left
maneuver = RotateLeft
GOSUB Servos_And_Sensors
NEXT
RETURN
' -----[ Subroutine - Go_Forward ]--------------------------------------------
Go_Forward:
maneuver = Forward ' 1 forward pulse
GOSUB Servos_And_Sensors
RETURN
' -----[ Subroutine - Track_Front_Left_Object ]-------------------------------
Track_Front_Left_Object:
counter = 0
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
maneuver = CurveLeft ' Curve left 15
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30
maneuver = RotateLeft ' Rotate left 30
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
RETURN
' -----[ Subroutine - Track_Front_Right_Object ]------------------------------
Track_Front_Right_Object:
counter = 0
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 15
maneuver = CurveRight ' Curve right 15
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
DO UNTIL (irLF = 1 AND irRF = 1) OR counter > 30
maneuver = RotateRight ' Rotate right 30
GOSUB Servos_And_Sensors
counter = counter + 1
LOOP
RETURN
' -----[ Subroutine - Track_Side_Left_Object ]--------------------------------
Track_Side_Left_Object:
DO UNTIL irRF = 1 OR irLF = 1 ' Rotate left
maneuver = RotateLeft
GOSUB Servos_And_Sensors
LOOP
RETURN
' -----[ Subroutine - Track_Side_Right_Object ]-------------------------------
Track_Side_Right_Object:
DO UNTIL irRF = 1 OR irLF = 1 ' Rotate right
maneuver = RotateRight
GOSUB Servos_And_Sensors
LOOP
RETURN
' -----[ Subroutine - Search_Pattern ]----------------------------------------
Search_Pattern:
FOR counter = 1 TO 35 ' and watch all sensors
maneuver = Forward ' Forward
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
Look_About:
FOR counter = 1 TO 12 ' Look right
maneuver = RotateRight
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
FOR counter = 1 TO 24 ' Look left
maneuver = RotateLeft
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
FOR counter = 1 TO 12 ' Re-align to forward
maneuver = RotateRight
GOSUB Servos_And_Sensors
IF sensors <> 0 THEN GOTO Next_State
NEXT
Next_State: ' Exit point of search pattern
RETURN