Boe-Bot + Ping))) Rangefinder - Scan for and Go to Closest Object
Andy Lindsay (Parallax)
Posts: 1,919
Scan For and Go To Closest Object
·
In Ping)))Dar – A Radar Style Display, we used the debug terminal to get an idea of what objects look like when the Boe-Bot scans them with the Ping))) Ultrasonic Rangefinder and Mounting Bracket kits. In this activity, we'll use the same setup along with a program that makes the Boe-Bot scan a field of objects and go to the closest one. Figure 1 shows the Boe-Bot's “scan and go to” sequence as it runs GotoClosestObject.bs2. Here's what it does:
·
√······· (The Boe-Bot pauses for 2 seconds after the RESET button on the Board of Education is pressed and released.
√······· The Boe-Bot directs the Ping))) Ultrasonic Rangefinder to the right (0°).
√······· It scans from right to left (180°) while taking successive distance measurements and saving the angle and distance of the closest measurement it found.
√······· The Boe-Bot then points the Ping))) rangefinder in the direction of its closest measurement.
√······· The Ping))) is then swept to the right in smaller increments until the object's right edge is detected.
√······· The Ping))) is swept to the left (in the same small increments) until the object's left edge is detected.
√······· The Boe-Bot calculates the middle of the object and turns in place to face it.
√······· The Boe-Bot goes to the object.
View Video Introduction (YouTube)
Download Source Code – Scan For and Go To Closest Object
·
Figure 1 - Finding the Angle that Points Toward the Middle of the Object
Getting Started
This project contains advanced topics, not intended for Boe-Bot beginners.· Before continuing, it is highly recommended you complete the following tasks in order to gain a better understanding on how this application works:
·
√······· Complete all activities in What’s a Microcontroller
√······· Complete at minimum Chapters 1-4 in Robotics with the Boe-Bot
√······· Complete at minimum Chapters 1-3 in Smart Sensors and Applications
√······· Download the product documentation and assembly instructions for the PING))) Bracket Kit
√······· Complete the Ping)))Dar – A Radar Style Display forum post
Parts Required
(1) Fully assembled and tested Boe-Bot® Robot
(1) Ping))) Ultrasonic Rangefinder
(1) Ping))) Mounting Bracket Kit
Finding, Facing, and Going to the Closest Object
·
Finding the closest object is pretty simple. Start by initializing a word variable, named minDist in GotoClosestObject.bs2, to 65536. Then, repeatedly take distance measurements with the Ping))) rangefinder while sweeping the mounting bracket servo from right to left. Each time the current distance measurement is less than minDist, copy the current distance measurement to minDist, and also copy pingDir, which is the mounting bracket servo control pulse duration to a variable named markDir. When the sweep is done, minDist will store the closest distance, but more importantly, markDir will store the pulse duration that made the mounting bracket servo point the rangefinder at the closest object. As we saw in the previous activity, once the pulse duration to the mounting bracket servo is known, the angle the mounting bracket servo has pointed the rangefinder to can be calculated.
Single_Sweep:
·
· DO UNTIL pingDir >= LimitLeft
··· GOSUB Sweep_Increment
··· GOSUB Get_Ping_Cm
··· IF distance < minDist THEN
····· minDist = distance
····· markDir = pingDir
··· ENDIF
··LOOP
·
· RETURN
·
A quick sweep with fairly large servo increments may not be enough for a heading toward the middle of the closest object. Next, GotoClosestObject.bs2 points the rangefinder in the direction where it took its closest measurement. Then, it scans to the·right until the measured distance is more than 10 cm beyond than the closest distance measurement shown in Figure 3 (a). After that, it scans to the left until the measured distance is 10 cm beyond the closest distance shown in Figure 3 (b). The program marks both these servo pulse durations, and calculates the average of the two. This gives the Boe-Bot's BASIC Stamp a pulse duration to make the mounting bracket servo point the rangefinder to the object about half way between the two edges as shown in Figure 3 (c).
·
Figure 3 - Finding the Angle that Points Toward the Middle of the Object
Although the Ping))) Mounting Bracket servo is now pointing the Ping))) Ultrasonic Rangefinder directly at the object, it's not the right angle to help the Boe-Bot turn to face the object. Figure 4 shows why. Notice that θ1 (θping-object) is much less than θ2 (θBoe-Bot-object). So, before facing the object, the program has to figure out what θ2 is given θ1 and the object's distance (d), which is (d·
·θ1) in polar coordinates.
Figure 4 - PULSOUT Duration Argument vs. Rotation Angle
Ping)))Dar.bs2 demonstrated how to make polar to Cartesian coordinate conversions using x = d cos(θ[noparse];)[/noparse] and y = d sin(θ[noparse];)[/noparse]. Assuming the Boe-Bot's continuous rotation servos are mounted so that their output shafts are toward the back of the Boe-Bot, the offset between the front of the Ping))) rangefinder and the Boe-Bot's axis is about 7 cm. If they are mounted with their output shafts toward the front of the Boe-Bot, the offset is more like 5 cm. Assuming 7 cm, after the program calculates the x and y centimeter distances, all it has to do is add 7 cm to the y distance, and those are the x and y distances for θ2. Then, θ2 (θBoe-Bot-Object) can be calculated using the inverse tangent of the (y + 7 cm) ÷ x distances.
Once the program has figured out θ2, all it has to do to is face the object and make the continuous rotation servos cause the Boe-Bot to turn in place for that number of binary radians. One of the calibration programs in this activity involves figuring out the continuous rotation servo PULSOUT Duration arguments that will make the Boe-Bot turn 128 brads (180°) with 128 pulses. The constants that are determined will then be used in GotoClosestObject.bs2 to give the Boe-Bot turn-in-place control in terms of brads.
Software Continuous Rotation Servo Calibration
In Ping)))Dar, you adjusted the Ping))) Mounting Bracket servo's hardware to make sure it points straight forward when it receives PULSOUT 14, 750 signals. You also adjusted LimitLeft and LimitRight CON directives to make sure the mounting bracket servo scanned the Ping))) rangefinder across a 180° field in front of the Boe-Bot. There are two more sets of CON directives that have to be updated, and they will be used in PULSOUT commands that control the continuous rotation servos that turn the Boe-Bot's wheels. These CON directives are FwdLeftFast, FwdRightFast, RotateLeft, and RotateRight. FwdLeftFast and FwdRightFast are used in PULSOUT commands to make the Boe-Bot drive forward at top speed. They have to be adjusted so that they make the Boe-Bot roll straight forward (instead of a curved path that's mostly forward) to make sure the Boe-Bot can roll straight to the object. RotateLeft and RotateRight have to be adjusted so that they make the Boe-Bot turn 180° (128 brads) when 128 pulses are delivered. This adjustment will make it possible for GotoClosestObject.bs2 to turn the Boe-Bot to the correct angle so that it faces the closest object before driving up to it.
CON Directives for Adjusting Boe-Bot Maneuvers
Below are the two PULSOUT commands along with the PAUSE command to send signals to the Boe-Bot's continuous rotation servos that make them turn full speed forward. The number of times a loop repeats these signals dictates how long the servos will turn full speed forward. The speed at which each servo turns is controlled by the PULSOUT commands' Duration arguments, which are 850 and 650 in the example. Since one servo typically turns a little faster than the other at full speed, the 850 or 650 values have to be adjusted to equalize the speeds the wheels turn.
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
The CON directives below make these values adjustable, which is especially useful when they are used more than one place in the program. All you have to do is change a CON directive, and any PULSOUT command that uses the name for Duration argument will automatically be updated. For example, if you change the value of the FwdLeftFast CON directive from 850 to 820, the command PULSOUT BotServoLeft, FwdFastLeft will be changed from PULSOUT 13, 850 to PULSOUT 13, 820. The net effect will be that the left wheel will rotate slower when the program runs.
BotServoLeft ·PIN 13 ··················· ' Boe-Bot servo pins
BotServoRight PIN 12
.
.
.
Center ·······CON 750 ·················· ' Center servo pulse width
FwdLeftFast ··CON 850 ·················· ' Fast settings - straight ahead
FwdRightFast ·CON 650
RotateRight ··CON 758 ·················· ' Boe-Bot rotate right pulse
RotateLeft ···CON 729 ·················· ' Boe-Bot rotate left pulse
.
.
.
· PULSOUT BotServoLeft, RotateRight
· PULSOUT BotServoRight, RotateRight
· PAUSE 20
.
.
.
· PULSOUT BotServoLeft, FwdLeftFast
· PULSOUT BotServoRight, FwdRightFast
· PAUSE 20
.
.
.
Example Program − CalibrateStraightForward.bs2
In this calibration task, you have to observe the Boe-Bot's forward travel and determine if it is tending to curve to either the left or right. Then, adjust the FwdLeftFast or FwdRightFast CON directives to correct any tendency toward traveling in a curve. To make the correction, adjust the CON directive for the faster wheel to make its speed equal to that of the slower wheel.
·
√······· Run CalibrateStraightForward.bs2 and watch the Boe-Bot as it travels forward. Does it curve to the left, or right, or is its travel almost perfectly straight?
√······· If the Boe-Bot already goes in a straight line (almost no curving whatsoever), skip to the next section: Example Program - Calibrate180DegreeTurns.bs2.
√······· If the Boe-Bot curves to the left, the right wheel is going too fast. Try running the program with FwdRightFast set to 660. Still curving left? Try 670 to make it slower still. Still curving left? Try 680. Keep bringing the FwdRightFast value closer to 750 and running and testing the program until it either makes the Boe-Bot travel in a straight line, or it starts curving to the right instead. If the Boe-Bot starts to curve to the right, try a value between the current and previous FwdRightFast settings. Fine tune the value until it makes the Boe-Bot go in the straightest path possible.
√······· If the Boe-Bot curves to the right, the left wheel is going too fast. Try running the program with FwdLeftFast set to 840. Still curving right? Try 830 to make it slower still. Still curving right? Try 820. Keep bringing the FwdLeftFast value closer to 750 and running and testing the program until it either makes the Boe-Bot travel in a straight line, or it starts curving to the left instead. If the Boe-Bot starts to curve to the left, try a value between the current and previous FwdLeftFast settings. Fine tune the value until it makes the Boe-Bot go in the straightest path possible.
√······· Save your modified program and make a note of your FwdLeftFast and FwdRightFast CON directives so that you can update them in the upcoming navigation program, GotoClosestObject.bs2.
Example Program − Calibrate180DegreeTurns.bs2
This calibration task involves adjusting CON directives for PULSOUT command Duration arguments that will make the Boe-Bot rotate 128 brads (180°) when 128 pulses are sent to its continuous rotation drive servos. These constants will likely be different for left and right rotation, so Calibrate180DegreeTurns.bs2 has separate RotateRight and RotateLeft CON directives. By adjusting the values of the RotateRight and RotateLeft CON directives, you will adjust how far the Boe-Bot rotates left and right when it receives 128 pulses. The goal again is to adjust the constants until the program makes the Boe-Bot execute a 180° right turn that's as close to perfect as you can get it, followed by an equally perfect 180° left turn.
√······· Open and run Calibrate180DegreeTurns.bs2 and watch how far the Boe-Bot turns to the left and then to the right. The calibration goal is for it to turn 180° to the right, then 180° to the left.
√······· If the Boe-Bot turns more than 180° to the right, reduce the value of the RotateRight CON directive, and re-run the program. If it turns less than 180° to the right, increase the RotateRight CON directive. Repeat until you have tuned it to turn 180° to the right.
√······· For tuning left rotation, if the Boe-Bot turns too far, increase the RotateLeft CON directive, or if it doesn't turn far enough, decrease the RotateLeft CON directive.
√······· Keep adjusting the RotateRight and RotateLeft CON directives until your Boe-Bot executes perfect (or at least really good) 180° left and right turns.
√······· Save your modified program, and be prepared to use those values when you update the constants in GoToClosestObject.bs2.
Going to the Closest Object
Once GotoClosestObject.bs2's CON directives that control the Boe-Bot's servos are updated, it should perform a respectable demo for objects 1 meter away or less. It also has code that deals with the anomalies you experimented with in the Ping)))Dar such as when two objects are so close together that they look like one object in the Ping)))Dar Debug Terminal display. In addition, GoToClosestObject.bs2 has code that can figure out if the closest object is slightly outside its field of detection to the left or right, and it can adjust the Boe-Bot's position and re-scan the object before going straight to it.
Example Program − GoToClosestObject.bs2
As you went through this and Ping)))Dar, you used TestPingDirection.bs2, CalibrateStraightForward.bs2, and Calibrate180DegreeTurns.bs2 to determine values for the calibration constants listed below. GoToClosestObject.bs2 relies on these eight constants to scan a field of objects, determine which one is closest, and accurately turn to face and go to the object. The calibration results for these CON directives have to be updated GotoClosestObject.bs2 before you run it.
•········· LimitLeft
•········· LimitRight
•········· PingDirToAngle
•········· ForwardLeftFast
•········· ForwardRightFast
•········· RotateRight
•········· RotateLeft
·
√······· IMPORTANT: Make sure to update GoToClosestObject.bs2's CON directives for the eight names listed above with the values you determined as you went through this and the previous activity.
√······· Measure the distance from the front of the Ping))) rangefinder to the center of the Boe-Bot's turning axis. It should either be 7 cm or around 5 cm. If it's 5 cm, update the PingAxleOffset CON directive accordingly.
√······· Save and run your modified version of GoToClosestObject.bs2.
√······· Start by testing it with a single, cylindrical object 1 ft (30 cm) away from the Boe-Bot at various angles, such as 60°, 90°, and 120°.
√······· In each case, the Boe-Bot should be able to scan its surroundings, find the object, and go straight to it. If it doesn't succeed, revisit the various calibration instructions in this and the previous activity. Then, use the values you collected, and update the CON directives in GoToClosestObject.bs2 and try again.
√······· When the Boe-Bot can reliably go to one object that's 1 ft (30 cm) away, try a variety of other angles and distances. How far out can the Boe-Bot detect and go to an object?
√······· Try 1 object 1 ft (30 cm) away from the Boe-Bot at 120° and the other one 2 ft (60 cm) away from the Boe-Bot at 60°. The Boe-Bot should go straight to the one at 120°. Swap the distances and try again; the Boe-Bot should go to the one at 60°.
√······· Try two objects 1 ft (30 cm) apart and 1.5 ft (45 cm) from the front of the Boe-Bot. The Boe-Bot will initially think it sees a single object and attempt to go between the two cylinders. As it gets closer, it should detect the gap between them and choose the one it's closest to.
√······· Try one object one ft (30 cm) away at 0° and the other object 2 ft (60 cm) away at 180 degrees. The Boe-Bot should discover that the right edge of the object at 0° is outside its field of detection. So it should rotate and re-scan the object, and then go directly to it.
__________________________________________________ ___________________________
··
(c) 2009·by Parallax Inc - all rights reserved.··
Post Edited By Moderator (Jessica Uelmen (Parallax)) : 8/25/2010 6:07:28 PM GMT
·
In Ping)))Dar – A Radar Style Display, we used the debug terminal to get an idea of what objects look like when the Boe-Bot scans them with the Ping))) Ultrasonic Rangefinder and Mounting Bracket kits. In this activity, we'll use the same setup along with a program that makes the Boe-Bot scan a field of objects and go to the closest one. Figure 1 shows the Boe-Bot's “scan and go to” sequence as it runs GotoClosestObject.bs2. Here's what it does:
·
√······· (The Boe-Bot pauses for 2 seconds after the RESET button on the Board of Education is pressed and released.
√······· The Boe-Bot directs the Ping))) Ultrasonic Rangefinder to the right (0°).
√······· It scans from right to left (180°) while taking successive distance measurements and saving the angle and distance of the closest measurement it found.
√······· The Boe-Bot then points the Ping))) rangefinder in the direction of its closest measurement.
√······· The Ping))) is then swept to the right in smaller increments until the object's right edge is detected.
√······· The Ping))) is swept to the left (in the same small increments) until the object's left edge is detected.
√······· The Boe-Bot calculates the middle of the object and turns in place to face it.
√······· The Boe-Bot goes to the object.
View Video Introduction (YouTube)
Download Source Code – Scan For and Go To Closest Object
·
Figure 1 - Finding the Angle that Points Toward the Middle of the Object
Getting Started
This project contains advanced topics, not intended for Boe-Bot beginners.· Before continuing, it is highly recommended you complete the following tasks in order to gain a better understanding on how this application works:
·
√······· Complete all activities in What’s a Microcontroller
√······· Complete at minimum Chapters 1-4 in Robotics with the Boe-Bot
√······· Complete at minimum Chapters 1-3 in Smart Sensors and Applications
√······· Download the product documentation and assembly instructions for the PING))) Bracket Kit
√······· Complete the Ping)))Dar – A Radar Style Display forum post
Parts Required
(1) Fully assembled and tested Boe-Bot® Robot
(1) Ping))) Ultrasonic Rangefinder
(1) Ping))) Mounting Bracket Kit
Finding, Facing, and Going to the Closest Object
·
Finding the closest object is pretty simple. Start by initializing a word variable, named minDist in GotoClosestObject.bs2, to 65536. Then, repeatedly take distance measurements with the Ping))) rangefinder while sweeping the mounting bracket servo from right to left. Each time the current distance measurement is less than minDist, copy the current distance measurement to minDist, and also copy pingDir, which is the mounting bracket servo control pulse duration to a variable named markDir. When the sweep is done, minDist will store the closest distance, but more importantly, markDir will store the pulse duration that made the mounting bracket servo point the rangefinder at the closest object. As we saw in the previous activity, once the pulse duration to the mounting bracket servo is known, the angle the mounting bracket servo has pointed the rangefinder to can be calculated.
Single_Sweep:
·
· DO UNTIL pingDir >= LimitLeft
··· GOSUB Sweep_Increment
··· GOSUB Get_Ping_Cm
··· IF distance < minDist THEN
····· minDist = distance
····· markDir = pingDir
··· ENDIF
··LOOP
·
· RETURN
·
A quick sweep with fairly large servo increments may not be enough for a heading toward the middle of the closest object. Next, GotoClosestObject.bs2 points the rangefinder in the direction where it took its closest measurement. Then, it scans to the·right until the measured distance is more than 10 cm beyond than the closest distance measurement shown in Figure 3 (a). After that, it scans to the left until the measured distance is 10 cm beyond the closest distance shown in Figure 3 (b). The program marks both these servo pulse durations, and calculates the average of the two. This gives the Boe-Bot's BASIC Stamp a pulse duration to make the mounting bracket servo point the rangefinder to the object about half way between the two edges as shown in Figure 3 (c).
·
Figure 3 - Finding the Angle that Points Toward the Middle of the Object
Although the Ping))) Mounting Bracket servo is now pointing the Ping))) Ultrasonic Rangefinder directly at the object, it's not the right angle to help the Boe-Bot turn to face the object. Figure 4 shows why. Notice that θ1 (θping-object) is much less than θ2 (θBoe-Bot-object). So, before facing the object, the program has to figure out what θ2 is given θ1 and the object's distance (d), which is (d·
Figure 4 - PULSOUT Duration Argument vs. Rotation Angle
Ping)))Dar.bs2 demonstrated how to make polar to Cartesian coordinate conversions using x = d cos(θ[noparse];)[/noparse] and y = d sin(θ[noparse];)[/noparse]. Assuming the Boe-Bot's continuous rotation servos are mounted so that their output shafts are toward the back of the Boe-Bot, the offset between the front of the Ping))) rangefinder and the Boe-Bot's axis is about 7 cm. If they are mounted with their output shafts toward the front of the Boe-Bot, the offset is more like 5 cm. Assuming 7 cm, after the program calculates the x and y centimeter distances, all it has to do is add 7 cm to the y distance, and those are the x and y distances for θ2. Then, θ2 (θBoe-Bot-Object) can be calculated using the inverse tangent of the (y + 7 cm) ÷ x distances.
Once the program has figured out θ2, all it has to do to is face the object and make the continuous rotation servos cause the Boe-Bot to turn in place for that number of binary radians. One of the calibration programs in this activity involves figuring out the continuous rotation servo PULSOUT Duration arguments that will make the Boe-Bot turn 128 brads (180°) with 128 pulses. The constants that are determined will then be used in GotoClosestObject.bs2 to give the Boe-Bot turn-in-place control in terms of brads.
Software Continuous Rotation Servo Calibration
In Ping)))Dar, you adjusted the Ping))) Mounting Bracket servo's hardware to make sure it points straight forward when it receives PULSOUT 14, 750 signals. You also adjusted LimitLeft and LimitRight CON directives to make sure the mounting bracket servo scanned the Ping))) rangefinder across a 180° field in front of the Boe-Bot. There are two more sets of CON directives that have to be updated, and they will be used in PULSOUT commands that control the continuous rotation servos that turn the Boe-Bot's wheels. These CON directives are FwdLeftFast, FwdRightFast, RotateLeft, and RotateRight. FwdLeftFast and FwdRightFast are used in PULSOUT commands to make the Boe-Bot drive forward at top speed. They have to be adjusted so that they make the Boe-Bot roll straight forward (instead of a curved path that's mostly forward) to make sure the Boe-Bot can roll straight to the object. RotateLeft and RotateRight have to be adjusted so that they make the Boe-Bot turn 180° (128 brads) when 128 pulses are delivered. This adjustment will make it possible for GotoClosestObject.bs2 to turn the Boe-Bot to the correct angle so that it faces the closest object before driving up to it.
CON Directives for Adjusting Boe-Bot Maneuvers
Below are the two PULSOUT commands along with the PAUSE command to send signals to the Boe-Bot's continuous rotation servos that make them turn full speed forward. The number of times a loop repeats these signals dictates how long the servos will turn full speed forward. The speed at which each servo turns is controlled by the PULSOUT commands' Duration arguments, which are 850 and 650 in the example. Since one servo typically turns a little faster than the other at full speed, the 850 or 650 values have to be adjusted to equalize the speeds the wheels turn.
PULSOUT 13, 850
PULSOUT 12, 650
PAUSE 20
The CON directives below make these values adjustable, which is especially useful when they are used more than one place in the program. All you have to do is change a CON directive, and any PULSOUT command that uses the name for Duration argument will automatically be updated. For example, if you change the value of the FwdLeftFast CON directive from 850 to 820, the command PULSOUT BotServoLeft, FwdFastLeft will be changed from PULSOUT 13, 850 to PULSOUT 13, 820. The net effect will be that the left wheel will rotate slower when the program runs.
BotServoLeft ·PIN 13 ··················· ' Boe-Bot servo pins
BotServoRight PIN 12
.
.
.
Center ·······CON 750 ·················· ' Center servo pulse width
FwdLeftFast ··CON 850 ·················· ' Fast settings - straight ahead
FwdRightFast ·CON 650
RotateRight ··CON 758 ·················· ' Boe-Bot rotate right pulse
RotateLeft ···CON 729 ·················· ' Boe-Bot rotate left pulse
.
.
.
· PULSOUT BotServoLeft, RotateRight
· PULSOUT BotServoRight, RotateRight
· PAUSE 20
.
.
.
· PULSOUT BotServoLeft, FwdLeftFast
· PULSOUT BotServoRight, FwdRightFast
· PAUSE 20
.
.
.
Example Program − CalibrateStraightForward.bs2
In this calibration task, you have to observe the Boe-Bot's forward travel and determine if it is tending to curve to either the left or right. Then, adjust the FwdLeftFast or FwdRightFast CON directives to correct any tendency toward traveling in a curve. To make the correction, adjust the CON directive for the faster wheel to make its speed equal to that of the slower wheel.
·
√······· Run CalibrateStraightForward.bs2 and watch the Boe-Bot as it travels forward. Does it curve to the left, or right, or is its travel almost perfectly straight?
√······· If the Boe-Bot already goes in a straight line (almost no curving whatsoever), skip to the next section: Example Program - Calibrate180DegreeTurns.bs2.
√······· If the Boe-Bot curves to the left, the right wheel is going too fast. Try running the program with FwdRightFast set to 660. Still curving left? Try 670 to make it slower still. Still curving left? Try 680. Keep bringing the FwdRightFast value closer to 750 and running and testing the program until it either makes the Boe-Bot travel in a straight line, or it starts curving to the right instead. If the Boe-Bot starts to curve to the right, try a value between the current and previous FwdRightFast settings. Fine tune the value until it makes the Boe-Bot go in the straightest path possible.
√······· If the Boe-Bot curves to the right, the left wheel is going too fast. Try running the program with FwdLeftFast set to 840. Still curving right? Try 830 to make it slower still. Still curving right? Try 820. Keep bringing the FwdLeftFast value closer to 750 and running and testing the program until it either makes the Boe-Bot travel in a straight line, or it starts curving to the left instead. If the Boe-Bot starts to curve to the left, try a value between the current and previous FwdLeftFast settings. Fine tune the value until it makes the Boe-Bot go in the straightest path possible.
√······· Save your modified program and make a note of your FwdLeftFast and FwdRightFast CON directives so that you can update them in the upcoming navigation program, GotoClosestObject.bs2.
[color=#008000]' Smart Sensors and Applications - CalibrateStraightForward.bs2[/color]
[color=#008000]' {$STAMP BS2}[/color]
[color=#008000]' {$PBASIC 2.5}[/color]
[color=#000000]BoeBotServoL PIN 13[/color]
[color=#000000]BoeBotServoR PIN 12[/color]
[color=#008000]' Adjust these constants until the Boe-Bot goes full speed forward[/color]
[color=#008000]' in a straight line.[/color]
[color=#000000]FwdLeftFast CON 850[/color]
[color=#000000]FwdRightFast CON 650[/color]
[color=#000000]counter VAR Byte[/color]
[color=#020FC0]FOR[/color][color=#000000] counter = 0 [/color][color=#020FC0]TO[/color][color=#000000] 250[/color]
[color=#020FC0] PULSOUT[/color][color=#000000] BoeBotServoL, FwdLeftFast[/color]
[color=#020FC0] PULSOUT[/color][color=#000000] BoeBotServoR, FwdRightFast[/color]
[color=#020FC0] PAUSE[/color][color=#000000] 20[/color]
[color=#020FC0]NEXT[/color]
[color=#020FC0]END[/color]
Example Program − Calibrate180DegreeTurns.bs2
This calibration task involves adjusting CON directives for PULSOUT command Duration arguments that will make the Boe-Bot rotate 128 brads (180°) when 128 pulses are sent to its continuous rotation drive servos. These constants will likely be different for left and right rotation, so Calibrate180DegreeTurns.bs2 has separate RotateRight and RotateLeft CON directives. By adjusting the values of the RotateRight and RotateLeft CON directives, you will adjust how far the Boe-Bot rotates left and right when it receives 128 pulses. The goal again is to adjust the constants until the program makes the Boe-Bot execute a 180° right turn that's as close to perfect as you can get it, followed by an equally perfect 180° left turn.
√······· Open and run Calibrate180DegreeTurns.bs2 and watch how far the Boe-Bot turns to the left and then to the right. The calibration goal is for it to turn 180° to the right, then 180° to the left.
√······· If the Boe-Bot turns more than 180° to the right, reduce the value of the RotateRight CON directive, and re-run the program. If it turns less than 180° to the right, increase the RotateRight CON directive. Repeat until you have tuned it to turn 180° to the right.
√······· For tuning left rotation, if the Boe-Bot turns too far, increase the RotateLeft CON directive, or if it doesn't turn far enough, decrease the RotateLeft CON directive.
√······· Keep adjusting the RotateRight and RotateLeft CON directives until your Boe-Bot executes perfect (or at least really good) 180° left and right turns.
√······· Save your modified program, and be prepared to use those values when you update the constants in GoToClosestObject.bs2.
[color=#008000]' Smart Sensors and Applications - Calibrate180DegreeTurns.bs2[/color]
[color=#008000]' {$STAMP BS2}[/color]
[color=#008000]' {$PBASIC 2.5}[/color]
[color=#000000]BotServoLeft PIN 13 [/color][color=#008000]' Left drive servo[/color]
[color=#000000]BotServoRight PIN 12 [/color][color=#008000]' Right drive servo[/color]
[color=#000000]RotateRight CON 765 [/color][color=#008000]' Boe-Bot rotate right pulse[/color]
[color=#000000]RotateLeft CON 735 [/color][color=#008000]' Boe-Bot rotate left pulse[/color]
[color=#000000]counter VAR Word[/color]
[color=#020FC0]FOR[/color][color=#000000] counter = 1 [/color][color=#020FC0]TO[/color][color=#000000] 128[/color]
[color=#020FC0] PULSOUT[/color][color=#000000] BotServoLeft, RotateRight[/color]
[color=#020FC0] PULSOUT[/color][color=#000000] BotServoRight, RotateRight[/color]
[color=#020FC0] PAUSE[/color][color=#000000] 20[/color]
[color=#020FC0]NEXT[/color]
[color=#020FC0]FOR[/color][color=#000000] counter = 127 [/color][color=#020FC0]TO[/color][color=#000000] 0[/color]
[color=#020FC0] PULSOUT[/color][color=#000000] BotServoLeft, RotateLeft[/color]
[color=#020FC0] PULSOUT[/color][color=#000000] BotServoRight, RotateLeft[/color]
[color=#020FC0] PAUSE[/color][color=#000000] 20[/color]
[color=#020FC0]NEXT[/color]
[color=#020FC0]END[/color]
Going to the Closest Object
Once GotoClosestObject.bs2's CON directives that control the Boe-Bot's servos are updated, it should perform a respectable demo for objects 1 meter away or less. It also has code that deals with the anomalies you experimented with in the Ping)))Dar such as when two objects are so close together that they look like one object in the Ping)))Dar Debug Terminal display. In addition, GoToClosestObject.bs2 has code that can figure out if the closest object is slightly outside its field of detection to the left or right, and it can adjust the Boe-Bot's position and re-scan the object before going straight to it.
Example Program − GoToClosestObject.bs2
As you went through this and Ping)))Dar, you used TestPingDirection.bs2, CalibrateStraightForward.bs2, and Calibrate180DegreeTurns.bs2 to determine values for the calibration constants listed below. GoToClosestObject.bs2 relies on these eight constants to scan a field of objects, determine which one is closest, and accurately turn to face and go to the object. The calibration results for these CON directives have to be updated GotoClosestObject.bs2 before you run it.
•········· LimitLeft
•········· LimitRight
•········· PingDirToAngle
•········· ForwardLeftFast
•········· ForwardRightFast
•········· RotateRight
•········· RotateLeft
·
√······· IMPORTANT: Make sure to update GoToClosestObject.bs2's CON directives for the eight names listed above with the values you determined as you went through this and the previous activity.
√······· Measure the distance from the front of the Ping))) rangefinder to the center of the Boe-Bot's turning axis. It should either be 7 cm or around 5 cm. If it's 5 cm, update the PingAxleOffset CON directive accordingly.
√······· Save and run your modified version of GoToClosestObject.bs2.
√······· Start by testing it with a single, cylindrical object 1 ft (30 cm) away from the Boe-Bot at various angles, such as 60°, 90°, and 120°.
√······· In each case, the Boe-Bot should be able to scan its surroundings, find the object, and go straight to it. If it doesn't succeed, revisit the various calibration instructions in this and the previous activity. Then, use the values you collected, and update the CON directives in GoToClosestObject.bs2 and try again.
√······· When the Boe-Bot can reliably go to one object that's 1 ft (30 cm) away, try a variety of other angles and distances. How far out can the Boe-Bot detect and go to an object?
√······· Try 1 object 1 ft (30 cm) away from the Boe-Bot at 120° and the other one 2 ft (60 cm) away from the Boe-Bot at 60°. The Boe-Bot should go straight to the one at 120°. Swap the distances and try again; the Boe-Bot should go to the one at 60°.
√······· Try two objects 1 ft (30 cm) apart and 1.5 ft (45 cm) from the front of the Boe-Bot. The Boe-Bot will initially think it sees a single object and attempt to go between the two cylinders. As it gets closer, it should detect the gap between them and choose the one it's closest to.
√······· Try one object one ft (30 cm) away at 0° and the other object 2 ft (60 cm) away at 180 degrees. The Boe-Bot should discover that the right edge of the object at 0° is outside its field of detection. So it should rotate and re-scan the object, and then go directly to it.
[color=#020FC0][color=#008000]' -----[noparse][[/noparse] Title ]--------------------------------------------------------------[/color]
[color=#008000]' Smart Sensors and Applications - GoToClosestObject.bs2[/color]
[color=#008000]' Sweep Ping))) Ultrasonic Rangefinder across 180-degrees and find the closest[/color]
[color=#008000]' object. Then calculate and execute the turn required to face the object.[/color]
[color=#008000]' Travel forward until the object is less than or equal to 5 cm from the front[/color]
[color=#008000]' of the rangefinder.[/color]
[color=#008000]' IMPORTANT: This program has several constants that have to be tuned before[/color]
[color=#008000]' it will work right. Follow the instructions in Smart Sensors[/color]
[color=#008000]' and Applications, Chapter 8, Activity #4 and #5 before you run[/color]
[color=#008000]' this program![/color]
[color=#008000]' {$STAMP BS2} ' Target device = BASIC Stamp 2[/color]
[color=#008000]' {$PBASIC 2.5} ' Language = PBASIC 2.5[/color]
[color=#008000]' -----[noparse][[/noparse] I/O Definitions ]----------------------------------------------------[/color]
[color=#008000]' Ping))) Ultrasonc Rangefinder and Mounting Bracket[/color]
[color=#000000]PingServo PIN 14[/color] [color=#008000]' Servo that directs Ping))) snsr[/color]
[color=#000000]Ping PIN 15[/color] [color=#008000]' Ping))) sensor signal pin[/color]
[color=#008000]' Boe-Bot servo pins (Left and right are from driver's seat perspective.)[/color]
[color=#000000]BotServoLeft PIN 13[/color] [color=#008000]' Left drive servo[/color]
[color=#000000]BotServoRight PIN 12[/color] [color=#008000]' Right drive servo[/color]
[color=#008000]' -----[noparse][[/noparse] Constants ]----------------------------------------------------------[/color]
[color=#008000]' Ping))) mounting bracket constants[/color]
[color=#000000]LimitLeft CON 1250[/color] [color=#008000]' Bracket 90-degrees Left[/color]
[color=#000000]LimitRight CON 250[/color] [color=#008000]' Bracket 90-degrees Right[/color]
[color=#000000]Center CON 750[/color] [color=#008000]' Center/0-degree pulse duration[/color]
[color=#000000]Increment CON 15[/color] [color=#008000]' Increment for pulse sweeping[/color]
[color=#000000]MinSweep CON 40[/color] [color=#008000]' Pulses -> 90-degree right[/color]
[color=#008000]' Boe-Bot continuous rotation servo control constants[/color]
[color=#000000]FwdLeftFast CON 850[/color] [color=#008000]' Fast settings - straight ahead[/color]
[color=#000000]FwdRightFast CON 650[/color]
[color=#000000]RotateRight CON 765[/color] [color=#008000]' Boe-Bot rotate right pulse[/color]
[color=#000000]RotateLeft CON 735[/color] [color=#008000]' Boe-Bot rotate left pulse[/color]
[color=#000000]BtwnPulses CON 20[/color] [color=#008000]' ms between servo pulses[/color]
[color=#008000]' Ping))) Ultrasonic Rangefinder constants[/color]
[color=#000000]CmConstant CON 2260[/color] [color=#008000]' Echo time -> cm with **[/color]
[color=#000000]SinCosTo256 CON[/color] [color=#000000]517[/color] [color=#008000]' For */ -127..127 -> -256..256[/color]
[color=#000000]Ms20 CON[/color] [color=#000000]330[/color] [color=#008000]' 20 ms worth of cm[/color]
[color=#008000]' MSB sign (twos complement)[/color]
[color=#000000]Negative CON[/color] [color=#000000]1[/color] [color=#008000]' Negative sign (bit-15)[/color]
[color=#000000]Positive[/color] [color=#000000]CON[/color] [color=#000000]0[/color] [color=#008000]' Positive sign (bit-15)[/color]
[color=#008000]' Ping and turning axis geometry[/color]
[color=#000000]PingDirToAngle[/color] [color=#000000]CON[/color] [color=#000000]8454[/color] [color=#008000]' Servo pulse to angle ** con[/color]
[color=#000000]PingAxleOffset[/color] [color=#000000]CON[/color] [color=#000000]7[/color] [color=#008000]' 7 cm between ping))) and axis[/color]
[color=#008000]' -----[noparse][[/noparse] Variables ]----------------------------------------------------------[/color]
[color=#000000]time VAR Word [/color][color=#008000]' Ping))) echo time[/color]
[color=#000000]pingDir VAR Word [/color][color=#008000]' Pulse duration -> direction[/color]
[color=#000000]x VAR Word [/color][color=#008000]' x coordinate[/color]
[color=#000000]y VAR Word [/color][color=#008000]' y coordinate[/color]
[color=#000000]distance VAR Time [/color][color=#008000]' Object centimeter distance[/color]
[color=#000000]markDir VAR y [/color][color=#008000]' Pulse points to closest object[/color]
[color=#000000]rightMark VAR x [/color][color=#008000]' Pulse to object's right side[/color]
[color=#000000]leftMark VAR pingDir [/color][color=#008000]' Pulse to object's left side[/color]
[color=#000000]pulses VAR x [/color][color=#008000]' +/- brads to turn toward object[/color]
[color=#000000]PrevDist VAR Byte [/color][color=#008000]' Previous distance measurement[/color]
[color=#000000]angle VAR Byte [/color][color=#008000]' Servo angle from right in brads[/color]
[color=#000000]counter VAR angle [/color][color=#008000]' Loop counter[/color]
[color=#000000]minDist VAR angle [/color][color=#008000]' Minimum distance measurement[/color]
[color=#000000]sweepInc VAR Nib [/color][color=#008000]' Increment for servo sweep[/color]
[color=#000000]sweepDir VAR Bit [/color][color=#008000]' Increment/decrement pingDir[/color]
[color=#000000]xSign VAR Bit [/color][color=#008000]' Stores sign of x variable[/color]
[color=#000000]ySign VAR xSign [/color][color=#008000]' Stores sign of x variable[/color]
[color=#000000]pSign VAR Bit [/color][color=#008000]' Sign of pulses variable[/color]
[color=#000000]edgesFound VAR Bit [/color][color=#008000]' Navigation flag[/color]
[color=#008000]' -----[noparse][[/noparse] Initialization ]-----------------------------------------------------[/color]
PAUSE[color=#000000] 2000[/color] [color=#008000]' Delay program start by 2 s.[/color]
[color=#008000]' -----[noparse][[/noparse] Main Routine ]-------------------------------------------------------[/color]
GOSUB[color=#000000] Get_Ping_Cm [/color][color=#008000]' First distance measurement[/color]
DO UNTIL[color=#000000] distance <= 5 [/color][color=#008000]' Repeat until distance <= 5 cm[/color]
[color=#000000] edgesFound = 0 [/color][color=#008000]' Clear edges found flag[/color]
DO UNTIL[color=#000000] edgesFound = 1 [/color][color=#008000]' Repeat until edges found = 1[/color]
GOSUB[color=#000000] Face_Closest_Object [/color][color=#008000]' Find & face closest object[/color]
LOOP
GOSUB[color=#000000] Get_Ping_Cm [/color][color=#008000]' Get current distance[/color]
DO UNTIL[color=#000000] distance <= 5 [/color][color=#008000]' Drive toward object[/color]
[color=#000000] prevDist = distance MAX 255 [/color][color=#008000]' Current distance -> previous[/color]
GOSUB[color=#000000] Get_Ping_Cm [/color][color=#008000]' Get new distance[/color]
PULSOUT[color=#000000] BotServoLeft, FwdLeftFast [/color][color=#008000]' Boe-Bot forward[/color]
PULSOUT[color=#000000] BotServoRight, FwdRightFast[/color]
PAUSE[color=#000000] BtwnPulses - (distance / Ms20) [/color][color=#008000]' 20 ms pause between pulses[/color]
IF[color=#000000] distance >= prevDist + 5 [/color]THEN EXIT [color=#008000]' Exit if distance increasing[/color]
LOOP
LOOP [color=#008000]' Main routine's outermost loop[/color]
END
[color=#008000]' -----[noparse][[/noparse] Subroutines - BoeBot_Turn_Brads ]-----------------------------------[/color]
[color=#008000]' Boe-Bot turns a certain number of binary radians to face an object.[/color]
[color=#000000]BoeBot_Turn_Brads:[/color]
IF[color=#000000] pSign = Positive [/color]THEN
FOR[color=#000000] counter = 0 TO ABS(pulses)[/color]
PULSOUT[color=#000000] BotServoLeft, RotateRight[/color]
PULSOUT[color=#000000] BotServoRight, RotateRight[/color]
PAUSE[color=#000000] BtwnPulses - (distance / Ms20)[/color]
NEXT
ELSE
FOR[color=#000000] counter = 0 TO ABS(pulses)[/color]
PULSOUT[color=#000000] BotServoLeft, RotateLeft[/color]
PULSOUT[color=#000000] BotServoRight, RotateLeft[/color]
PAUSE[color=#000000] BtwnPulses - (distance / Ms20)[/color]
NEXT
ENDIF
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutines - Face_Closest_Object ]----------------------------------[/color]
[color=#008000]' Scan for closest object using a Ping))) rangefinder mounted on a standard[/color]
[color=#008000]' servo. Locate the middle fo the object, and turn Boe-Bot to face it.[/color]
[color=#000000]Face_Closest_Object:[/color]
[color=#008000] ' Initialize sweep increment.[/color]
[color=#000000] sweepInc = Increment[/color]
[color=#008000] ' Start Servo rotated to the far right.[/color]
[color=#000000] pingDir = LimitRight[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
[color=#008000] ' Make minDist large and sweepDir positive (0). Single_Sweep sweeps[/color]
[color=#008000] ' left -> right while measuring object distances and stores the direction[/color]
[color=#008000] ' of the closest object in markDir.[/color]
[color=#000000] minDist = 65535[/color]
[color=#000000] sweepDir = Positive[/color]
GOSUB[color=#000000] Single_Sweep[/color]
[color=#008000] ' Point the servo in the direction of the closest distance measurement.[/color]
[color=#000000] pingDir = markDir[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
[color=#008000] ' Scan to find object's right side.[/color]
GOSUB[color=#000000] Find_Right_Side[/color]
IF[color=#000000] edgesFound = 0 [/color]THEN RETURN
[color=#008000] ' Point the servo in the direction of the closest distance measurement.[/color]
[color=#000000] pingDir = markDir[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
[color=#008000] ' Scan to find object's right side.[/color]
GOSUB[color=#000000] Find_Left_Side[/color]
IF[color=#000000] edgesFound = 0 [/color]THEN RETURN
[color=#008000] ' Average the angles to the object's left and right sides. That's the[/color]
[color=#008000] ' middle of the object. Point rangefinder in that direction.[/color]
[color=#000000] pingDir = leftMark + rightMark / 2[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
[color=#008000] ' At this point, the Ping))) should be pointing directly at the closest[/color]
[color=#008000] ' object.[/color]
[color=#008000] ' Calculate the angle to the object's angle in brads, and turn the[/color]
[color=#008000] ' Boe-Bot to face that angle.[/color]
GOSUB[color=#000000] Turn_Angle_Adjust[/color]
GOSUB[color=#000000] BoeBot_Turn_Brads[/color]
[color=#008000] ' Face Ping))) rangefinder straight ahead.[/color]
[color=#000000] pingDir = Center[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutines - Find_Left_Side ]---------------------------------------[/color]
[color=#008000]' Scan left until the measured distance is 10 cm beyond the closest distance,[/color]
[color=#008000]' which is assumed to mean the object's side has been found.[/color]
[color=#008000]' If the object's side has been found within the 180-degree field of vision,[/color]
[color=#008000]' set edgesFound = 1 and store the pulse duration (pingDir) at which the[/color]
[color=#008000]' object was found in the leftMark variable.[/color]
[color=#008000]' If the side was not found by the 180-degree point in the scan, rotate the[/color]
[color=#008000]' Boe-Bot until the edge is found, and then set edgesFound to 0 signifying[/color]
[color=#008000]' that the scan will have to be repeated because the Boe-Bot rotated[/color]
[color=#008000]' to find the side, which would otherwise cause the markDir variable to[/color]
[color=#008000]' store an incorrect value.[/color]
[color=#000000]Find_Left_Side:[/color]
[color=#000000] sweepDir = Positive[/color]
[color=#000000] distance = minDist[/color]
[color=#000000] sweepInc = 1[/color]
DO UNTIL[color=#000000] distance > minDist + 10[/color]
GOSUB[color=#000000] Sweep_Increment[/color]
GOSUB[color=#000000] Get_Ping_Cm[/color]
IF[color=#000000] pingDir >= LimitLeft - 10 [/color]THEN
[color=#000000] pingDir = LimitLeft - 50[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
DO UNTIL[color=#000000] distance > minDist + 10[/color]
PULSOUT[color=#000000] BotServoLeft, RotateLeft[/color]
PULSOUT[color=#000000] BotServoRight, RotateLeft[/color]
GOSUB[color=#000000] Get_Ping_Cm[/color]
PAUSE[color=#000000] 20[/color]
LOOP
[color=#000000] edgesFound = 0[/color]
RETURN
ENDIF
LOOP
[color=#000000] leftMark = pingDir[/color]
[color=#000000] edgesFound = 1[/color]
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutines - Find_Right_Side ]--------------------------------------[/color]
[color=#008000]' Mirror image of Find_Left_Side.[/color]
[color=#000000]Find_Right_Side:[/color]
[color=#000000] sweepDir = Negative[/color]
[color=#000000] distance = minDist[/color]
[color=#000000] sweepInc = 1[/color]
DO UNTIL[color=#000000] distance > minDist + 10[/color]
GOSUB[color=#000000] Sweep_Increment[/color]
GOSUB[color=#000000] Get_Ping_Cm[/color]
IF[color=#000000] pingDir <= LimitRight + 10 [/color]THEN
[color=#000000] pingDir = LimitRight + 50[/color]
GOSUB[color=#000000] Point_At_PingDir[/color]
DO UNTIL[color=#000000] distance > minDist + 10[/color]
PULSOUT[color=#000000] BotServoLeft, RotateRight[/color]
PULSOUT[color=#000000] BotServoRight, RotateRight[/color]
GOSUB[color=#000000] Get_Ping_Cm[/color]
PAUSE[color=#000000] 20[/color]
LOOP
[color=#000000] edgesFound = 0[/color]
RETURN
ENDIF
LOOP
[color=#000000] rightMark = pingDir[/color]
[color=#000000] edgesFound = 1[/color]
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutine - Get_Ping_Cm ]-------------------------------------------[/color]
[color=#008000]' Gets Ping))) rangefinder measurement and converts time to centimeters.[/color]
[color=#008000]' Distance may be declared as time to save variable space.[/color]
[color=#000000]Get_Ping_Cm:[/color]
PULSOUT[color=#000000] Ping, 5[/color]
PULSIN[color=#000000] Ping, 1, time[/color]
[color=#000000] distance = time ** CmConstant[/color]
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutines - Point_At_PingDir ]-------------------------------------[/color]
[color=#008000]' Points servo mounted Ping))) rangefinder at an angle determined by the[/color]
[color=#008000]' value of the pingDir variable.[/color]
[color=#000000]Point_At_PingDir:[/color]
FOR[color=#000000] counter = 0 [/color]TO[color=#000000] MinSweep[/color]
PULSOUT[color=#000000] PingServo, pingDir[/color]
PAUSE[color=#000000] BtwnPulses[/color]
NEXT
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutine - Polar_To_Cartesian ]------------------------------------[/color]
[color=#008000]' Calculates x and y (Cartesian coordinates) given distance and angle[/color]
[color=#008000]' (polar coordinates).[/color]
[color=#000000]Polar_To_Cartesian:[/color]
[color=#008000]' Calculate left/right component.[/color]
[color=#000000] x = COS angle [/color][color=#008000]' Polar to Cartesian[/color]
[color=#000000] xSign = x.BIT15 [/color][color=#008000]' Store sign bit[/color]
[color=#000000] x = ABS(x) */ SinCOsTo256 [/color][color=#008000]' Polar to Cartesian continued[/color]
[color=#000000] x = distance */ x[/color]
IF[color=#000000] xSign = negative [/color]THEN[color=#000000] x = -x [/color][color=#008000]' Correct sign with sign bit[/color]
[color=#008000]' Calculate straight ahead component.[/color]
[color=#000000] y = SIN angle [/color][color=#008000]' Polar to Cartesian[/color]
[color=#000000] ySign = y.BIT15 [/color][color=#008000]' Store sign bit[/color]
[color=#000000] y = ABS(y) */ SinCOsTo256 [/color][color=#008000]' Polar to Cartesian continued[/color]
[color=#000000] y = distance */ y[/color]
[color=#000000] IF ySign = negative THEN y = -y [/color][color=#008000]' Correct sign with sign bit[/color]
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutines - Single_Sweep ]-----------------------------------------[/color]
[color=#008000]' Do one sweep, and find the closest distance measurement and the[/color]
[color=#008000]' pulse value that points the servo in that direction.[/color]
[color=#000000]Single_Sweep:[/color]
DO UNTIL[color=#000000] pingDir >= LimitLeft[/color]
GOSUB[color=#000000] Sweep_Increment[/color]
GOSUB[color=#000000] Get_Ping_Cm[/color]
IF[color=#000000] distance < minDist [/color]THEN
[color=#000000] minDist = distance[/color]
[color=#000000] markDir = pingDir[/color]
ENDIF
LOOP
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutine - Sweep_Increment ]---------------------------------------[/color]
[color=#008000]' Increment/decrement the position of the servo that directs the Ping)))[/color]
[color=#008000]' rangefinder. When pingDir goes outside either LimitRight or LimitLeft,[/color]
[color=#008000]' the sweep direction toggles.[/color]
[color=#000000]Sweep_Increment:[/color]
[color=#008000]' Change sweepDir for adding/subtracting increment if at rotation limit.[/color]
IF[color=#000000] pingDir <= LimitRight [/color]THEN
[color=#000000] sweepDir = Positive[/color]
ELSEIF[color=#000000] pingDir >= LimitLeft [/color]THEN
[color=#000000] sweepDir = Negative[/color]
ENDIF
[color=#008000] ' Add/subtract increment to/from pingDir.[/color]
IF[color=#000000] sweepDir = negative [/color]THEN
[color=#000000] pingDir = pingDir - sweepInc[/color]
ELSEIF[color=#000000] sweepDir = Positive [/color]THEN
[color=#000000] pingDir = pingDir + sweepInc[/color]
ENDIF
[color=#008000] ' Send positioning pulse to Ping))) Mounting Bracket servo.[/color]
PULSOUT[color=#000000] PingServo, pingDir[/color]
RETURN
[color=#008000]' -----[noparse][[/noparse] Subroutines - Turn_Angle_Adjust ]------------------------------------[/color]
[color=#008000]' Adjusts required turn angle based on 7 mm offset of Ping))) rangefinder from[/color]
[color=#008000]' Boe-Bot's turning axis.[/color]
[color=#000000]Turn_Angle_Adjust:[/color]
[color=#008000] ' Get the object's distance at its center.[/color]
GOSUB[color=#000000] Get_Ping_Cm[/color]
[color=#008000] ' Position servo & calculate angle from far-right in brads.[/color]
[color=#000000] angle = pingDir - 250 ** PingDirToAngle[/color]
GOSUB[color=#000000] Polar_To_Cartesian[/color]
[color=#008000] ' Add distance between Ping))) and center of Boe-Bot axis.[/color]
[color=#000000] y = y + PingAxleOffset[/color]
[color=#008000] ' Recalculate the turning angle with respect to Boe-Bot's turning axis.[/color]
[color=#000000] angle = x ATN y[/color]
[color=#000000] pulses = 64 - angle[/color]
[color=#000000] pSign = pulses.BIT15[/color]
RETURN
[/color]
__________________________________________________ ___________________________
··
(c) 2009·by Parallax Inc - all rights reserved.··
Post Edited By Moderator (Jessica Uelmen (Parallax)) : 8/25/2010 6:07:28 PM GMT
472 x 790 - 48K
388 x 339 - 13K
496 x 404 - 20K
393 x 441 - 26K
534 x 594 - 23K
731 x 580 - 94K
731 x 331 - 58K
731 x 577 - 103K
731 x 80 - 13K
Comments
Ran this code myself this afternoon. Very impressive! The Pings))) look like eyes and the Boe-Bot looks smarter than ever in this application. It's very "alive" (though not as much as that Linda Blair head that John was making pop out of the 50-gallon drum last week. . .that thing made me run out of the machine shop).
The simplicity is really nice and it the results are very consistent.
Ken Gracey
Parallax, Inc.
I'm glad to hear about the repeatability.· There's a lot more calibration here than there is in most of the other Boe-Bot apps.
I discovered a bug yesterday that you may want to keep in mind if you are doing any demos in the near future. For good performance with the current program/hardware, the maximum distance threshold appears to be 50 cm.
If the closest object is further out than that, the Boe-Bot does not correctly swivel the Ping))) rangefinder to locate the object's left and right sides. So keep the closest object within a 50 cm radius, and it should still work really well. I'll post updated code as soon as I fix the bug.
Andy
Post Edited (Andy Lindsay (Parallax)) : 6/7/2006 11:04:09 PM GMT
Am I supposed to know all of the answers because I work at Parallax?
First I'll set the stage. Today I did a 40-minute presentation (six times) for my son's elementary school. I brought the most amazing set of robots from Parallax (lawnmowers, fish robots, hexcrawler, 10 Boe-Bots, a half-dozen Scribblers) and the best robot videos out there (Mars - Spirit, SumoBots, the "RipSaw", the running gas-powered dog thing, etc) and the kids were going nuts over the whole gig. I even threw some rock music over the videos which caused them to jump up and down like monkeys when they saw the Delta rocket launch.·
But that's·just the background·to my question.
Trying to make the "GoToClosestObject" Ping))) demo even more interesting, I thought I'd use a couple of stuffed animals (a big cow and a small purple thing called "Baloo") instead of wooden blocks. Strangely, the ultrasonic sensor had a fair amount of trouble with them. Sometimes it seemed to not see them at all, and other times it would track them and loose them. The small purple one with less soft areas seemed to work better.
I assume that small furry critters absorb the ultrasonic waves, is that the case?
Ken Gracey
Parallax, Inc.
Post Edited (Ken Gracey (Parallax)) : 6/9/2006 3:42:51 AM GMT
At the base level ultrasonics are nothing more or less than sound. Sound is reflected by "hard" objects, and absorbed by "soft" objects, regardless of the frequency of the sound.
If you have a room which is too noisy, or has echoes, due to its size or configuration, then carpets, drapes and furniture will all help to ameliorate the problem. So too, "softening" the "hard" ceiling with acoustical tile. All are "soft" objects (comparatively speaking) and all will absorb sound.
I'd love to have some Aero-Gel to play with. I'd bet ultrasonics would pass right through it, as though it weren't even there. Aero-Gel is about 99% air! Speaking of SOFT! Any LLNL or NASA folks out there want to give it a shot, and report back?
Regards,
Bruce Bates
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
<!--StartFragment -->
Post Edited (Bruce Bates) : 6/9/2006 7:48:56 AM GMT
It'll be interesting to see what the stuffed animals' signatures are with Ping)))Dar - a Radar Style Display, especially compared to a soda can or water bottle. Try taking a screen capture of the Ping)))Dar display with a stuffed animal along with a photo, then replace it with a water bottle and take another screen capture and photo.
The demo is designed for use with cylinders, not blocks. I put one of those i-boxes about that on page 342. I also put a !-box about how important calibration is on page 347. If you have any suggestions about how I can more effectively convey those two important points to the reader, I'll certainly do my best to make it so.
Andy
1) So far, I haven't found background noise to be a problem, and I've done this demo in several really noisy environments. Of course, there are a lot of different flavors of noice, and maybe the grade school environment has one that interferes while trade shows do not.
2) If the stuffed animals were placed more than 50 cm from the front of the Boe-Bot, you may have seen that bug I mentioned earlier. Of course, if it worked with the blocks at the same distance in the same environment, that's probably not it. I'm still working on that bug.
3) Aaach! I just remembered, carpet can be a problem, and I didn't include a warning in the PDF. Yes, in many cases, carpet can look like an array of objects within the 180-degree Ping))) rangefinder's field of detection.· Sorry about that...
Post Edited (Andy Lindsay (Parallax)) : 6/9/2006 6:00:47 PM GMT
Brilliant series of projects based around sensors! I can't wait for the new SIC book to come out. The Ping))) Rangefinder has helped me to finally get to grips with ultrasonic sensors and I am waiting for my ordered bracket set to arrive so I can try out the newer programs. I echo the comment that others have made - the Ping))) really looks like eyes and, swivelling around on the bracket, it makes the BOE-Bot 'come alive' as·the robot·resembles a small, curious animal. As 'superworms' posted,·I would be interested to know how far off the book's publication is; will there be a complete kit?
I've also ordered the Sensor Collection as these projects really show some practical uses for Basic Stamps and sensors·in control and monitoring·systems.
Keep up the excellent work and posting the chapters for review.
John
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Manxstamp,
Isle of Man, British Isles
Thanks again, Mr. Lindsay, for the elegant and incredibly explanatory code projects.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST
One thing that I took into account in my code, that I do not find in "Closest Object" is checking for x/y values greater than (abs) -127/+127 before using ATN (to derive angle from a target x/y point).
In my own code, I normalize x/y values before doing ATN; "Closest Object" does not. Wouldn't you get bogus values if the closest target found has a distance of greater than 127 cm? I realize on a table top this highly unlikely, and out-of-bounds values don't matter until target heading calculations are performed in your code... but still. Or am I missing something? In my code·I shift x/y right with rounding until both are ABS <= 127, so I get the best resolution -- my inputs are signed Word x/y values in cm).
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST
1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php
pulses = 64 - angle
The angle variable should store a value ranging from 0 to 128. If you swap the 64 and the angle, it will turn the same distance in the opposite direction. That should work really well for 30 degrees or more off center. However, if the object is only one brad off center, that could be a problem since the Boe-Bot would turn one brad in the opposite direction. An IF...THEN statement increasing the pulses value if it's close to 64 would fix that problem.
In the Main Routine, you might need to get rid of the UNTIL distance <= 5, and move GoSUB Get_Ping_Cm just below the DO (that used to have UNTIL next to it). This will cause the Boe-Bot to repeat the check the surroundings and navigate loop periodically.
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Andy Lindsay
Education Department
Parallax, Inc.
I thank you for the Excellent code.
Post Edited (MovieMaker) : 11/22/2008 4:51:28 AM GMT
Andy, Mike and ALL of the Parallax Gang, Thanks for making this a nice Holiday Season for me by your EXCELLENT Support!
Post Edited (MovieMaker) : 12/25/2008 4:42:08 PM GMT
Thanks, and keep up the good work, the series of manuals/books is really what makes this a great learning experience!
Polbit
In short though, all you need to do (with a regular BS2) is send a PULSOUT on the correct pin with a value between 500 and 1000. Going outside these values can damage your servo. 750 is approximately the center position (for a standard servo), while 1000 and 500 are left and right (maybe not in that order). The second thing you need to do is to refresh this every 20 ms or so.
Just like Ping)))Dar, we've updated this project to include a brief video introduction/demonstration for your viewing pleasure.· You can check it out on YouTube here.
Happy Developing!
Jessica
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
Jessica Uelmen
Education Department
Parallax, Inc.
First is the program listed somewhere as a file or I to copy and paste from the thread ? It is long to retype and·not have errors.
Second, in this thread listing of the program·it says to complete chapter 8, activity 4 & 5 of the manual on "Smart Sensors and Applications". My manual does not have a chapter 8. What do I do?
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
······ Joe Fishback
-Robots are my friends-
"Smart Sensors" can be downloaded here: www.parallax.com/tabid/535/Default.aspx
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
When the going gets weird, the weird turn pro. -- HST
1uffakind.com/robots/povBitMapBuilder.php
1uffakind.com/robots/resistorLadder.php
▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔▔
DARRELL
Thanks...
Here's the source code I used.
Here's a pic of what I edited.
Here's a video of what happens.
Do you HAVE to have the "center" at 750? (mine is at 730 and [noparse][[/noparse]PingDirToAngle CON·· ·9290] using the calibrated values that I came up with.
Robot works fine in Dar mode and finds closest target, then looks at it and goes Bump Bump closer to it then stops.
I've tried from 1 ft. (or even closer) to 1 meter away.
Post Edited (Erik Elmore) : 7/17/2010 10:39:09 AM GMT
I added another LOOP to the Main Routine so that it would continue its search. The repeating LOOP proved to be quite entertaining.
New Main Routine:
Speaker PIN 5 ' piezo speaker and LED
'
[ Main Routine ]
DO WHILE(1)
GOSUB Get_Ping_Cm ' First distance measurement
DO UNTIL distance <= StopDistance ' Repeat until distance <= 5 cm
edgesFound = 0 ' Clear edges found flag
DO UNTIL edgesFound = 1 ' Repeat until edges found = 1
GOSUB Face_Closest_Object ' Find & face closest object
LOOP
GOSUB Get_Ping_Cm ' Get current distance
DO UNTIL distance <= StopDistance ' Drive toward object
prevDist = distance MAX 255 ' Current distance -> previous
GOSUB Get_Ping_Cm ' Get new distance
PULSOUT BotServoLeft, FwdLeftFast ' Boe-Bot forward
PULSOUT BotServoRight, FwdRightFast
PAUSE BtwnPulses - (distance / Ms20) ' 20 ms pause between pulses
IF distance >= prevDist + 5 THEN EXIT ' Exit if distance increasing
LOOP
LOOP ' Main routine's outermost loop
FREQOUT Speaker, 500, 3500, 3500 ' Stop Beep
PAUSE 1000
FOR counter = 0 TO 20 ' Back up about 10 cm
PULSOUT BotServoLeft, 800
PULSOUT BotServoRight, 700
PAUSE 20
NEXT
FOR counter = 1 TO 120 ' Turn Around
PULSOUT BotServoLeft, RotateRight
PULSOUT BotServoRight, RotateRight
PAUSE 20
NEXT
LOOP ' End of While Loop
END
'
[ Subroutines - BoeBot_Turn_Brads ]
Could someone please help me with this.
I ran the ' Smart Sensors and Applications - Ping)))Dar.bs2 code and my Debug terminal doesn't seem to display the objects
instead I just get a X
any help is greatly appreciated
Cheers
I am sure my code is an identical copy of the one provided, I have also changed the limits and recalculated my PingDirToAngle.
As for my wiring, I have followed the instructions carefully.
Here is a picture of my wiring.
I attached the servo to pin 15(X4) and the the Ping Sensor to pin5.
I have also modified the pin numbers in my code.
And Yes, the Debug terminal works fine with all other programs except this one
PING))) product page
PING))) documentation
Have you tried the simple demo program in the PING))) documentation to verify that it works?
Your picture looks ok although there's this big shadow right where the signal wire connects to the Stamp I/O pin socket. Also, color coding wires really does help. It's important to use black for ground and red for positive supply voltages. Other colors are not so important, but it's helpful to use a different color for each different signal, at least in the same area of a breadboard.
Hi Andy.
I posted a couple of days ago seek assistance with getting the GoToClosestObject program to work but I don't think I addressed the comments to you. After coping the program to my Boe-Bot I got the following results: the Ping)) mounting bracket sweeps the full 180 degrees and pauses but the Boe-Bot doesn't move. The Ping)) Dar test was successful in that I was able to see the radar style measurement in the DEBUG terminal. The TestPingDirection was partially successful in that the Ping))) brackets sweeps to the right and pauses but doesn't pause to the left or center. The PingServoCenter was successful; however, the CalibrateStrightForward required changes in constant values 850 was changed to 8000 and 650 was changed to 450 to get the Boe-Bot move fast. Any assistance will be appreciated.
James