Help with program flow in large BS2 program
Rocko
Posts: 8
Hello everyone,
I need help with writing the program flow for a large BS2 program for my Boe-Bot.
A little background on this project:
I've been involved in a robotics competition at my robotics club for over 6 months now.
The competition (called, "The Challenge") involves using a robot (I am using a Boe-Bot) to:
1) Locate a 1-1/2" sq. block of wood on a tabletop
(I am using an SRF04 ultrasonic sensor and the code from a program called, "PingDar_GoToClosetObject.bs2".
Note: Although the "PingDar_GoToClosetObject.bs2" program uses a Parallax PingDar, I have successfully modified and run this program using a generic SRF04.
I have also modified this program so that the "PingDar" code, if you will, only goes through finding the edges & aligning itself to the block only 1 time, then it should move forward indefinitely or until:
2) It detects block. (I am using a Parallax QTI line sensor as a switch to stop the Boe-Bot at this point.)
3) gripper closes around block
4) Boe-Bot makes an About Face (turns 180* or around)
5) Boe-Bot moves forward until:
6) Parallax ColorPal detects a 3" dia. red circle on tabletop & stops
7) Boe-Bot backs up ~4" so block is on top of red circle
As the program stands now, it will successfully go through the process of locating the block and heading toward it. It then continues to move forward even though the QTI sensor has been tripped. (I know that the sensor is being tripped because I've run a DEBUG program to watch it switch on screen from 1 (no block detected) to 0 (block detected)
Here is the code:
I need help with writing the program flow for a large BS2 program for my Boe-Bot.
A little background on this project:
I've been involved in a robotics competition at my robotics club for over 6 months now.
The competition (called, "The Challenge") involves using a robot (I am using a Boe-Bot) to:
1) Locate a 1-1/2" sq. block of wood on a tabletop
(I am using an SRF04 ultrasonic sensor and the code from a program called, "PingDar_GoToClosetObject.bs2".
Note: Although the "PingDar_GoToClosetObject.bs2" program uses a Parallax PingDar, I have successfully modified and run this program using a generic SRF04.
I have also modified this program so that the "PingDar" code, if you will, only goes through finding the edges & aligning itself to the block only 1 time, then it should move forward indefinitely or until:
2) It detects block. (I am using a Parallax QTI line sensor as a switch to stop the Boe-Bot at this point.)
3) gripper closes around block
4) Boe-Bot makes an About Face (turns 180* or around)
5) Boe-Bot moves forward until:
6) Parallax ColorPal detects a 3" dia. red circle on tabletop & stops
7) Boe-Bot backs up ~4" so block is on top of red circle
As the program stands now, it will successfully go through the process of locating the block and heading toward it. It then continues to move forward even though the QTI sensor has been tripped. (I know that the sensor is being tripped because I've run a DEBUG program to watch it switch on screen from 1 (no block detected) to 0 (block detected)
Here is the code:
' -----[ Title ]--------------------------------------------------------------
' PARTS_Challenge.bs2
' {$STAMP BS2} ' Target device = BASIC Stamp 2
' {$PBASIC 2.5} ' Language = PBASIC 2.5
' -----[ I/O Definitions ]----------------------------------------------------
' ColorPal pin
ColorPal PIN 15
' 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
' Gripper servo pins
GrprServo PIN 11 ' directs gripper servo motor
' QTI Line Sensor pins
FQTIPwr PIN 6 ' front QTI sensor power
FQTIIn PIN 3 ' front QTI sensor input
StartLED PIN 0 ' display start delay
' -----[ Constants ]----------------------------------------------------------
' Baud rate for ColorPal
baud CON 119 + 32768
' Ping))) mounting bracket constants
LimitLeft CON 1200 ' Bracket 90-degrees Left
LimitRight CON 275 ' 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 850 ' Fast settings - straight ahead
FwdRightFast CON 660
RotateRight CON 765 ' Boe-Bot rotate right pulse
RotateLeft CON 735 ' Boe-Bot rotate left pulse
'Gripper servo constants
Open CON 800 ' gripper open position
Closed CON 1200 ' gripper closed position
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
QTIRaw VAR Word ' front QTI sensor raw reading
QTIDig VAR Bit ' decoded sensor value
QTIBit VAR Bit
temp VAR Byte
pulseCount VAR Byte ' Used for measuring turns
red VAR Word ' Received RGB values from ColorPAL.
grn VAR Word
blu VAR Word
' -----[ 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
' PAUSE 2000 ' Removed (Delay program start by 2 s.)
' PULSOUT PingServo, pingDir ' FORUM said remove & replace with next 2 lines
pingRev = Limits - pingDir
PULSOUT PingServo, pingRev
' -----[ Main Routine ]-------------------------------------------------------------------------
' This first part of this program is from 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.
' Note: This program has been modified to use a generic SRF04 ultra sonic sensor and travel forward
' until a QTI line Sensor is tripped.
GOSUB Get_Ping_Cm ' First distance measurement
'DO UNTIL distance <= 5 ' Removed from GoToClosestObject.bs2 (Repeat until distance <= 5 cm)
DO
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 ' Removed from GoToClosestObject.bs2 (Drive toward object)
DO
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 ' Removed from GoToClosestObject.bs2 (Exit if distance increasing)
LOOP
LOOP
GOSUB Read_QTI_Sensor ' Read QTI sensor value
DO UNTIL QTIDig = 0
PULSOUT BotServoLeft, FwdLeftFast ' Boe-Bot forward
PULSOUT BotServoRight, FwdRightFast
PAUSE BtwnPulses - (distance / Ms20) ' 20 ms pause between pulses
LOOP
DO
GOSUB Hold_Position ' Hold position
LOOP
DO
GOSUB Close_Gripper ' Close Gripper
LOOP
DO
GOSUB Turn_Around ' About 180 degree Turn Around to the Right
LOOP
DO
GOSUB Keep_Straight ' Keep Straight
LOOP
DO
GOSUB Reset_ColorPal ' ColorPal
LOOP
DO
GOSUB color_check ' ColorPal
LOOP
DO
IF (red <= 124) AND (grn >= 100) AND (blu >= 125) THEN ' Current tabletop color values here
GOSUB Keep_Straight
ELSEIF (red > 125) AND (grn < 50) AND (blu < 50) THEN ' Red Circle values
GOSUB Hold_Position
ENDIF
LOOP
DO
GOSUB Back_Up
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
' -----[ Subroutine - Read_QTI_Sensor ]-------------------------------------------
' Activates and reads QTI sensor
Read_QTI_Sensor:
HIGH FQTIPwr ' activate QTI sensor
HIGH FQTIIn ' discharge caps
PAUSE 1
RCTIME FQTIIn, 1, QTIRaw ' read QTI sensor
LOW FQTIPwr ' deactivate QTI sensor
LOOKDOWN QTIRaw, >=[1000, 0], QTIDig ' convert reading to bit
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
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 ' This line of code 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
' -----[ Subroutines - Hold_Position ]------------------------------------
Hold_Position:
FOR pulseCount = 0 TO 5
PULSOUT BotServoLeft, 750
PULSOUT BotServoRight, 750
PAUSE 20
NEXT
RETURN
' -----[ Subroutines - Close_Gripper ]------------------------------------
Close_Gripper:
FOR pulseCount = 0 TO 5
PULSOUT GrprServo, Closed ' Close signal -> Gripper servo
PAUSE BtwnPulses ' 20 ms delay between pulses
NEXT
RETURN
' -----[ Subroutines - Turn_Around ]------------------------------------
Turn_Around: 'About 180 degree Turn Around to the Right
FOR pulseCount = 0 TO 170
PULSOUT BotServoLeft, 850
PULSOUT BotServoRight, 850
PAUSE 20
NEXT
RETURN
' -----[ Subroutines - Keep_Straight ]------------------------------------
Keep_Straight:
FOR pulseCount = 0 TO 20
PULSOUT BotServoLeft, 850 ' Left Servo Forward Pulse Value
PULSOUT BotServoRight, 650 ' Right Servo Forward Pulse Value
PAUSE 20
NEXT
RETURN
' -----[ Subroutines - Back_Up ]------------------------------------
Back_Up:
FOR pulseCount = 0 TO 10 ' Number Of Pulses To Backup
PULSOUT BotServoLeft, 650 ' Left Servo Backup Pulse Value
PULSOUT BotServoRight, 850 ' Right Servo Backup Pulse Value
PAUSE 20 ' Refresh Delay
NEXT
RETURN
' -----[ Subroutines for ColorPal ]---------------------------------------------
Reset_ColorPal:
LOW ColorPal 'Pull sio low to eliminate any residual charge.
INPUT ColorPal 'Return pin to input.
DO UNTIL ColorPal : LOOP 'Wait for pin to be pulled high by ColorPAL.
LOW ColorPal 'Pull pin low.
PAUSE 80 'Keep low for 80ms to enter Direct mode.
INPUT ColorPal 'Return pin to input.
PAUSE 10 'Pause another 10ms
RETURN
color_check:
SEROUT ColorPal, baud, ["= m !"]
SERIN ColorPal, baud, [HEX3 red, HEX3 grn, HEX3 blu] ' Receive RGB data back.
RETURN
Comments
I'll try to be more specific.
Why doesn't the Boe-Bot Hold_Position when it sees the block ?
As I stated earlier, the first part of the code works correctly; the Boe-Bot identifies the wooden block and heads towards it.
As I understand it, the way these programs work is that they go down through the program line by line, so the next thing it should do is go into the subroutine that reads the value of the QTI sensor (GOSUB Read_QTI_Sensor).
Upon returning from that subroutine, let's say the value is 1 (hasn't seen the block yet), the next thing the program does is go into the first DO LOOP, looping UNTIL the value is 0 (QTI sensor has seen block). When the value is 1, it exits that loop, and goes into the next loop in line which is Hold_Position.
Again, why doesn't the Boe-Bot Hold_Position when it sees the block ?
Thanks for any further help on this!
DO / LOOP means "loop forever" unless an EXIT (or GOTO) is executed.