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.
Figure 1 - Boe-Bot Performing Hill-Climbing Behavior
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 (#28106). The Boe-Bot Tank Tread Kit Documentation has an 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 is too steep and might cause it to flip. The Tank Tread Kit modification is shown in Figure 2.
Figure 2 - Boe-Bot with Tank Tread Modification
If you are new to the BASIC Stamp microcontroller or to programming, it would be a good idea to review Chapters 1-4 in Robotics with the Boe-Bot (#28125).
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) Fully assembled and tested Boe-Bot Robot (#28132)
(1) Memsic 2125 Dual-axis Accelerometer (#28017)
(2) 220 Ω resistors (#150-02210)
(1) Piezospeaker (#900-00001)
Source Code for Boe-Bot Navigation with Accelerometer Incline Sensing [1]
The image below shows the schematic for the Boe-Bot with the accelerometer circuit. The servo and piezo speaker connections are from Robotics with the Boe-Bot (#28125). The connections for the accelerometer are featured in Chapter 3 of Smart Sensors and Applications.
Figure 3 - Schematic for Boe-Bot with Accelerometer
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.
' 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 is a common term for making the servos speed up and slow down gradually, with 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 could even end up on a slope, paralyzed, sitting in one place and jittering.
This next example demonstrates how ramping combined with EEPROM navigation smooths out the Boe-Bot's transitions between maneuvers.
If the Boe-Bot starts out going backward instead of forward
Your servo cables may need to be swapped. The cable connected to the Boe-Bot's left servo should be connected to the P13 servo port on the Board of Education. The cable connected to the right servo should be connected to the P12 port.
' -----[ Title ]-------------------------------------------------------------- ' EepromNavigationWithRamping.bs2 ' Demonstrate how ramping routine smoothes out EEPROM navigation. '{$STAMP BS2} '{$PBASIC 2.5} ' -----[ 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 ' -----[ 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 ' -----[ Initialization ]----------------------------------------------------- FREQOUT 4, 2000, 3000 ' Beep to signify program start pulseLeft = 750 ' Set all pulse values to center pulseRight = 750 pulseLeftOld = 750 pulseRightOld = 750 ' -----[ 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
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 the first item in each of the two DATA directives is fetched. Then, the value of index gets 1 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. Essentially, 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 or drop below 650.
Especially if the same value appears in several places in a program, it’s a good idea to replace it with a constant.
rampIncrement CON 2
This will make it a lot easier to experiment with different values for rampIncrement.
UphillBoeBot.bs2 is the program that performs the hill climbing shown in Figure 8‑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 to decide whether it needs to rotate to correct sideways (x-axis) tilt, or go forward or backward to respond to uphill/downhill (y-axis) tilt. Ramping is used to smooth out the Boe-Bot's changes in direction, 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.
' -----[ Title ]-------------------------------------------------------------- ' UphillBoeBot.bs2 ' Boe-Bot detects and climbs the steepest slope it can find. '{$STAMP BS2} '{$PBASIC 2.5} ' -----[ 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 ' -----[ Initialization ]----------------------------------------------------- FREQOUT 4, 2000, 3000 ' Beep to signify program start pulseLeft = 750 ' Start with all pulses centered pulseRight = 750 pulseLeftOld = 750 pulseRightOld = 750 ' -----[ 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.
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
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.
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 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 to r subtracted 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 pulseRightOld = pulseRight
Here are the familiar servo pulses. After these, the LOOP command sends the program back up to DO, and the main routine repeats itself.
PULSOUT 13, pulseLeft PULSOUT 12, pulseRight PAUSE 20
You can make the Boe-Bot more or less sensitive to tilt by modifying the IF...ELSEIF...ELSE...ENDIF statement. For example, IF ABS(y) > 60 will make it so that you have to tilt the Boe-Bot higher before it starts to move. Changing the value of 8 in the IF...THEN statements that do the increment/decrement job will cause the Boe-Bot to ramp quicker or more slowly.
Links
[1] https://learn.parallax.com/sites/default/files/Files/Docs/Projects/BoeBotNav/Boe-Bot-Nav-with-Incline.zip