The Whole Shebang
Congratulations, you’ve completed the calibration process! After calibration, the final program may seem long, but it’s really just a compilation of all the short programs we’ve just written and tested. Breaking maze navigation, or and any other robotic problem, down into small pieces and calibrating at each step helps ensure success. If you develop these habits now, it can save you tremendous amounts of time preparing for your next robotics project or contest entry.
The final code has some extra features to make navigation more reliable. For example, there’s a Turn_Check subroutine that prevents the Boe-Bot from misinterpreting %0111 or %1110 as a full 90° turn. It’s true that those QTI patterns do sometimes indicate a turn, but the same patterns can also indicate that the Boe-Bot has just drifted a little to the left or right of the line. The Turn_Check subroutine figures out whether the Boe-Bot is off course or on a turn by:
- Moving the Boe-Bot forward for 5 pulses
- Obtaining another QTI reading
- If the sensors still tell Boe-Bot to turn, it will, any other reading returns the program to the main routine for line adjustment
Another extra feature of the final code is that each SELECT…CASE statement in the main routine is sent to a subroutine. This is because we want the code to execute as quickly as possible. Each QTI reading is crucial to accurate maze navigation, so the faster the main routine executes, the more QTI readings we obtain. If we put every movement in the main routine, this severely slows down our code, which could cause missed readings in important areas, such as turns or T-intersections. That’s why the code in the main routine only checks the QTI states, and depending on the result, gets sent to the corresponding subroutine.
Before the code below will work on your Boe-Bot, it is essential for you to go through and update all the values you determined in this project’s calibration and testing steps. When running your Boe-Bot through the maze, you may find that some values need to be adjusted to account for unforeseen obstacles. That’s OK! Experiment with the code until your Boe-Bot can consistently navigate through your electrical tape maze.
' -----[ 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 above before running this
' program.
' {$STAMP BS2} ' Target device = BASIC Stamp 2
' {$PBASIC 2.5} ' Language = PBASIC 2.5
' -----[ Variables ]---------------------------------------------------------
rng VAR Byte ' Random number generator variable
pulseCount VAR Byte ' FOR..NEXT loop counter for turning
turnDecision VAR rng.BIT0 ' Bit 0 of random number generated
AI VAR Bit ' "AI" remembers dead ends
qtis VAR Nib ' QTI black/white states
' -----[ Initialization ]----------------------------------------------------
OUTB = %1111 ' Set OUTB bits to 1
AI = 0 ' Set AI variable to 0
FREQOUT 15, 1000, 2000 ' Battery Tester
' -----[ 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 = off track
GOSUB BackOnTrack_OneSensorRight
CASE %1000 ' Far Left sensor detected = off track
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
' -----[ Subroutines - Check_Qtis ]------------------------------------------
' Checks the state of each QTI Sensor. 0 means white surface, 1 means black
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
' -----[ Subroutines - Forward ]---------------------------------------------
' Boe-Bot moves forward.
Forward:
PULSOUT 13, 770
PULSOUT 12, 730
PAUSE 20
RETURN
' -----[ 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 ' return to main routine loop
ENDSELECT
RETURN
' -----[ 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
' -----[ 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
' -----[ 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
' -----[ 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
' -----[ 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
' -----[ 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
' -----[ 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
' -----[ 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
' -----[ 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
PULSOUT 12, 740 ' 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
' -----[ Subroutines - BackOnTrack_OneSensorRight ]--------------------------
' Boe-Bot is off track. Boe-Bot slowly rotates until the two middle sensors
' are detected, and then centers itself on the line.
BackOnTrack_OneSensorRight:
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, 760 ' 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
Your Turn: Challenge Yourself and Friends!
Although this code was written specifically for the maze shown in Figure 2 (previous section), it can be adapted to a variety of other situations as well as expanded for more complex mazes. Challenge yourself by creating a random maze and see if you can program your Boe-Bot to get through it! You can also challenge your friends and see who can best navigate through mazes, or who can do it the fastest. As a final touch, try adding obstacles that can be detected by whiskers, infrared or light sensors, and try programming your Boe-Bot to navigate around these obstacles and get back on the maze.