Ramping in Navigation
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.
EepromNavigationWithRamping.bs2
This next example demonstrates how ramping combined with EEPROM navigation smooths 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.
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.
- 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°.
' -----[ 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
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 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.
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 2’s 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 from 2 to 3 in the IF...THEN statements that set the pulse durations. 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 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.
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 in a different direction, the Boe-Bot detects this change and adjusts to a new heading up the steepest path on the slope.
' -----[ 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.
How UphillBoeBot.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
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
Your Turn – Incline Sensitivity and Response Speed
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.
- Experiment with the ABS and increment/decrement values to get your Boe-Bot to perform the best it can on hills.