Thanks Publison, here's an updated demo I promised you (Erco's laser sensor).
Martin_H
Posts: 4,051
Thanks for the better reflector tape. I'm getting a ten foot range with it pretty easily. In this demo I use the Boe-bot and laser to fetch a target from across the kitchen and bring it back.
If I didn't have grout lines in the floor I could have use line sensors to recognize a home area as well. But I was able to tighten up the code enough that I no longer needed the ServoPal to tend the servos as well. The code is below and represents about five hours work start to finish.
If I didn't have grout lines in the floor I could have use line sensors to recognize a home area as well. But I was able to tighten up the code enough that I no longer needed the ServoPal to tend the servos as well. The code is below and represents about five hours work start to finish.
' ================================================== ============================
'
' File...... FetchTargetAndFindBeacon.BS2
' Purpose... Find a target using the Ping))) and laser, then returns to
' the beacon
' Authors... Martin Heermance.
' E-mail.... mheermance@gmail.com
' Started... 2014 September 6th
' Updated... 2014 September 7th
'
' {$STAMP BS2}
' {$PBASIC 2.5}
'
' ================================================== ============================
' ------------------------------------------------------------------------------
' Program Description
' ------------------------------------------------------------------------------
' A program to locate a target using a laser, then return to the Pololu beacon
' ------------------------------------------------------------------------------
' Revision History
' ------------------------------------------------------------------------------
' 2014-09-XX Initial creation and ongoing development.
' ------------------------------------------------------------------------------
' I/O Definitions
' ------------------------------------------------------------------------------
Laser PIN 0 ' Laser input
BLeft PIN 6 ' Beacon to left indicator
BBack PIN 7 ' Beacon in back indicator
BRght PIN 8 ' Beacon to right indicator
BFront PIN 9 ' Beacon in front indicator
Buzzer PIN 10 ' pin for output sound
RMotor PIN 12 ' Pin for right servo
LMotor PIN 13 ' Pin for left servo
Gripper PIN 14 ' Pin for gripper servo
Ping PIN 15 ' Pin for PING))) Sensor
' ------------------------------------------------------------------------------
' Constants
' ------------------------------------------------------------------------------
'--------[Calibration Constants, must be adjusted for your specific robot]------
GripperOpened CON 500 ' PULSOUT pulse width to open gripper servo
GripperClosed CON 960 ' PULSOUT pulse width to close gripper servo
' These values need tweaking for each robot's servos.
SpeedF75L CON 840 ' Fast forward left
SpeedF50L CON 808 ' Med forward left
SpeedF25L CON 775 ' Slow forward left
Speed000L CON 750 ' stop left
SpeedR25L CON 710 ' slow reverse left
SpeedR50L CON 690 ' med reverse left
SpeedR75L CON 650 ' fast reverse left
SpeedF75R CON 650 ' Fast forward right
SpeedF50R CON 690 ' med forward right
SpeedF25R CON 720 ' slow forward right
Speed000R CON 750 ' Stop right
SpeedR25R CON 775 ' slow reverse right
SpeedR50R CON 810 ' med reverse right
SpeedR75R CON 850 ' Fast reverse right
'---------[Other Global Constants]----------------------------------------------
NULL CON 750 ' Pulse width for 1.5ms stop pulse.
Trigger CON 5 ' trigger pulse = 10 uS
Scale CON $200 ' raw x 2.00 = uS
RawToIn CON 889 ' 1 / 73.746 (with **)
IsHigh CON 1 ' for PULSOUT
IsLow CON 0
Success CON 0 ' Constant for zero success
ErrorBeaconLost CON -1 ' Constant for lost on move
ErrorTooClose CON -2 ' Constant for farther than max
RIGHT CON 0 ' Subscripts into arrays.
LEFT CON 1
FRONT CON 2
BACK CON 3
COM_SPEED CON 84 ' 9600 baudnon-inverted.
' ------------------------------------------------------------------------------
' Variables - for simplicty I use exactly the same variables in all modules.
' ------------------------------------------------------------------------------
' General purpose volatile variables which are modified by subroutines or tasks
ACC VAR Word
' Loop index variables and for argument passing.
X VAR Word
Y VAR Word
Z VAR Word
' DirReads is a nibble array
DirReads VAR Nib(4)
' State variables which are modified to reflect changes of robot state.
RSpeed VAR Word ' Holds current r motor pulse
LSpeed VAR Word ' Holds current l motor pulse
GripState VAR Word ' Holds current gripper pulse
' ------------------------------------------------------------------------------
' Initialization
' ------------------------------------------------------------------------------
' ------------------------------------------------------------------------------
' Program Code
' ------------------------------------------------------------------------------
' Pause at start of program to let user set down robot.
FREQOUT Buzzer, 1000, 2500
GripState = GripperOpened
GOSUB Halt ' stop robot
GOSUB FindTarget
GOSUB Fetch
GripState = GripperClosed
GOSUB Halt
GOSUB FindBeacon
END
' ------------------------------------------------------------------------------
' Subroutines
' ------------------------------------------------------------------------------
' slowly rotate until the laser target finds target
FindTarget:
RSpeed = SpeedF25R
LSpeed = SpeedR25L
DO
' Each iteration refresh the servos
GOSUB PulseServos
IF (Laser = IsLow) THEN
GOSUB Halt
FREQOUT Buzzer, 1000, 2500
RETURN
ENDIF
LOOP
' moves towards the target and stops when it is in range
Fetch:
Y = 0
DO
Y = Y + 1
' Every so often check that we still see the target
' If not then turn TO face it
IF (Y > 35 AND Laser = IsHigh) THEN
GOSUB FindTarget
Y = 0
ENDIF
' slowly move forward to the desired distance to the target
RSpeed = SpeedF50R
LSpeed = SpeedF50L
GOSUB PulseServos
' Exit if we're close to the target
GOSUB PingDistance
IF (X < 3) THEN
GOSUB Halt
RETURN
ENDIF
LOOP
' PingDistance - reads the distance to the target.
' Inputs:
' Outputs:
' X - distance in inches.
PingDistance:
Ping = IsLow ' make trigger 0-1-0
PULSOUT Ping, Trigger ' activate sensor
PULSIN Ping, IsHigh, X ' measure echo pulse
X = X */ Scale ' convert to uS
X = X / 2 ' remove return trip
X = X ** RawToIn ' convert to inches
RETURN
' Turn the robot on its axis by the amount in signed byte Arg at speed Veloc.
' Inputs:
' Arg - number of brads to turn. > 0 turns left; Arg < 0 turns right.
' Veloc - peak speed where 0 is stop and 15 is fast.
' Outputs:
' X - 0 success, -1 beacon lost, -2 closer than min
Turn:
RETURN
' Stop moving
Halt:
RSpeed = Speed000R
LSpeed = Speed000L
' Called to refresh the servos.
PulseServos:
PULSOUT RMotor, RSpeed
PULSOUT LMotor, LSpeed
PULSOUT Gripper, GripState
PAUSE 20
RETURN
' ReadBeacon - scans the beacon pins, removes spurious noise and returns value.
' Inputs:
' none
' Outputs:
' ACC - most common direction
' Z - number of occurances
ReadBeacon:
FOR Y = RIGHT TO BACK
DirReads(Y) = 0
NEXT
FOR Y = 0 TO 6
IF BFront = IsLow THEN
DirReads(Front) = DirReads(Front) + 1
ENDIF
IF BRght = IsLow THEN
DirReads(Right) = DirReads(Right) + 1
ENDIF
IF BLeft = IsLow THEN
DirReads(Left) = DirReads(Left) + 1
ENDIF
IF BBack = IsLow THEN
DirReads(BACK) = DirReads(BACK) + 1
ENDIF
NEXT
' Have a bias towards LEFT to initiate beacon search
ACC = LEFT
Z = DirReads(LEFT)
FOR Y = RIGHT TO BACK
' If this direction is more common than current max, make it max
IF DirReads(Y) > Z THEN
ACC = Y
Z = DirReads(Y)
ENDIF
NEXT
RETURN
' Find the beacon, althouh at the present no exit condition is specified
FindBeacon:
' slowly rotate until the beacon locater spots its target
RSpeed = SpeedF25R
LSpeed = SpeedR25L
DO
' Each iteration refresh the servos
GOSUB PulseServos
GOSUB ReadBeacon
IF (ACC = Back) THEN
' quickly do a 180
RSpeed = SpeedR75R
LSpeed = SpeedF75L
Y = 40
ELSEIF (ACC = Right) THEN
' rotate until the beacon is front
RSpeed = SpeedR50R
LSpeed = SpeedF50L
Y = 20
ELSEIF (ACC = Left) THEN
' rotate until the beacon is in front
RSpeed = SpeedF50R
LSpeed = SpeedR50L
Y = 20
ELSEIF (ACC = Front) THEN
' Move towards beacon
RSpeed = SpeedF75R
LSpeed = SpeedF75L
Y = 1
ENDIF
DO UNTIL Y = 0
GOSUB PulseServos
Y = Y - 1
LOOP
LOOP
Comments
Nice work!
Outstanding work being able to clock everything so smoothly with the code!
They based around a pair of Pololu IR beacons that parallax sells, here's a link: http://www.parallax.com/product/28049 . But I built the app mod board that lets me plug one into the Boe-bot, and a stand for the other one. This keeps them roughly at the same height and above the other sensors. I think anyone who's gone through the "Robotics with the Boe-Bot" book can understand how they work and put them to good use. But because they're surface mount with their own microcontroller they offload a lot of complexity from the BS2 in not much space.
That's very cool!
I bet you could get close to 15' if the reflective was not wrapped, but a 3" flat strip on the front of the target. That's how I got my best results.
Jim
I'm going to make more targets for three point navigation. So I'll try the flat strip as that's easier to fabricate than a round target anyway. I've done integer trig with the BS2 before, so how hard can triangulation be?