Boe-Bot Maze Navigation with QTIs
pluesmike
Posts: 1
i am using the boe-bot in my electronics class and we are using the qti maze program.· My boe-bot runs the program fine untill it reaches the finish and acts like a dead end and reverses to find the line (which it can not do) till i turn it off.
this is the program i am using can anyone help
'
[noparse][[/noparse] Title ]
' FullMazeNavigation.bs2
' Navigate your Boe-Bot through a "maze" made of electrical tape using the QTI Line
' Follower App Kit.
·
' IMPORTANT: This program has several operations that have to be tuned before it will work
' right! Follow the instructions on forums.parallax.com -> Stamps in Class -> Stamps in
' Class "Mini Projects" -> Boe-Bot Maze Navingation before you run this program.
·
' {$STAMP BS2}··························· ' Target device = BASIC Stamp 2
' {$PBASIC 2.5}·························· ' Language = PBASIC 2.5
·
'
[noparse][[/noparse] Variables ]
·
rng············ VAR·· Byte······· ········' Random number generator variable
pulseCount····· VAR·· Byte··············· ' FOR..NEXT loop counter for turning
turnDecision··· VAR·· rng.BIT0··········· ' Bit zero of the random number generated
AI············· VAR·· Bit················ ' "Artificial Intelligence" remembers dead ends
qtis··········· VAR·· Nib················ ' QTI black/white states
·
'
[noparse][[/noparse] Initialization ]
·
OUTB = %1111····························· ' Set OUTB bits to 1
AI = 0··································· ' Set AI variable to 0
·
FREQOUT 15, 1000, 2000··················· ' Battery Tester
·
'
[noparse][[/noparse] Main Routine ]
·
DO······································· ' Main DO...LOOP
· RANDOM rng····························· ' Create a random number
· GOSUB Check_Qtis··· ····················' Get QTI states
· SELECT qtis
·
· CASE %0110····························· ' Mid Left and Right sensors detected, go forward
··· GOSUB Forward
· CASE %1100····························· ' Far and Mid Left sensors detected = off track
··· GOSUB BackOnTrack_TwoSensorsLeft
· CASE %0011····························· ' Far and Mid Right sensor detected = off track
··· GOSUB BackOnTrack_TwoSensorsRight
· CASE %1110, %0111······················ ' Turn Detected
··· GOSUB Turn_Check
· CASE %1111··· ··························' All sensors detected = T-Intersection
··· GOSUB T_Intersection
· CASE %0000····························· ' No track = dead end
··· GOSUB Dead_End
· CASE %0001····························· ' Far Right sensor detected
··· GOSUB BackOnTrack_OneSensorRight
· CASE %1000····························· ' Far Left sensor detected
··· GOSUB BackOnTrack_OneSensorLeft
· CASE ELSE······························ ' Any other combination, move forward until
··· PULSOUT 13, 770···················· ··' something else is detected.
··· PULSOUT 12, 730
··· PAUSE 20
· ENDSELECT
LOOP
·
'
[noparse][[/noparse] Subroutines - Check_Qtis ]
' Checks the state of each QTI Sensor. 0 means white surface, 1 means black surface.
·
Check_Qtis:
· DIRB = %1111··························· ' P7..P4 -> output
· PAUSE 0································ ' Delay = 230 us
· DIRB = %0000··························· ' P7..P4 -> input
· PAUSE 0································ ' Delay = 230 us
· qtis = INB····························· ' Store QTI outputs in INB
RETURN
·
'
[noparse][[/noparse] Subroutines - Forward ]
' Boe-Bot moves forward.
·
Forward:
· PULSOUT 13, 770
· PULSOUT 12, 730
· PAUSE 20
RETURN
·
'
[noparse][[/noparse] Subroutines - Turn_Check ]
' When three sensors are detected, this subroutine checks if there is actually a turn, or
' if the Boe-Bot got off track.
·
Turn_Check:
· PULSOUT 13, 750····· ···················' Stop moving
· PULSOUT 12, 750
·
· FOR pulseCount = 0 TO 5················ ' Move forward a bit
··· PULSOUT 13, 770
··· PULSOUT 12, 730
··· PAUSE 20
· NEXT
·
· GOSUB Check_Qtis······················· ' Check the new QTI states
· SELECT qtis
·
· CASE %1110····························· ' Left turn is detected
··· GOSUB AI_Decision
· CASE %0111····························· ' Right turn is detected
··· GOSUB Turn_Right
· CASE ELSE······························ ' Boe-Bot was off track, return to main program
··· RETURN
· ENDSELECT
RETURN
·
'
[noparse][[/noparse] Subroutines - AI_Decision ]
' When a left turn is detected, check if the Boe-Bot hit a dead end last.· This will keep
' it from returning to Start.
·
AI_Decision:
· IF (AI = 1) THEN······················· ' If the AI variable is 1, Boe-Bot was in dead end
··· AI = 0······························· ' Set AI variable to 0
··· GOSUB Ignore_Turn···················· ' Ignore the turn that leads to Start
· ELSEIF (AI = 0) THEN··················· ' If the AI variable is 0, Boe-Bot was not in
··· GOSUB Turn_Left······················ ' dead end, so it's OK to turn left.
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - Ignore_Turn ]
' AI variable was 1, so ignore the left turn by going forward for 50 pulses.
·
Ignore_Turn:
· FOR pulseCount = 0 TO 50
··· PULSOUT 13, 770
··· PULSOUT 12, 730
··· PAUSE 20
· NEXT
RETURN
·
'
[noparse][[/noparse] Subroutines - Turn_Left ]
' Boe-Bot executes a left turn.
·
Turn_Left:
· FOR pulseCount = 0 TO 15··············· ' Go forward a bit so Boe-Bot stays on course.
··· PULSOUT 13, 770
··· PULSOUT 12, 730
··· PAUSE 20
· NEXT
· FOR pulseCount = 0 TO 17··············· ' Turn left, about 90-degrees
··· PULSOUT 13, 650
··· PULSOUT 12, 650
··· PAUSE 20
· NEXT
RETURN
·
'
[noparse][[/noparse] Subroutines - Turn_Right ]
' Boe-Bot executes a right turn.
·
Turn_Right:
· FOR pulseCount = 0 TO 15··············· ' Go forward a bit so Boe-Bot stays on course
··· PULSOUT 13, 770
··· PULSOUT 12, 730
·· PAUSE 20
· NEXT
· FOR pulseCount = 0 TO 17··············· ' Turn right, about 90-degrees
··· PULSOUT 13, 850
··· PULSOUT 12, 850
··· PAUSE 20
· NEXT
RETURN
·
'
[noparse][[/noparse] Subroutines - T_Intersection ]
' Boe-Bot makes a random turn when it comes to a T-Intersection
·
T_Intersection:
· IF turnDecision = 0 THEN··············· ' If Bit0 is 0 then turn right
··· GOSUB Turn_Right
· ELSEIF turnDecision = 1 THEN··········· ' If Bit0 is 1 then turn left
··· GOSUB Turn_Left
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - Dead_End ]
' Boe-Bot detects a dead end, backs up, and turns around to get back on track.
·
Dead_End:
· DO
··· GOSUB Check_Qtis
··· PULSOUT 13, 730
··· PULSOUT 12, 770
··· PAUSE 20
· LOOP UNTIL (qtis = %0111)
·
· IF (qtis = %0111) THEN
··· PAUSE 20
··· FOR pulseCount = 0 TO 15
····· PULSOUT 13, 770
····· PULSOUT 12, 730
····· PAUSE 20
··· NEXT
··· FOR pulseCount = 0 TO 17
····· PULSOUT 13, 850
····· PULSOUT 12, 850
····· PAUSE 20
··· NEXT
· ENDIF
· AI = 1
RETURN
·
'
[noparse][[/noparse] Subroutines - BackOnTrack_TwoSensorsLeft ]
' Boe-Bot is off track.· Boe-Bot slowly rotates until the two middle sensors are detected,
' and then centers itself on the line.
·
· PULSOUT 13, 750························ ' Stop
· PULSOUT 12, 750
· DO
··· GOSUB Check_Qtis····· ················' Slowly rotate until the middle
··· PULSOUT 13, 750······················ ' QTIs are back on the line
··· PULSOUT 12, 740
··· PAUSE 20
· LOOP UNTIL (qtis = %0110)
·
· IF (qtis = %0110) THEN················· ' When the middle QTIs are back
··· FOR pulseCount = 1 TO 20············· ' on the line, rotate left for
····· PULSOUT 13, 750···················· ' 20 pulses and then rotate right
····· PULSOUT 12, 730···················· ' for 10 pulses to center the Boe-
····· PAUSE 20··············· ············' Bot on the line.
··· NEXT
··· FOR pulseCount = 0 TO 10
····· PULSOUT 13, 770
····· PULSOUT 12, 750
····· PAUSE 20
··· NEXT
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - BackOnTrack_TwoSensorsRight ]
' Boe-Bot is off track.· Boe-Bot slowly rotates until the two middle sensors are detected,
' and then centers itself on the line.
·
· PULSOUT 13, 750························ ' Stop
· PULSOUT 12, 750
· DO
··· GOSUB Check_Qtis····················· ' Slowly rotate until the middle
··· PULSOUT 13, 760······················ ' QTIs are back on the line
··· PULSOUT 12, 750
··· PAUSE 20
· LOOP UNTIL (qtis = %0110)
·
· IF (qtis = %0110) THEN················· ' When the middle QTIs are back
··· FOR pulseCount = 1 TO 20········· ····' on the line, rotate right for
·· ···PULSOUT 13, 770···················· ' 20 pulses and then rotate left
·· ···PULSOUT 12, 750···················· ' for 10 pulses to center the Boe-
····· PAUSE 20··························· ' Bot on the line.
··· NEXT
··· FOR pulseCount = 0 TO 10
····· PULSOUT 13, 750
····· PULSOUT 12, 730
····· PAUSE 20
··· NEXT
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - BackOnTrack_OneSensorLeft ]
' Boe-Bot is off track.· Boe-Bot slowly rotates until the two middle sensors are detected,
' and then centers itself on the line.
BackOnTrack_OneSensorLeft:
· PULSOUT 13, 750························ ' Stop
· PULSOUT 12, 750
· DO····································· ' Slowly rotate until the middle
··· GOSUB Check_Qtis····················· ' QTIs are back on the line
··· PULSOUT 13, 750
··· PULSOUT 12, 740
··· PAUSE 20
· LOOP UNTIL (qtis = %0110)
·
· IF (qtis = %0110) THEN················· ' When the middle QTIs are back
··· FOR pulseCount = 1 TO 20····· ········' on the line, rotate left for
····· PULSOUT 13, 750···················· ' 20 pulses and then rotate right
····· PUL
this is the program i am using can anyone help
'
[noparse][[/noparse] Title ]
' FullMazeNavigation.bs2
' Navigate your Boe-Bot through a "maze" made of electrical tape using the QTI Line
' Follower App Kit.
·
' IMPORTANT: This program has several operations that have to be tuned before it will work
' right! Follow the instructions on forums.parallax.com -> Stamps in Class -> Stamps in
' Class "Mini Projects" -> Boe-Bot Maze Navingation before you run this program.
·
' {$STAMP BS2}··························· ' Target device = BASIC Stamp 2
' {$PBASIC 2.5}·························· ' Language = PBASIC 2.5
·
'
[noparse][[/noparse] Variables ]
·
rng············ VAR·· Byte······· ········' Random number generator variable
pulseCount····· VAR·· Byte··············· ' FOR..NEXT loop counter for turning
turnDecision··· VAR·· rng.BIT0··········· ' Bit zero of the random number generated
AI············· VAR·· Bit················ ' "Artificial Intelligence" remembers dead ends
qtis··········· VAR·· Nib················ ' QTI black/white states
·
'
[noparse][[/noparse] Initialization ]
·
OUTB = %1111····························· ' Set OUTB bits to 1
AI = 0··································· ' Set AI variable to 0
·
FREQOUT 15, 1000, 2000··················· ' Battery Tester
·
'
[noparse][[/noparse] Main Routine ]
·
DO······································· ' Main DO...LOOP
· RANDOM rng····························· ' Create a random number
· GOSUB Check_Qtis··· ····················' Get QTI states
· SELECT qtis
·
· CASE %0110····························· ' Mid Left and Right sensors detected, go forward
··· GOSUB Forward
· CASE %1100····························· ' Far and Mid Left sensors detected = off track
··· GOSUB BackOnTrack_TwoSensorsLeft
· CASE %0011····························· ' Far and Mid Right sensor detected = off track
··· GOSUB BackOnTrack_TwoSensorsRight
· CASE %1110, %0111······················ ' Turn Detected
··· GOSUB Turn_Check
· CASE %1111··· ··························' All sensors detected = T-Intersection
··· GOSUB T_Intersection
· CASE %0000····························· ' No track = dead end
··· GOSUB Dead_End
· CASE %0001····························· ' Far Right sensor detected
··· GOSUB BackOnTrack_OneSensorRight
· CASE %1000····························· ' Far Left sensor detected
··· GOSUB BackOnTrack_OneSensorLeft
· CASE ELSE······························ ' Any other combination, move forward until
··· PULSOUT 13, 770···················· ··' something else is detected.
··· PULSOUT 12, 730
··· PAUSE 20
· ENDSELECT
LOOP
·
'
[noparse][[/noparse] Subroutines - Check_Qtis ]
' Checks the state of each QTI Sensor. 0 means white surface, 1 means black surface.
·
Check_Qtis:
· DIRB = %1111··························· ' P7..P4 -> output
· PAUSE 0································ ' Delay = 230 us
· DIRB = %0000··························· ' P7..P4 -> input
· PAUSE 0································ ' Delay = 230 us
· qtis = INB····························· ' Store QTI outputs in INB
RETURN
·
'
[noparse][[/noparse] Subroutines - Forward ]
' Boe-Bot moves forward.
·
Forward:
· PULSOUT 13, 770
· PULSOUT 12, 730
· PAUSE 20
RETURN
·
'
[noparse][[/noparse] Subroutines - Turn_Check ]
' When three sensors are detected, this subroutine checks if there is actually a turn, or
' if the Boe-Bot got off track.
·
Turn_Check:
· PULSOUT 13, 750····· ···················' Stop moving
· PULSOUT 12, 750
·
· FOR pulseCount = 0 TO 5················ ' Move forward a bit
··· PULSOUT 13, 770
··· PULSOUT 12, 730
··· PAUSE 20
· NEXT
·
· GOSUB Check_Qtis······················· ' Check the new QTI states
· SELECT qtis
·
· CASE %1110····························· ' Left turn is detected
··· GOSUB AI_Decision
· CASE %0111····························· ' Right turn is detected
··· GOSUB Turn_Right
· CASE ELSE······························ ' Boe-Bot was off track, return to main program
··· RETURN
· ENDSELECT
RETURN
·
'
[noparse][[/noparse] Subroutines - AI_Decision ]
' When a left turn is detected, check if the Boe-Bot hit a dead end last.· This will keep
' it from returning to Start.
·
AI_Decision:
· IF (AI = 1) THEN······················· ' If the AI variable is 1, Boe-Bot was in dead end
··· AI = 0······························· ' Set AI variable to 0
··· GOSUB Ignore_Turn···················· ' Ignore the turn that leads to Start
· ELSEIF (AI = 0) THEN··················· ' If the AI variable is 0, Boe-Bot was not in
··· GOSUB Turn_Left······················ ' dead end, so it's OK to turn left.
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - Ignore_Turn ]
' AI variable was 1, so ignore the left turn by going forward for 50 pulses.
·
Ignore_Turn:
· FOR pulseCount = 0 TO 50
··· PULSOUT 13, 770
··· PULSOUT 12, 730
··· PAUSE 20
· NEXT
RETURN
·
'
[noparse][[/noparse] Subroutines - Turn_Left ]
' Boe-Bot executes a left turn.
·
Turn_Left:
· FOR pulseCount = 0 TO 15··············· ' Go forward a bit so Boe-Bot stays on course.
··· PULSOUT 13, 770
··· PULSOUT 12, 730
··· PAUSE 20
· NEXT
· FOR pulseCount = 0 TO 17··············· ' Turn left, about 90-degrees
··· PULSOUT 13, 650
··· PULSOUT 12, 650
··· PAUSE 20
· NEXT
RETURN
·
'
[noparse][[/noparse] Subroutines - Turn_Right ]
' Boe-Bot executes a right turn.
·
Turn_Right:
· FOR pulseCount = 0 TO 15··············· ' Go forward a bit so Boe-Bot stays on course
··· PULSOUT 13, 770
··· PULSOUT 12, 730
·· PAUSE 20
· NEXT
· FOR pulseCount = 0 TO 17··············· ' Turn right, about 90-degrees
··· PULSOUT 13, 850
··· PULSOUT 12, 850
··· PAUSE 20
· NEXT
RETURN
·
'
[noparse][[/noparse] Subroutines - T_Intersection ]
' Boe-Bot makes a random turn when it comes to a T-Intersection
·
T_Intersection:
· IF turnDecision = 0 THEN··············· ' If Bit0 is 0 then turn right
··· GOSUB Turn_Right
· ELSEIF turnDecision = 1 THEN··········· ' If Bit0 is 1 then turn left
··· GOSUB Turn_Left
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - Dead_End ]
' Boe-Bot detects a dead end, backs up, and turns around to get back on track.
·
Dead_End:
· DO
··· GOSUB Check_Qtis
··· PULSOUT 13, 730
··· PULSOUT 12, 770
··· PAUSE 20
· LOOP UNTIL (qtis = %0111)
·
· IF (qtis = %0111) THEN
··· PAUSE 20
··· FOR pulseCount = 0 TO 15
····· PULSOUT 13, 770
····· PULSOUT 12, 730
····· PAUSE 20
··· NEXT
··· FOR pulseCount = 0 TO 17
····· PULSOUT 13, 850
····· PULSOUT 12, 850
····· PAUSE 20
··· NEXT
· ENDIF
· AI = 1
RETURN
·
'
[noparse][[/noparse] Subroutines - BackOnTrack_TwoSensorsLeft ]
' Boe-Bot is off track.· Boe-Bot slowly rotates until the two middle sensors are detected,
' and then centers itself on the line.
·
· PULSOUT 13, 750························ ' Stop
· PULSOUT 12, 750
· DO
··· GOSUB Check_Qtis····· ················' Slowly rotate until the middle
··· PULSOUT 13, 750······················ ' QTIs are back on the line
··· PULSOUT 12, 740
··· PAUSE 20
· LOOP UNTIL (qtis = %0110)
·
· IF (qtis = %0110) THEN················· ' When the middle QTIs are back
··· FOR pulseCount = 1 TO 20············· ' on the line, rotate left for
····· PULSOUT 13, 750···················· ' 20 pulses and then rotate right
····· PULSOUT 12, 730···················· ' for 10 pulses to center the Boe-
····· PAUSE 20··············· ············' Bot on the line.
··· NEXT
··· FOR pulseCount = 0 TO 10
····· PULSOUT 13, 770
····· PULSOUT 12, 750
····· PAUSE 20
··· NEXT
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - BackOnTrack_TwoSensorsRight ]
' Boe-Bot is off track.· Boe-Bot slowly rotates until the two middle sensors are detected,
' and then centers itself on the line.
·
· PULSOUT 13, 750························ ' Stop
· PULSOUT 12, 750
· DO
··· GOSUB Check_Qtis····················· ' Slowly rotate until the middle
··· PULSOUT 13, 760······················ ' QTIs are back on the line
··· PULSOUT 12, 750
··· PAUSE 20
· LOOP UNTIL (qtis = %0110)
·
· IF (qtis = %0110) THEN················· ' When the middle QTIs are back
··· FOR pulseCount = 1 TO 20········· ····' on the line, rotate right for
·· ···PULSOUT 13, 770···················· ' 20 pulses and then rotate left
·· ···PULSOUT 12, 750···················· ' for 10 pulses to center the Boe-
····· PAUSE 20··························· ' Bot on the line.
··· NEXT
··· FOR pulseCount = 0 TO 10
····· PULSOUT 13, 750
····· PULSOUT 12, 730
····· PAUSE 20
··· NEXT
· ENDIF
RETURN
·
'
[noparse][[/noparse] Subroutines - BackOnTrack_OneSensorLeft ]
' Boe-Bot is off track.· Boe-Bot slowly rotates until the two middle sensors are detected,
' and then centers itself on the line.
BackOnTrack_OneSensorLeft:
· PULSOUT 13, 750························ ' Stop
· PULSOUT 12, 750
· DO····································· ' Slowly rotate until the middle
··· GOSUB Check_Qtis····················· ' QTIs are back on the line
··· PULSOUT 13, 750
··· PULSOUT 12, 740
··· PAUSE 20
· LOOP UNTIL (qtis = %0110)
·
· IF (qtis = %0110) THEN················· ' When the middle QTIs are back
··· FOR pulseCount = 1 TO 20····· ········' on the line, rotate left for
····· PULSOUT 13, 750···················· ' 20 pulses and then rotate right
····· PUL