Ping)))Dar Go To & Find Closest Object Issues
Rocko
Posts: 8
I have the Ping)))Dar Go To & Find Closest Object program up & running on my Boe-Bot but need some help with a couple of things.
The PingDar works as it should: scans, locates each side then middle of object, aligns wheels accordingly. Then it starts forward toward object but only gets several inches before it stops and repeats the scanning/locating sequence again. Then it starts forward again and only goes another couple of inches before it stops and starts the scan again. This process continues again and again.
I have to make note here that the object is only a 1-1/2" sq. wooden block, so when the PingDar gets to within ~6", it looses "site" of the object.
Does anyone know how I can modify the code so the PingDar only makes a couple of scans and after the second scan move forward indefinitely ?
I've tried modifying the code every which way but I'm at a loss on trying to get out of this loop.
Here's the code:
Thanks for your help!
The PingDar works as it should: scans, locates each side then middle of object, aligns wheels accordingly. Then it starts forward toward object but only gets several inches before it stops and repeats the scanning/locating sequence again. Then it starts forward again and only goes another couple of inches before it stops and starts the scan again. This process continues again and again.
I have to make note here that the object is only a 1-1/2" sq. wooden block, so when the PingDar gets to within ~6", it looses "site" of the object.
Does anyone know how I can modify the code so the PingDar only makes a couple of scans and after the second scan move forward indefinitely ?
I've tried modifying the code every which way but I'm at a loss on trying to get out of this loop.
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
Thanks for your help!
Comments