Ping)))Dar "1 Scan Only, Please"
Rocko
Posts: 8
Hello,
I've gotten the Ping)))Dar program to run (on my Boe-Bot) as described but am still having a few issues.
How do I modify the code so the Ping)))Dar only scans once and then moves forward toward object.
My boebot goes through the scanning sequence, identifyies object, then moves forward maybe several inches. It then stops, goes through the scanning process again, moves forward another couple of inches, stops, scans again, etc.
Again, I want it to accept the initial scan and move forward to infinity.
Here's the code:
I've gotten the Ping)))Dar program to run (on my Boe-Bot) as described but am still having a few issues.
How do I modify the code so the Ping)))Dar only scans once and then moves forward toward object.
My boebot goes through the scanning sequence, identifyies object, then moves forward maybe several inches. It then stops, goes through the scanning process again, moves forward another couple of inches, stops, scans again, etc.
Again, I want it to accept the initial scan and move forward to infinity.
Here's the code:
' -----[ Title ]--------------------------------------------------------------
' Smart Sensors and Applications - GoToClosestObject.bs2
' Sweep Ping))) Ultrasonic Rangefinder across 180-degrees and find the closest
' object. Then calculate and execute the turn required to face the object.
' Travel forward until the object is less than or equal to 5 cm from the front
' of the rangefinder.
' IMPORTANT: This program has several constants that have to be tuned before
' it will work right. Follow the instructions in Smart Sensors
' and Applications, Chapter 8, Activity #4 and #5 before you run
' this program!
' {$STAMP BS2} ' Target device = BASIC Stamp 2
' {$PBASIC 2.5} ' Language = PBASIC 2.5
' -----[ I/O Definitions ]----------------------------------------------------
' Ping))) Ultrasonc Rangefinder and Mounting Bracket
PingServo PIN 14 ' Servo that directs Ping))) snsr
Ping PIN 1 ' Ping))) sensor signal pin
' Boe-Bot servo pins (Left and right are from driver's seat perspective.)
BotServoLeft PIN 13 ' Left drive servo
BotServoRight PIN 12 ' Right drive servo
StartLED PIN 0 ' display start delay
' -----[ Constants ]----------------------------------------------------------
' Ping))) mounting bracket constants
LimitLeft CON 1189 ' Bracket 90-degrees Left
LimitRight CON 286 ' Bracket 90-degrees Right
Limits CON LimitLeft + limitRight 'this added from Forum
Center CON 750 ' Center/0-degree pulse duration
Increment CON 15 ' Increment for pulse sweeping
MinSweep CON 40 ' Pulses -> 90-degree right
' Boe-Bot continuous rotation servo control constants
FwdLeftFast CON 817 ' Fast settings - straight ahead
FwdRightFast CON 650
RotateRight CON 763 ' Boe-Bot rotate right pulse
RotateLeft CON 740 ' Boe-Bot rotate left pulse
BtwnPulses CON 20 ' ms between servo pulses
' Ping))) Ultrasonic Rangefinder constants
CmConstant CON 2260 ' Echo time -> cm with **
SinCosTo256 CON 517 ' For */ -127..127 -> -256..256
Ms20 CON 330 ' 20 ms worth of cm
' MSB sign (twos complement)
Negative CON 1 ' Negative sign (bit-15)
Positive CON 0 ' Positive sign (bit-15)
' Ping and turning axis geometry
PingDirToAngle CON 8454 ' Servo pulse to angle ** con
PingAxleOffset CON 7 ' 7 cm between ping))) and axis
' -----[ Variables ]----------------------------------------------------------
time VAR Word ' Ping))) echo time
pingDir VAR Word ' Pulse duration -> direction
pingRev VAR Word ' Pulse duration -> direction for reversed servo
' added from Forum
x VAR Word ' x coordinate
y VAR Word ' y coordinate
distance VAR Time ' Object centimeter distance
markDir VAR y ' Pulse points to closest object
rightMark VAR x ' Pulse to object's right side
leftMark VAR pingDir ' Pulse to object's left side
pulses VAR x ' +/- brads to turn toward object
PrevDist VAR Byte ' Previous distance measurement
angle VAR Byte ' Servo angle from right in brads
counter VAR angle ' Loop counter
minDist VAR angle ' Minimum distance measurement
sweepInc VAR Nib ' Increment for servo sweep
sweepDir VAR Bit ' Increment/decrement pingDir
xSign VAR Bit ' Stores sign of x variable
ySign VAR xSign ' Stores sign of x variable
pSign VAR Bit ' Sign of pulses variable
edgesFound VAR Bit ' Navigation flag
temp VAR Byte
' -----[ Initialization ]-----------------------------------------------------
' -----[ EEPROM Data ]-----------------------------------------------------
RunStatus DATA $00 ' run status
' -----[ Initialization ]--------------------------------------------------
Reset:
READ RunStatus, temp ' read current status
temp = ~temp ' invert status
WRITE RunStatus, temp ' save for next reset
IF (temp > 0) THEN END ' run now?
Start_Delay:
HIGH StartLED ' show active
PAUSE 5000 ' start delay
LOW StartLED ' LED off
' following from Forum
pingDir = LimitRight ' Start servo at 0-degrees
FOR counter = 1 TO 40 ' Initialize servo position
'PULSOUT PingServo, pingDir ' Forum: removed & replaced by next 2 lines
pingRev = Limits - pingDir
PULSOUT PingServo, pingRev
PAUSE 20
NEXT
sweepInc = Increment ' Set the sweep increment
' -----[ Main Routine ]-------------------------------------------------------
GOSUB Get_Ping_Cm ' First distance measurement
DO UNTIL distance <= 5 ' 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 <= 5 ' 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
END
' -----[ Subroutines - BoeBot_Turn_Brads ]-----------------------------------
' Boe-Bot turns a certain number of binary radians to face an object.
BoeBot_Turn_Brads:
IF pSign = Positive THEN
FOR counter = 0 TO ABS(pulses)
PULSOUT BotServoLeft, RotateRight
PULSOUT BotServoRight, RotateRight
PAUSE BtwnPulses - (distance / Ms20)
NEXT
ELSE
FOR counter = 0 TO ABS(pulses)
PULSOUT BotServoLeft, RotateLeft
PULSOUT BotServoRight, RotateLeft
PAUSE BtwnPulses - (distance / Ms20)
NEXT
ENDIF
RETURN
' -----[ Subroutines - Face_Closest_Object ]----------------------------------
' Scan for closest object using a Ping))) rangefinder mounted on a standard
' servo. Locate the middle fo the object, and turn Boe-Bot to face it.
Face_Closest_Object:
' Initialize sweep increment.
sweepInc = Increment
' Start Servo rotated to the far right.
pingDir = LimitRight
GOSUB Point_At_PingDir
' Make minDist large and sweepDir positive (0). Single_Sweep sweeps
' left -> right while measuring object distances and stores the direction
' of the closest object in markDir.
minDist = 65535
sweepDir = Positive
GOSUB Single_Sweep
' Point the servo in the direction of the closest distance measurement.
pingDir = markDir
GOSUB Point_At_PingDir
' Scan to find object's right side.
GOSUB Find_Right_Side
IF edgesFound = 0 THEN RETURN
' Point the servo in the direction of the closest distance measurement.
pingDir = markDir
GOSUB Point_At_PingDir
' Scan to find object's right side.
GOSUB Find_Left_Side
IF edgesFound = 0 THEN RETURN
' Average the angles to the object's left and right sides. That's the
' middle of the object. Point rangefinder in that direction.
pingDir = leftMark + rightMark / 2
GOSUB Point_At_PingDir
' At this point, the Ping))) should be pointing directly at the closest
' object.
' Calculate the angle to the object's angle in brads, and turn the
' Boe-Bot to face that angle.
GOSUB Turn_Angle_Adjust
GOSUB BoeBot_Turn_Brads
' Face Ping))) rangefinder straight ahead.
pingDir = Center
GOSUB Point_At_PingDir
RETURN
' -----[ Subroutines - Find_Left_Side ]---------------------------------------
' Scan left until the measured distance is 10 cm beyond the closest distance,
' which is assumed to mean the object's side has been found.
' If the object's side has been found within the 180-degree field of vision,
' set edgesFound = 1 and store the pulse duration (pingDir) at which the
' object was found in the leftMark variable.
' If the side was not found by the 180-degree point in the scan, rotate the
' Boe-Bot until the edge is found, and then set edgesFound to 0 signifying
' that the scan will have to be repeated because the Boe-Bot rotated
' to find the side, which would otherwise cause the markDir variable to
' store an incorrect value.
Find_Left_Side:
sweepDir = Positive
distance = minDist
sweepInc = 1
DO UNTIL distance > minDist + 10
GOSUB Sweep_Increment
GOSUB Get_Ping_Cm
IF pingDir >= LimitLeft - 10 THEN
pingDir = LimitLeft - 50
GOSUB Point_At_PingDir
DO UNTIL distance > minDist + 10
PULSOUT BotServoLeft, RotateLeft
PULSOUT BotServoRight, RotateLeft
GOSUB Get_Ping_Cm
PAUSE 20
LOOP
edgesFound = 0
RETURN
ENDIF
LOOP
leftMark = pingDir
edgesFound = 1
RETURN
' -----[ Subroutines - Find_Right_Side ]--------------------------------------
' Mirror image of Find_Left_Side.
Find_Right_Side:
sweepDir = Negative
distance = minDist
sweepInc = 1
DO UNTIL distance > minDist + 10
GOSUB Sweep_Increment
GOSUB Get_Ping_Cm
IF pingDir <= LimitRight + 10 THEN
pingDir = LimitRight + 50
GOSUB Point_At_PingDir
DO UNTIL distance > minDist + 10
PULSOUT BotServoLeft, RotateRight
PULSOUT BotServoRight, RotateRight
GOSUB Get_Ping_Cm
PAUSE 20
LOOP
edgesFound = 0
RETURN
ENDIF
LOOP
rightMark = pingDir
edgesFound = 1
RETURN
' -----[ Subroutine - Get_Ping_Cm ]-------------------------------------------
' Gets Ping))) rangefinder measurement and converts time to centimeters.
' Distance may be declared as time to save variable space.
Get_Ping_Cm:
PULSOUT Ping, 5
PULSIN Ping, 1, time
distance = time ** CmConstant
RETURN
' -----[ Subroutines - Point_At_PingDir ]-------------------------------------
' Points servo mounted Ping))) rangefinder at an angle determined by the
' value of the pingDir variable.
Point_At_PingDir:
FOR counter = 0 TO MinSweep
' PULSOUT PingServo, pingDir ' Forum: removed & replaced by next 2 lines
pingRev = Limits - pingDir
PULSOUT PingServo, pingRev
PAUSE BtwnPulses
NEXT
RETURN
' -----[ Subroutine - Polar_To_Cartesian ]------------------------------------
' Calculates x and y (Cartesian coordinates) given distance and angle
' (polar coordinates).
Polar_To_Cartesian:
' Calculate left/right component.
x = COS angle ' Polar to Cartesian
xSign = x.BIT15 ' Store sign bit
x = ABS(x) */ SinCOsTo256 ' Polar to Cartesian continued
x = distance */ x
IF xSign = negative THEN x = -x ' Correct sign with sign bit
' Calculate straight ahead component.
y = SIN angle ' Polar to Cartesian
ySign = y.BIT15 ' Store sign bit
y = ABS(y) */ SinCOsTo256 ' Polar to Cartesian continued
y = distance */ y
IF ySign = negative THEN y = -y ' Correct sign with sign bit
RETURN
' -----[ Subroutines - Single_Sweep ]-----------------------------------------
' Do one sweep, and find the closest distance measurement and the
' pulse value that points the servo in that direction.
Single_Sweep:
DO UNTIL pingDir >= LimitLeft
GOSUB Sweep_Increment
GOSUB Get_Ping_Cm
IF distance < minDist THEN
minDist = distance
markDir = pingDir
ENDIF
LOOP
RETURN
' -----[ Subroutine - Sweep_Increment ]---------------------------------------
' Increment/decrement the position of the servo that directs the Ping)))
' rangefinder. When pingDir goes outside either LimitRight or LimitLeft,
' the sweep direction toggles.
Sweep_Increment:
' Change sweepDir for adding/subtracting increment if at rotation limit.
IF pingDir <= LimitRight THEN
sweepDir = Positive
ELSEIF pingDir >= LimitLeft THEN
sweepDir = Negative
ENDIF
' Add/subtract increment to/from pingDir.
IF sweepDir = negative THEN
pingDir = pingDir - sweepInc
ELSEIF sweepDir = Positive THEN
pingDir = pingDir + sweepInc
ENDIF
' Send positioning pulse to Ping))) Mounting Bracket servo.
' PULSOUT PingServo, pingDir ' Forum: removed & replaced by next 2 lines
pingRev = Limits - pingDir
PULSOUT PingServo, pingRev
RETURN
' -----[ Subroutines - Turn_Angle_Adjust ]------------------------------------
' Adjusts required turn angle based on 7 mm offset of Ping))) rangefinder from
' Boe-Bot's turning axis.
Turn_Angle_Adjust:
' Get the object's distance at its center.
GOSUB Get_Ping_Cm
' Position servo & calculate angle from far-right in brads.
angle = pingDir - 250 ** PingDirToAngle
GOSUB Polar_To_Cartesian
' Add distance between Ping))) and center of Boe-Bot axis.
y = y + PingAxleOffset
' Recalculate the turning angle with respect to Boe-Bot's turning axis.
angle = x ATN y
pulses = 64 - angle
pSign = pulses.BIT15
RETURN
Comments
I would suggest making sure your batteries are fresh. The servos can draw a significant amount of current causing the BOE to reset, restarting the program. I had a similar problem with Ping roaming and it took me a bit to determine the batteries were weak.
Try replacing the batteries and if the issue persists, we have another problem to sort out.
Good Luck!
Amanda
Change this section:
DO UNTIL distance <= 5 ' 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 LOOPTo this:
DO PULSOUT BotServoLeft, FwdLeftFast ' Boe-Bot forward PULSOUT BotServoRight, FwdRightFast PAUSE 20 LOOPI just removed all the distance checks and calculations once the robot starts moving toward the target. The robot will just keep going without looking where it's going anymore.