Boe-Bot® Robot Navigation with Accelerometer Incline Sensing
Andy Lindsay (Parallax)
Posts: 1,919
Boe-Bot® Robot Navigation with Accelerometer Incline Sensing
Let's say your Boe-Bot is attempting to navigate terrain with hills and valleys.· Depending on how steep the hills are, the Boe-Bot may need to detect whether or not it is about to roll back down the hill.· As part of a contest, the Boe-Bot may need to find the top of the hill (or the bottom of a valley) faster than its competitors.· The Memsic MX2125 Dual-Axis Accelerometer makes all this and more possible, and this activity will show you how.
+ Download zipped video clip (765 KB) - HillClimbingBoeBot.wmv·
·
The ability to sense incline and make navigation decisions is also especially important when the Boe-Bot is equipped with the Boe-Bot Tank Tread Kit.· The Boe-Bot Tank Tread Kit Documentation (.pdf) has example with an accelerometer added to the IR roaming application from Robotics with the Boe-Bot to keep it from trying to climb an obstacle that are too steep and might cause it to flip.·
·
Parts and Equipment
With the exception of the Memsic MX2125 dual-axis accelerometer, all the rest of the parts are already included in the Boe-Bot™ Robot kit.
·
(1) Boe-Bot – fully assembled and tested
(1) Memsic 2125 Dual-axis Accelerometer
(2) Resistors – 220 Ω (Red Red Brown)
(4) Jumper Wires
(1) Piezospeaker
Circuits
Figure 3 shows the schematic and wiring diagram for the Boe-Bot with the accelerometer circuit.· The servo and piezospeaker connections are from Robotics with the Boe-Bot.· The left servo is connected to P13, the right to P12, and the piezospeaker to P4.· The connections for the accelerometer are featured in various Stamps in Class Forum accelerometer posts.
·
√······· Build the circuit shown in Figure 3.
·
Tilt Axes on the Boe-Bot
It's important to get a feel for how the Boe-Bot's forward/backward and left/right tilt axes relate to the accelerometer's X and Y axes.
·
√······· Enter, save, and run BoeBotTiltTest.bs2, and keep an eye on the Debug Terminal as you tilt your Boe-Bot.
√······· Verify that the piezospeaker beeped for two seconds before the program continued.· If not double-check your program and wiring.
√······· Try tilting your Boe-Bot to the left and right.· Verify that the x-axis values range from -625 to 625 and that they are negative when you tilt the Boe-Bot to the left and positive when you tilt it to the right.
√······· Next tilt the Boe-Bot as though it is driving downhill, then uphill.· Verify that the y-axis values range from -625 to 625 and that they are positive when the Boe-Bot is tilted downhill and negative when the Boe-Bot is tilted uphill.
Example Program – Boe-BotTiltTest.bs2
·
' BoeBotTiltTest.bs2
' Test accelerometer readings.
·
'{$STAMP BS2}
'{$PBASIC 2.5}
·
x············· VAR···· Word················· ' Left/right tilt
y ·············VAR···· Word················· ' Forward/backward tilt
·
DEBUG "Beep!!!"····························· ' Test piezospeaker
FREQOUT 4, 2000, 3000
DEBUG CLS, "· x···· y", CR·················· ' Axis headings
·
DO···································· ······' Begin main routine
·
· PULSIN 6, 1, x···························· ' Get X-axis tilt
· PULSIN 7, 1, y···························· ' Get Y-axis tilt
·
· ' Instead of measurements that range from 1875 to 3125 with 2500 = no tilt,
· ' These two statements shift the tilt measurements to range from -625 to 625
· ' with 0 = no tilt.
· x = (x MIN 1875 MAX 3125) - 2500
· y = (y MIN 1875 MAX 3125) - 2500
·
· ' Display x and y tilt measurements.
· DEBUG CRSRX, 0, "(", SDEC3 x, ", ", SDEC3 y, ")", CLREOL
·
· PAUSE 100································· ' Pause 1/10 s
·
LOOP········································ ' Repeat main routine
Ramping in Navigation
Ramping is a common term for making the servos speed up and slow down gradually, no sudden starts, stops or direction changes.· For hill detection and climbing, it's crucial to have a reliable ramping routine for Boe-Bot navigation.· Without it, the Boe-Bot's abrupt motions cause acceleration measurements to get mixed in with the tilt measurements.· This in turn gives the Boe-Bot a false reading of the hill's steepness.·
·
The consequences of these false measurements can be pretty disabling for a hill climbing Boe-Bot.· Depending on how the program is designed, the Boe-Bot can actually move one direction, get a wrong tilt measurement, and try to correct with a sudden change of motion.· This in turn leads to another wrong tilt measurement, in the opposite direction, which the Boe-Bot again tries yet again to correct with another abrupt change in motion.· In some cases, the Boe-Bot can even end up on a slope paralyzed, just sitting in one place jittering.
EepromNavigationWithRamping.bs2
This next example demonstrates how ramping combined with EEPROM navigation smoothes out the Boe-Bot's transitions between maneuvers.·
·
√······· Enter, save, and run EepromNavigationWithRamping.bs2
√······· Verify that the Boe-Bot starts out by moving forward.· Depending on your servos, it may curve to the left or the right a little, which is okay.·
·
·
√······· Fine tune the distances so that they make your Boe-Bot travel in a rectangle.· The turn at the first corner should be left, 90°.· The second should be right, 270°, and the third should be another left at 90°.
·
'
[noparse][[/noparse] Title ]
' EepromNavigationWithRamping.bs2
' Demonstrate how ramping routine smoothes out EEPROM navigation.
·
'{$STAMP BS2}
'{$PBASIC 2.5}
·
'
[noparse][[/noparse] EEPROM Data ]
' Two DATA directives store the following navigation instructions:
' Forward, left 90-degrees, forward, right 270-degrees forward,
' left 90-degrees, forward, stop.
Directions DATA "F", "L", "F", "R", "F", "L", "F", "Q"
Distances· DATA· 70,· 53,· 80,· 77,· 90,· 53,· 80,· 0
·
'
[noparse][[/noparse] Variables ]
·
counter······· VAR···· Byte················· ' FOR...NEXT loop counter
index········· VAR···· Byte················· ' Points to EEPROM instructions
instruction··· VAR···· Byte················· ' Stores navigation instruction
pulseCount···· VAR···· Byte················· ' Stores navigation distance
·
pulseLeft····· VAR···· Word················· ' Stores pulse to left servo
pulseLeftOld·· VAR···· Word················· ' Previous pulse to left servo
·
pulseRight···· VAR···· Word················· ' Stores pulse to right servo
pulseRightOld· VAR···· Word················· ' Previous pulse to right servo
·
'
[noparse][[/noparse] Initialization ]
·
FREQOUT 4, 2000, 3000······················· ' Beep to signify program start
·
pulseLeft···· = 750························· ' Set all pulse values to center
pulseRight··· = 750
pulseLeftOld· = 750
pulseRightOld = 750
·
'
[noparse][[/noparse] Main Routine ]
·
DO·········································· ' Begin main routine
·
· ' Fetch EEPROM instruction.
· READ index + Directions, instruction······ ' Get navigation character
· READ index + Distances, pulseCount········ ' Get number of pulses
· index = index + 1
·
· ' This FOR...NEXT loop includes a navigation routine with ramping.
· FOR counter = 1 TO pulseCount············· ' Repeat for number of pulses
·
··· ' Navigation routine sets pulse width based on navigation character.
··· SELECT instruction
····· CASE "F"······························ ' Pulse durations for forward
······· pulseLeft = 850·····················
······· pulseRight = 650
····· CASE "L"······························ ' Pulse durations for left
······· pulseleft = 650·····················
······· pulseRight = 650
····· CASE "R"······························ ' Pulse durations for Right
······· pulseLeft = 850
······· pulseRight = 850
····· CASE "B"······························ ' Pulse durations for backward
······· pulseLeft = 650
······· pulseRight = 850
····· CASE "Q"······························ ' End the program
······· END
··· ENDSELECT
·
··· ' Increment/decrement routine only changes pulse durations by 2 at a time.
··· IF pulseLeft > (pulseLeftOld· + 2) THEN pulseleft· = pulseLeftOld· + 2
··· IF pulseLeft· < (pulseLeftOld· - 2) THEN pulseLeft· = pulseLeftOld· - 2
··· IF pulseRight > (pulseRightOld + 2) THEN pulseRight = pulseRightOld + 2
··· IF pulseRight < (pulseRightOld - 2) THEN pulseRight = pulseRightOld - 2
·
··· pulseLeft· = pulseLeft· MIN 650 MAX 850· ' Keep 650 <= durations <= 850
··· pulseRight = pulseRight MIN 650 MAX 850
·
··· pulseLeftOld· = pulseLeft··············· ' Remember old values
··· pulseRightOld = pulseRight
·
··· PULSOUT 13, pulseLeft··················· ' Send control pulses to servos
··· PULSOUT 12, pulseRight
··· PAUSE 20
·
· NEXT······································ ' Check FOR counter... condition
·
LOOP········································ ' Repeat main routine
How EepromNavigationWithRamping.bs2 Works
The Directions DATA directive stores a series of instructions that include "F" for forward, "L", for left, "R" for right, "B" for backward, and "Q" for quit.· Each character in the Directions DATA directive has a number of pulses stored in the Distances DATA directive.· The first "F" is delivered for 70 pulses, then the "L" delivers 53 pulses, and so on.· Since these values are stored in the BASIC Stamp® Microcontroller's EEPROM, the main routine must use READ commands to retrieve these instructions.
·
There are two READ commands in the main routine that fetch the instructions from the BASIC Stamp's EEPROM.· The first fetches the direction from the Directions DATA, and the second fetches the number of pulses from the Distances DATA.· The first time through the main routine, index starts at 0, so first item in each of the two DATA directives is fetched.· Then, the value of index gets one added to it.· The next time through the main routine, index will be 1, so the second item from each DATA directive is fetched.· The third time through, index is 2, so the third item in the DATA directive is fetched, and so on.
·
The ramping occurs in a FOR...NEXT loop that repeats pulseCount times.· The code blocks in a SELECT...CASE statement set the target full speed values of the pulseLeft and pulseRight variables.· Next, a series of IF...THEN statements compare the target values to the previous values of pulseLeft and pulseRight.· These values were stored as pulseLeftOld and pulseRightOld near the end of the previous pass through the FOR...NEXT loop.·
·
The IF...THEN statements make sure that the pulse durations only increase by increments of 2.· They do it by comparing pulseLeft and pulseRight to pulseLeftOld and pulseRightOld.· Essentailly, The IF...THEN statements make sure that the pulse that gets delivered this time through the FOR...NEXT loop is only 2 more (or less) than the pulse that was delivered previously.· MAX and MIN operators make sure that the pulse durations never increase above 850 of drop below 650.
Your Turn - Changing the Ramping Increments
Especially if the same value appears in several places in a program, it's a good idea to replace it with a constant.·
·
√······· Try replacing all the 2s in the IF...THEN statements with the name rampIncrement.·
√······· Then, at the beginning of the program, add a constants section with this declaration:
·
···· rampIncrement· CON· 2
·
This will make it a lot easier to experiment with different values for rampIncrement.
·
√······· Try changing the values in the IF...THEN statements that set the pulse durations from 2 to 3.· Also, try values like 1, 4, 5, and 6, 7, 8, and 10, and observe how the navigation changes.·
√······· After each change to the pulse increment value, you will also have to update the values in the Durations DATA directive to get back to a good rectangular path for your Boe-Bot.
Ramping on a Ramp
UphillBoeBot.bs2 is the program that performs the hill climbing shown in Figure 1.· This program is different from EepromNavigationWithRamping.bs2 in several ways.· First, instead of instructions stored in EEPROM, the program makes all its navigation decisions based on accelerometer measurements.· It uses IF...THEN statements do decided whether it needs to rotate to correct sideways (x-axis) tilt or forward or backward to respond to uphill/downhill (y-axis) tilt.· Ramping, is used to smooth out the Boe-Bot's changes in direction and starts and stops so that it doesn't get stuck reacting and overreacting to the incorrect readings that result from sudden starts stops, and turns.
Example Program - UphillBoeBot.bs2
√······· Find a convenient platform for controlling the slope your Boe-Bot is on.
√······· Enter, save, and run UphillBoeBot.bs2
√······· Use the platform's tilt to guide your Boe-Bot.· The program will make it always try to climb uphill.· As you tilt the platform and cause uphill to be a different direction, detect this change and adjust to a new heading up the steepest path on the slope.·
·
'
[noparse][[/noparse] Title ]
' UphillBoeBot.bs2
' Boe-Bot detects and climbs the steepest slope it can find.
·
'{$STAMP BS2}
'{$PBASIC 2.5}
·
'
[noparse][[/noparse] Variables ]
·
x············· VAR···· Word················· ' Left/right tilt
y············· VAR···· Word··· ··············' Uphill/downhill tilt
·
pulseLeft····· VAR···· Word················· ' Current left pulse value
pulseLeftOld·· VAR···· Word················· ' Previous left pulse value
·
pulseRight···· VAR···· Word················· ' Current right pulse value
pulseRightOld· VAR···· Word················· ' Previous right pulse value
·
'
[noparse][[/noparse] Initialization ]
·
FREQOUT 4, 2000, 3000······ ·················' Beep to signify program start
·
pulseLeft···· = 750·· ·······················' Start with all pulses centered
pulseRight··· = 750
pulseLeftOld· = 750
pulseRightOld = 750
·
'
[noparse][[/noparse] Main Routine ]
·
DO·········································· ' Begin main routine.
·
· PULSIN 6, 1, x···························· ' Get tilt pulses.
· PULSIN 7, 1, y····························
·
· x = (x MIN 1875 MAX 3125) - 2500·········· ' -625 to 625 with 0 = no tilt
· y = (y MIN 1875 MAX 3125) - 2500
·
· ' Navigation decisions
· ' Remember, the x measurement indicates that the Boe-Bot is tilted left when
· ' it's negative, and right when it's positive.· The y measurement indicates
· ' that the Boe-Bot is tilted downhill when it's negative and uphill when
· ' it's positive. ··
· IF ABS(x) > 40 THEN······················· ' x < 40 --> turn right,
··· pulseLeft = 750 - x····················· ' x > 40 --> turn left
··· pulseRight = 750 - x
· ELSEIF ABS(y) > 40 THEN··················· ' y < 40 --> forward,
··· pulseLeft = 750 - y····················· ' y > 40 --> backward
··· pulseRight = 750 + y
· ELSE······································ ' Stay still
··· pulseLeft = 750
··· pulseRight = 750
· ENDIF
·
· ' Increment/decrement routine only adjusts pulse durations by 8 at a time.
· IF pulseLeft· > (pulseLeftOld· + 8) THEN pulseleft· = pulseLeftOld ·+ 8
· IF pulseLeft· < (pulseLeftOld· - 8) THEN pulseLeft· = pulseLeftOld ·- 8
· IF pulseRight > (pulseRightOld + 8) THEN pulseRight = pulseRightOld + 8
· IF pulseRight < (pulseRightOld - 8) THEN pulseRight = pulseRightOld - 8
·
· pulseLeft· = pulseLeft· MIN 650 MAX 850··· ' Keep 650 <= durations <= 850
· pulseRight = pulseRight MIN 650 MAX 850
·
· pulseLeftOld· = pulseLeft················· ' Remember old values
· pulseRightOld = pulseRight
·
· PAUSE 20
· PULSOUT 13, pulseLeft····················· ' Send control pulses to servos
· PULSOUT 12, pulseRight
·
LOOP········································ ' Repeat main routine.
How HillClimbingBoeBot.bs2 Works
The first step for incline based navigation is to get the x and y-axis tilt measurements from the accelerometer.
·
· PULSIN 6, 1, x·········
· PULSIN 7, 1, y·········
·
Next, the tilt measurements are adjusted for a scale of -625 to 625, where 0 is approximately level.· The result is that the x-axis measurement is negative when the Boe-Bot is tilted to the left, and positive when it's tilted to the right.· Likewise, the y-axis measurement is negative when the Boe-Bot is pointing uphill and positive when it's pointing downhill.
·
· x = (x MIN 1875 MAX 3125) - 2500······
· y = (y MIN 1875 MAX 3125) - 2500
·
Since zero is level on the x-axis, anything greater than or less than zero indicates some level of tilt.· The statement IF ABS(x) > 40 THEN... translates to "if the absolute value of x is greater than 40, then...".· That means that anything greater than 40 or less than -40 will make the first IF...THEN statement true.· The code block inside that IF...THEN statement simply subtracts the x measurement from both the pulseLeft and the pulseRight variables.· That's because when the Boe-Bot is tilting left, x will be negative.· A negative value subtracted from something is the same as adding that value, so x is actually added to 750 for both pulseLeft and pulseRight.· For example, if x is -100, then both pulseLeft and pulseRight will be set to 850, which are the pulse durations for full speed rotate-right.· The net result is a right turn, to point the front of the Boe-Bot uphill.· The same applies when x is greater than 40.· The Boe-Bot is tilting right, and both pulse values are 650, which causes the Boe-Bot to execute a left turn, again to point it uphill.
·
· IF ABS(x) > 40 THEN······················· ' x < 40 --> turn right,
··· pulseLeft = 750 - x····················· ' x > 40 --> turn left
··· pulseRight = 750 - x
·
The same reasoning applies for the y-axis (uphill/downhill) measurement.· If the Boe-Bot is tilted so that y is more than 40 or less than -40, the Boe-Bot must be pointing downhill or uphill.· Let's say that the Boe-Bot is pointing uphill, and the y-axis measurement happens to be -100.· That makes pulseLeft 850 and pulseRight 650, which means full speed ahead.· If y happens to be 100, that would make pulseLeft 650 and pulseRight 850, which would cause the Boe-Bot to go full speed in reverse.
·
· ELSEIF ABS(y) > 40 THEN··················· ' y < 40 --> forward,
··· pulseLeft = 750 - y····················· ' y > 40 --> backward
··· pulseRight = 750 + y
·
The ELSE condition is when neither the x nor y-axis absolute value measurements are greater than 40.· In other words, the surface must be pretty level, and the Boe-Bot can just sit still.
·
· ELSE······································ ' Stay still
··· pulseLeft = 750
··· pulseRight = 750
· ENDIF
·
These IF...THEN statements change the value of pulseLeft and pulseRight so that they only increase or decrease by 8.· This ensures that the servo speeds change gradually, either increasing or decreasing by 8 each time through the loop.
·
· ' Increment/decrement routine only adjusts pulse durations by 8 at a time.
· IF pulseLeft· > (pulseLeftOld· + 8) THEN pulseleft· = pulseLeftOld
· IF pulseLeft· < (pulseLeftOld· - 8) THEN pulseLeft· = pulseLeftOld
· IF pulseRight > (pulseRightOld + 8) THEN pulseRight = pulseRightOld
· IF pulseRight < (pulseRightOld - 8) THEN pulseRight = pulseRightOld
·
The pulse durations to the servos should range from 650 for full speed clockwise to 850 for full speed counterclockwise.· The center pulse duration of 750, of course, keeps the centered servos at a standstill.· Thus far in the program, there's nothing to prevent 8 to be added to either pulseLeft or pulseRight even when it's already larger than 850.· There's also nothing preventing 8 from being subtracted from one or both of the variables when they are already 650 or less.· These two lines take care of that problem by setting the minimum values for both pulseLeft and pulseRight at 650, and the maximum at 850.·
·
· pulseLeft· = pulseLeft· MIN 650 MAX 850··· ' Keep 650 <= durations <= 850
· pulseRight = pulseRight MIN 650 MAX 850
·
The next time through the main routine's DO...LOOP, pulseLeft and pulseRight will have new values because of new tilt measurements. ·However, the previous values of pulseLeft and pulseRight have to be remembered so that 8 can either be added or subtracted to or from each.· These two lines store the values of pulseLeft into pulseLeftOld, and pulseRight into pulseRightOld so that the current values are remembered the next time through the loop.
·
· pulseLeftOld· = pulseLeft···········
Let's say your Boe-Bot is attempting to navigate terrain with hills and valleys.· Depending on how steep the hills are, the Boe-Bot may need to detect whether or not it is about to roll back down the hill.· As part of a contest, the Boe-Bot may need to find the top of the hill (or the bottom of a valley) faster than its competitors.· The Memsic MX2125 Dual-Axis Accelerometer makes all this and more possible, and this activity will show you how.
+ Download zipped video clip (765 KB) - HillClimbingBoeBot.wmv·
·
The ability to sense incline and make navigation decisions is also especially important when the Boe-Bot is equipped with the Boe-Bot Tank Tread Kit.· The Boe-Bot Tank Tread Kit Documentation (.pdf) has example with an accelerometer added to the IR roaming application from Robotics with the Boe-Bot to keep it from trying to climb an obstacle that are too steep and might cause it to flip.·
·
Parts and Equipment
With the exception of the Memsic MX2125 dual-axis accelerometer, all the rest of the parts are already included in the Boe-Bot™ Robot kit.
·
(1) Boe-Bot – fully assembled and tested
(1) Memsic 2125 Dual-axis Accelerometer
(2) Resistors – 220 Ω (Red Red Brown)
(4) Jumper Wires
(1) Piezospeaker
Circuits
Figure 3 shows the schematic and wiring diagram for the Boe-Bot with the accelerometer circuit.· The servo and piezospeaker connections are from Robotics with the Boe-Bot.· The left servo is connected to P13, the right to P12, and the piezospeaker to P4.· The connections for the accelerometer are featured in various Stamps in Class Forum accelerometer posts.
·
√······· Build the circuit shown in Figure 3.
·
Tilt Axes on the Boe-Bot
It's important to get a feel for how the Boe-Bot's forward/backward and left/right tilt axes relate to the accelerometer's X and Y axes.
·
√······· Enter, save, and run BoeBotTiltTest.bs2, and keep an eye on the Debug Terminal as you tilt your Boe-Bot.
√······· Verify that the piezospeaker beeped for two seconds before the program continued.· If not double-check your program and wiring.
√······· Try tilting your Boe-Bot to the left and right.· Verify that the x-axis values range from -625 to 625 and that they are negative when you tilt the Boe-Bot to the left and positive when you tilt it to the right.
√······· Next tilt the Boe-Bot as though it is driving downhill, then uphill.· Verify that the y-axis values range from -625 to 625 and that they are positive when the Boe-Bot is tilted downhill and negative when the Boe-Bot is tilted uphill.
Example Program – Boe-BotTiltTest.bs2
·
' BoeBotTiltTest.bs2
' Test accelerometer readings.
·
'{$STAMP BS2}
'{$PBASIC 2.5}
·
x············· VAR···· Word················· ' Left/right tilt
y ·············VAR···· Word················· ' Forward/backward tilt
·
DEBUG "Beep!!!"····························· ' Test piezospeaker
FREQOUT 4, 2000, 3000
DEBUG CLS, "· x···· y", CR·················· ' Axis headings
·
DO···································· ······' Begin main routine
·
· PULSIN 6, 1, x···························· ' Get X-axis tilt
· PULSIN 7, 1, y···························· ' Get Y-axis tilt
·
· ' Instead of measurements that range from 1875 to 3125 with 2500 = no tilt,
· ' These two statements shift the tilt measurements to range from -625 to 625
· ' with 0 = no tilt.
· x = (x MIN 1875 MAX 3125) - 2500
· y = (y MIN 1875 MAX 3125) - 2500
·
· ' Display x and y tilt measurements.
· DEBUG CRSRX, 0, "(", SDEC3 x, ", ", SDEC3 y, ")", CLREOL
·
· PAUSE 100································· ' Pause 1/10 s
·
LOOP········································ ' Repeat main routine
Ramping in Navigation
Ramping is a common term for making the servos speed up and slow down gradually, no sudden starts, stops or direction changes.· For hill detection and climbing, it's crucial to have a reliable ramping routine for Boe-Bot navigation.· Without it, the Boe-Bot's abrupt motions cause acceleration measurements to get mixed in with the tilt measurements.· This in turn gives the Boe-Bot a false reading of the hill's steepness.·
·
The consequences of these false measurements can be pretty disabling for a hill climbing Boe-Bot.· Depending on how the program is designed, the Boe-Bot can actually move one direction, get a wrong tilt measurement, and try to correct with a sudden change of motion.· This in turn leads to another wrong tilt measurement, in the opposite direction, which the Boe-Bot again tries yet again to correct with another abrupt change in motion.· In some cases, the Boe-Bot can even end up on a slope paralyzed, just sitting in one place jittering.
EepromNavigationWithRamping.bs2
This next example demonstrates how ramping combined with EEPROM navigation smoothes out the Boe-Bot's transitions between maneuvers.·
·
√······· Enter, save, and run EepromNavigationWithRamping.bs2
√······· Verify that the Boe-Bot starts out by moving forward.· Depending on your servos, it may curve to the left or the right a little, which is okay.·
·
·
√······· Fine tune the distances so that they make your Boe-Bot travel in a rectangle.· The turn at the first corner should be left, 90°.· The second should be right, 270°, and the third should be another left at 90°.
·
'
[noparse][[/noparse] Title ]
' EepromNavigationWithRamping.bs2
' Demonstrate how ramping routine smoothes out EEPROM navigation.
·
'{$STAMP BS2}
'{$PBASIC 2.5}
·
'
[noparse][[/noparse] EEPROM Data ]
' Two DATA directives store the following navigation instructions:
' Forward, left 90-degrees, forward, right 270-degrees forward,
' left 90-degrees, forward, stop.
Directions DATA "F", "L", "F", "R", "F", "L", "F", "Q"
Distances· DATA· 70,· 53,· 80,· 77,· 90,· 53,· 80,· 0
·
'
[noparse][[/noparse] Variables ]
·
counter······· VAR···· Byte················· ' FOR...NEXT loop counter
index········· VAR···· Byte················· ' Points to EEPROM instructions
instruction··· VAR···· Byte················· ' Stores navigation instruction
pulseCount···· VAR···· Byte················· ' Stores navigation distance
·
pulseLeft····· VAR···· Word················· ' Stores pulse to left servo
pulseLeftOld·· VAR···· Word················· ' Previous pulse to left servo
·
pulseRight···· VAR···· Word················· ' Stores pulse to right servo
pulseRightOld· VAR···· Word················· ' Previous pulse to right servo
·
'
[noparse][[/noparse] Initialization ]
·
FREQOUT 4, 2000, 3000······················· ' Beep to signify program start
·
pulseLeft···· = 750························· ' Set all pulse values to center
pulseRight··· = 750
pulseLeftOld· = 750
pulseRightOld = 750
·
'
[noparse][[/noparse] Main Routine ]
·
DO·········································· ' Begin main routine
·
· ' Fetch EEPROM instruction.
· READ index + Directions, instruction······ ' Get navigation character
· READ index + Distances, pulseCount········ ' Get number of pulses
· index = index + 1
·
· ' This FOR...NEXT loop includes a navigation routine with ramping.
· FOR counter = 1 TO pulseCount············· ' Repeat for number of pulses
·
··· ' Navigation routine sets pulse width based on navigation character.
··· SELECT instruction
····· CASE "F"······························ ' Pulse durations for forward
······· pulseLeft = 850·····················
······· pulseRight = 650
····· CASE "L"······························ ' Pulse durations for left
······· pulseleft = 650·····················
······· pulseRight = 650
····· CASE "R"······························ ' Pulse durations for Right
······· pulseLeft = 850
······· pulseRight = 850
····· CASE "B"······························ ' Pulse durations for backward
······· pulseLeft = 650
······· pulseRight = 850
····· CASE "Q"······························ ' End the program
······· END
··· ENDSELECT
·
··· ' Increment/decrement routine only changes pulse durations by 2 at a time.
··· IF pulseLeft > (pulseLeftOld· + 2) THEN pulseleft· = pulseLeftOld· + 2
··· IF pulseLeft· < (pulseLeftOld· - 2) THEN pulseLeft· = pulseLeftOld· - 2
··· IF pulseRight > (pulseRightOld + 2) THEN pulseRight = pulseRightOld + 2
··· IF pulseRight < (pulseRightOld - 2) THEN pulseRight = pulseRightOld - 2
·
··· pulseLeft· = pulseLeft· MIN 650 MAX 850· ' Keep 650 <= durations <= 850
··· pulseRight = pulseRight MIN 650 MAX 850
·
··· pulseLeftOld· = pulseLeft··············· ' Remember old values
··· pulseRightOld = pulseRight
·
··· PULSOUT 13, pulseLeft··················· ' Send control pulses to servos
··· PULSOUT 12, pulseRight
··· PAUSE 20
·
· NEXT······································ ' Check FOR counter... condition
·
LOOP········································ ' Repeat main routine
How EepromNavigationWithRamping.bs2 Works
The Directions DATA directive stores a series of instructions that include "F" for forward, "L", for left, "R" for right, "B" for backward, and "Q" for quit.· Each character in the Directions DATA directive has a number of pulses stored in the Distances DATA directive.· The first "F" is delivered for 70 pulses, then the "L" delivers 53 pulses, and so on.· Since these values are stored in the BASIC Stamp® Microcontroller's EEPROM, the main routine must use READ commands to retrieve these instructions.
·
There are two READ commands in the main routine that fetch the instructions from the BASIC Stamp's EEPROM.· The first fetches the direction from the Directions DATA, and the second fetches the number of pulses from the Distances DATA.· The first time through the main routine, index starts at 0, so first item in each of the two DATA directives is fetched.· Then, the value of index gets one added to it.· The next time through the main routine, index will be 1, so the second item from each DATA directive is fetched.· The third time through, index is 2, so the third item in the DATA directive is fetched, and so on.
·
The ramping occurs in a FOR...NEXT loop that repeats pulseCount times.· The code blocks in a SELECT...CASE statement set the target full speed values of the pulseLeft and pulseRight variables.· Next, a series of IF...THEN statements compare the target values to the previous values of pulseLeft and pulseRight.· These values were stored as pulseLeftOld and pulseRightOld near the end of the previous pass through the FOR...NEXT loop.·
·
The IF...THEN statements make sure that the pulse durations only increase by increments of 2.· They do it by comparing pulseLeft and pulseRight to pulseLeftOld and pulseRightOld.· Essentailly, The IF...THEN statements make sure that the pulse that gets delivered this time through the FOR...NEXT loop is only 2 more (or less) than the pulse that was delivered previously.· MAX and MIN operators make sure that the pulse durations never increase above 850 of drop below 650.
Your Turn - Changing the Ramping Increments
Especially if the same value appears in several places in a program, it's a good idea to replace it with a constant.·
·
√······· Try replacing all the 2s in the IF...THEN statements with the name rampIncrement.·
√······· Then, at the beginning of the program, add a constants section with this declaration:
·
···· rampIncrement· CON· 2
·
This will make it a lot easier to experiment with different values for rampIncrement.
·
√······· Try changing the values in the IF...THEN statements that set the pulse durations from 2 to 3.· Also, try values like 1, 4, 5, and 6, 7, 8, and 10, and observe how the navigation changes.·
√······· After each change to the pulse increment value, you will also have to update the values in the Durations DATA directive to get back to a good rectangular path for your Boe-Bot.
Ramping on a Ramp
UphillBoeBot.bs2 is the program that performs the hill climbing shown in Figure 1.· This program is different from EepromNavigationWithRamping.bs2 in several ways.· First, instead of instructions stored in EEPROM, the program makes all its navigation decisions based on accelerometer measurements.· It uses IF...THEN statements do decided whether it needs to rotate to correct sideways (x-axis) tilt or forward or backward to respond to uphill/downhill (y-axis) tilt.· Ramping, is used to smooth out the Boe-Bot's changes in direction and starts and stops so that it doesn't get stuck reacting and overreacting to the incorrect readings that result from sudden starts stops, and turns.
Example Program - UphillBoeBot.bs2
√······· Find a convenient platform for controlling the slope your Boe-Bot is on.
√······· Enter, save, and run UphillBoeBot.bs2
√······· Use the platform's tilt to guide your Boe-Bot.· The program will make it always try to climb uphill.· As you tilt the platform and cause uphill to be a different direction, detect this change and adjust to a new heading up the steepest path on the slope.·
·
'
[noparse][[/noparse] Title ]
' UphillBoeBot.bs2
' Boe-Bot detects and climbs the steepest slope it can find.
·
'{$STAMP BS2}
'{$PBASIC 2.5}
·
'
[noparse][[/noparse] Variables ]
·
x············· VAR···· Word················· ' Left/right tilt
y············· VAR···· Word··· ··············' Uphill/downhill tilt
·
pulseLeft····· VAR···· Word················· ' Current left pulse value
pulseLeftOld·· VAR···· Word················· ' Previous left pulse value
·
pulseRight···· VAR···· Word················· ' Current right pulse value
pulseRightOld· VAR···· Word················· ' Previous right pulse value
·
'
[noparse][[/noparse] Initialization ]
·
FREQOUT 4, 2000, 3000······ ·················' Beep to signify program start
·
pulseLeft···· = 750·· ·······················' Start with all pulses centered
pulseRight··· = 750
pulseLeftOld· = 750
pulseRightOld = 750
·
'
[noparse][[/noparse] Main Routine ]
·
DO·········································· ' Begin main routine.
·
· PULSIN 6, 1, x···························· ' Get tilt pulses.
· PULSIN 7, 1, y····························
·
· x = (x MIN 1875 MAX 3125) - 2500·········· ' -625 to 625 with 0 = no tilt
· y = (y MIN 1875 MAX 3125) - 2500
·
· ' Navigation decisions
· ' Remember, the x measurement indicates that the Boe-Bot is tilted left when
· ' it's negative, and right when it's positive.· The y measurement indicates
· ' that the Boe-Bot is tilted downhill when it's negative and uphill when
· ' it's positive. ··
· IF ABS(x) > 40 THEN······················· ' x < 40 --> turn right,
··· pulseLeft = 750 - x····················· ' x > 40 --> turn left
··· pulseRight = 750 - x
· ELSEIF ABS(y) > 40 THEN··················· ' y < 40 --> forward,
··· pulseLeft = 750 - y····················· ' y > 40 --> backward
··· pulseRight = 750 + y
· ELSE······································ ' Stay still
··· pulseLeft = 750
··· pulseRight = 750
· ENDIF
·
· ' Increment/decrement routine only adjusts pulse durations by 8 at a time.
· IF pulseLeft· > (pulseLeftOld· + 8) THEN pulseleft· = pulseLeftOld ·+ 8
· IF pulseLeft· < (pulseLeftOld· - 8) THEN pulseLeft· = pulseLeftOld ·- 8
· IF pulseRight > (pulseRightOld + 8) THEN pulseRight = pulseRightOld + 8
· IF pulseRight < (pulseRightOld - 8) THEN pulseRight = pulseRightOld - 8
·
· pulseLeft· = pulseLeft· MIN 650 MAX 850··· ' Keep 650 <= durations <= 850
· pulseRight = pulseRight MIN 650 MAX 850
·
· pulseLeftOld· = pulseLeft················· ' Remember old values
· pulseRightOld = pulseRight
·
· PAUSE 20
· PULSOUT 13, pulseLeft····················· ' Send control pulses to servos
· PULSOUT 12, pulseRight
·
LOOP········································ ' Repeat main routine.
How HillClimbingBoeBot.bs2 Works
The first step for incline based navigation is to get the x and y-axis tilt measurements from the accelerometer.
·
· PULSIN 6, 1, x·········
· PULSIN 7, 1, y·········
·
Next, the tilt measurements are adjusted for a scale of -625 to 625, where 0 is approximately level.· The result is that the x-axis measurement is negative when the Boe-Bot is tilted to the left, and positive when it's tilted to the right.· Likewise, the y-axis measurement is negative when the Boe-Bot is pointing uphill and positive when it's pointing downhill.
·
· x = (x MIN 1875 MAX 3125) - 2500······
· y = (y MIN 1875 MAX 3125) - 2500
·
Since zero is level on the x-axis, anything greater than or less than zero indicates some level of tilt.· The statement IF ABS(x) > 40 THEN... translates to "if the absolute value of x is greater than 40, then...".· That means that anything greater than 40 or less than -40 will make the first IF...THEN statement true.· The code block inside that IF...THEN statement simply subtracts the x measurement from both the pulseLeft and the pulseRight variables.· That's because when the Boe-Bot is tilting left, x will be negative.· A negative value subtracted from something is the same as adding that value, so x is actually added to 750 for both pulseLeft and pulseRight.· For example, if x is -100, then both pulseLeft and pulseRight will be set to 850, which are the pulse durations for full speed rotate-right.· The net result is a right turn, to point the front of the Boe-Bot uphill.· The same applies when x is greater than 40.· The Boe-Bot is tilting right, and both pulse values are 650, which causes the Boe-Bot to execute a left turn, again to point it uphill.
·
· IF ABS(x) > 40 THEN······················· ' x < 40 --> turn right,
··· pulseLeft = 750 - x····················· ' x > 40 --> turn left
··· pulseRight = 750 - x
·
The same reasoning applies for the y-axis (uphill/downhill) measurement.· If the Boe-Bot is tilted so that y is more than 40 or less than -40, the Boe-Bot must be pointing downhill or uphill.· Let's say that the Boe-Bot is pointing uphill, and the y-axis measurement happens to be -100.· That makes pulseLeft 850 and pulseRight 650, which means full speed ahead.· If y happens to be 100, that would make pulseLeft 650 and pulseRight 850, which would cause the Boe-Bot to go full speed in reverse.
·
· ELSEIF ABS(y) > 40 THEN··················· ' y < 40 --> forward,
··· pulseLeft = 750 - y····················· ' y > 40 --> backward
··· pulseRight = 750 + y
·
The ELSE condition is when neither the x nor y-axis absolute value measurements are greater than 40.· In other words, the surface must be pretty level, and the Boe-Bot can just sit still.
·
· ELSE······································ ' Stay still
··· pulseLeft = 750
··· pulseRight = 750
· ENDIF
·
These IF...THEN statements change the value of pulseLeft and pulseRight so that they only increase or decrease by 8.· This ensures that the servo speeds change gradually, either increasing or decreasing by 8 each time through the loop.
·
· ' Increment/decrement routine only adjusts pulse durations by 8 at a time.
· IF pulseLeft· > (pulseLeftOld· + 8) THEN pulseleft· = pulseLeftOld
· IF pulseLeft· < (pulseLeftOld· - 8) THEN pulseLeft· = pulseLeftOld
· IF pulseRight > (pulseRightOld + 8) THEN pulseRight = pulseRightOld
· IF pulseRight < (pulseRightOld - 8) THEN pulseRight = pulseRightOld
·
The pulse durations to the servos should range from 650 for full speed clockwise to 850 for full speed counterclockwise.· The center pulse duration of 750, of course, keeps the centered servos at a standstill.· Thus far in the program, there's nothing to prevent 8 to be added to either pulseLeft or pulseRight even when it's already larger than 850.· There's also nothing preventing 8 from being subtracted from one or both of the variables when they are already 650 or less.· These two lines take care of that problem by setting the minimum values for both pulseLeft and pulseRight at 650, and the maximum at 850.·
·
· pulseLeft· = pulseLeft· MIN 650 MAX 850··· ' Keep 650 <= durations <= 850
· pulseRight = pulseRight MIN 650 MAX 850
·
The next time through the main routine's DO...LOOP, pulseLeft and pulseRight will have new values because of new tilt measurements. ·However, the previous values of pulseLeft and pulseRight have to be remembered so that 8 can either be added or subtracted to or from each.· These two lines store the values of pulseLeft into pulseLeftOld, and pulseRight into pulseRightOld so that the current values are remembered the next time through the loop.
·
· pulseLeftOld· = pulseLeft···········
634 x 323 - 136K
652 x 339 - 47K
249 x 187 - 44K
675 x 367 - 93K
450 x 231 - 28K
695 x 80 - 13K
672 x 361 - 39K
695 x 96 - 15K
Comments
Actually, maybe not, the·Stamps in Class Forum said this:·
Post Edited (Andy Lindsay (Parallax)) : 1/17/2005 7:01:13 AM GMT
I've got a question about this project. Won't the acceleration or deceleration of the boe-bot affect the readings of the accelerometer?
Thank you
Kevin
Yes, that's why the code has speed ramping, to gradually accelerate into a given maneuver.·
If the Boe-Bot were to make a sudden change in speed, it would result in a sudden large measurement, which would return in a sudden counteraction by the code.· It results in a unproductive backward/forward dance because the Boe-Bot thinks it's going up hill, then down hill, then up hill, etc...
Andy
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Andy Lindsay
Education Department
Parallax, Inc.