Control
Material taken from RobotSubsumption.pdf and SumoBot : Mini-Sumo Robotics
Remember Where Are We Going?
Sumo-Bot competitions
Controlling Robot Movement Based on Photo-Resistor Readings' -----[ Constants ]---------------------------------------LeftDark CON 108RightDark CON 114LeftWhite CON 20RightWhite CON 22' Average Scale factorLeftThreshold CON LeftWhite + LeftDark / 2 RightThreshold CON RightWhite + RightDark / 2 ' -----[ Variables ]----------------------------------------timeLeft VAR WordtimeRight VAR Word' -----[ Main Routine ]------------------------------------DO GOSUB Test_Photoresistors GOSUB NavigateLOOP
Code' -----[ Subroutine - Test_Photoresistors ]--------------Test_Photoresistors: HIGH 6 ' Left RC time measurement. PAUSE 3 RCTIME 6,1,timeLeft HIGH 3 ' Right RC time measurement. PAUSE 3 RCTIME 3,1,timeRightRETURN
Code' -----[ Subroutine - Navigate ]--------------------------Navigate: IF (timeLeft < LeftThreshold) AND (timeRight < RightThreshold)
THEN PULSOUT 13, 650 PULSOUT 12, 850 ELSEIF (timeLeft < LeftThreshold) THEN PULSOUT 13, 800 PULSOUT 12, 600 ELSEIF (timeRight < RightThreshold) THEN PULSOUT 13, 600 PULSOUT 12, 800 ELSE PULSOUT 13, 850 PULSOUT 12, 650 ENDIF PAUSE 20 RETURN
Finite State Machine (FSM) Representation
Read photo- resistors
backup turn right turn left go forward
both lowright low
left lowboth high
Controlling Robot Movement Based on Proximity
' {$STAMP BS2} ' {$PBASIC 2.5}' -----[ Variables ]-----------------------------------irDetectLeft VAR BitirDetectRight VAR BitpulseCount VAR Byte
Code
' -----[ Main Routine ]-----------------------------------DO GOSUB Read_IR IF (irDetectLeft = 0) AND (irDetectRight = 0) THEN GOSUB Forward_Pulse ELSEIF (irDetectLeft = 0) THEN GOSUB Turn_Left ELSEIF (irDetectRight = 0) THEN GOSUB Turn_Right ELSE GOSUB Forward_Pulse ENDIF LOOP
Code
' -----[ Subroutines ]-------------------------------------Read_IR: FREQOUT 8, 1, 38500 irDetectLeft = IN9 FREQOUT 2, 1, 38500 irDetectRight = IN0
Code
' -----[ Subroutines ]--------------------------------------Forward_Pulse: PULSOUT 13, 850 PULSOUT 12, 650 PAUSE 20RETURN
Turn_Left: PULSOUT 13, 650 PULSOUT 12, 650 PAUSE 20RETURN
Code' -----[ Subroutines ]--------------------------------------Turn_Right: PULSOUT 13, 850 PULSOUT 12, 850 PAUSE 20RETURN
Back_Up: PULSOUT 13, 650 PULSOUT 12, 850 PAUSE 20RETURN
Finite State Machine (FSM) Representation
Read IR Go forward turn left turn right go forward
Obj forwardObj right
Obj leftNo Obj
How to Put It Together?
Read IR Go forward turn left turn right go forward
Obj forwardObj right
Obj leftNo Obj
Read photo- resistors
backup turn right turn left go forward
both lowright low
left lowboth high
Possible Problems
Jerky or halting movement Chase object over boundary More?
Possible Solution
o Subsumption ArchitectureA programming process by which one behavior subsumes, or over-rides another based on an explicit priority that we have defined. First described by Dr. Rodney Brooks in "A robust layered control system for a mobile robot,” IEEE Journal of Robotics and Automation., RA-2, April, 14-23, 1986.
o FSM with exit conditionso Optimize PBasic control commands
FSM
read IRand set nextStatevariable
read Photoresistorsand set nextStatevariable
check nextStatevariable and branch
Go forward
Go backwards
turn teft
turn right
Alternative FSM
read Photoresistorsand set nextStatevariable
check nextStatevariable and branch
Read IR Go forward turn left turn right go forward
backup turn right turn left go forward
New PBasic Control Commands
BRANCH value, (Case_0, Case_1, Case_2)
is equivalent to:
Test_Value: IF (value = 0) THEN Case_0
' value = 0: go to label "Case_0" IF (value = 1) THEN Case_1
' value = 1: go to label "Case_1" IF (value = 2) THEN Case_2
' value = 2: go to label "Case_2"
New PBasic Control Commands
LOOKUP Index, (Value0, Value1, ...ValueN), Variable
Find the value at location Index and store it in Variable. If Index exceeds the highest index value of the items in the list, Variable is left unaffected.
LOOKDOWN Target, {ComparisonOp} [Value0, Value1, ...ValueN], Variable
Compare Target value to a list of values and store the index number of the first value that matches into Variable. If no value in the list matches, Variable is left unaffected. The optional ComparisonOp is used as criteria for the match; the default criteria is "equal to."
The ImplementationlineBits VAR NiblineLeft VAR lineBits.Bit1lineRight VAR lineBits.Bit0Main: GOSUB Read_Line_Sensors
BRANCH lineBits, [Search_For_Opponent, Spin_Left, Spin_Right, About_Face]
Read_Line_Sensors: HIGH LLineSnsIn HIGH RLineSnsIn PAUSE 10 RCTIME LLineSnsIn, 1, leftSense RCTIME RLineSnsIn, 1, rightSense ' convert readings to bits lineBits = %00 LOOKDOWN leftSense, >=[blackThresh, 0], lineLeft LOOKDOWN rightSense, >=[blackThresh, 0], lineRight RETURN
A Bit More ComplicatedSearch_For_Opponent: GOSUB Read_IR_Sensors ' If opponent is not in view, scan last known direction. Turn
toward ' opponent if seen by one "eye" -- if both, lunge forward BRANCH irBits, [Scan, Follow_Right, Follow_Left, Lunge]
Scan: BRANCH lastIR, [Move_Fwd, Scan_Right, Scan_Left]
Lunge: ' locked on -- go get him! FOR pulses = 1 TO 15 PULSOUT LMotor, LFwdFast PULSOUT RMotor, RFwdFast GOSUB Read_Line_Sensors IF (lineBits = %11) THEN Match_Over
' in sight and we're on the line NEXT GOTO Main