This is a modified program from the Romey robot from the latest Robot mag
With this program the robot does a very good job at staying and getting out of trouble.
[code]’ ------------------------------------------------------------------------------
’ I/O Definitions
’ ------------------------------------------------------------------------------
Pin_Ping CON 15
Pin_Turret CON 11
' GP2D12 IR sensors (analog)
Pin_Right CON 19
Pin_Left CON 18
VFD con p12 ' VFD Serial I/O Pin
’ ------------------------------------------------------------------------------
’ Constants
’ ------------------------------------------------------------------------------
' Servo (turret) positions (adjust for your own robot)
' Center position
TurretCenter CON 125
' Scan positions R45 R90 Center L45 L90 | L45 L90 Center R45 R90
ScanPos LongTable 925, 1900, TurretCenter, -800, -1700, -800, -1700, TurretCenter, 925, 1900
' Min/Max Distances
PingMin CON 35
LeftMin CON 20
LeftMax CON 33
RightMin CON 20
RightMax CON 33
TRUE CON 1
FALSE CON 0
'VFD Baud rate
Baud CON i38400 ' 338.4 Kbps (BasicAtom)
’ ------------------------------------------------------------------------------
’ Variables
’ ------------------------------------------------------------------------------
PingDistance VAR word
Gp2DistRight VAR word
Gp2DistLeft VAR word
Gp2DistCenter VAR word
NavCount VAR word
Rolling VAR bit
ScanStart var byte
ScanStop var byte
ScanSeed var long
Duration VAR word
Direction VAR byte
x VAR word
’ ------------------------------------------------------------------------------
’ Initialization
’ ------------------------------------------------------------------------------
'Start up VFD display
SEROUT VFD, Baud, [001bh,0040h,000ch] ' Initialize Display
PAUSE 200 ' Allow Time To Settle
SEROUT VFD, Baud, "Initalizing Roaming Program"]
' Center the turret servo
servo Pin_Turret,TurretCenter,50
' Generate a (somewhat) random seed by using the Ping
gosub GetPingDistance[Pin_Ping], ScanSeed
pause 1000
’ ------------------------------------------------------------------------------
’ Main Program Loop
’ ------------------------------------------------------------------------------
Main
if NavCount = 8 then
’ Execute the Navigate behavior every 8th time
gosub Navigate
NavCount = 0
endif
' Execute the Danger and Avoid behaviors
gosub Danger
gosub Avoid
NavCount = NavCount + 1
'Show navcount
SEROUT VFD, Baud, [000ch, "IR Sensors ", sdec Gp2DistLeft," ", sdec Gp2DistRight," Sonar ", sdec PingDistance, " cm"]
goto Main
’ ------------------------------------------------------------------------------
’ Behaviors
’ ------------------------------------------------------------------------------
’
’ Navigate Behavior
’
Navigate
gosub GetPingDistance[Pin_Ping], PingDistance
if PingDistance > 0 and PingDistance < PingMin then
' Backup
gosub BackAway
' Look for a clear path
gosub Scanner
' BEWARE!
' The documentation says you need a THEN for the ELSEIF clauses. This will
' generate a syntax error. Use THEN for the first IF clause only.
if Direction = 0 or Direction = 8 then
gosub SpinRight[500]
elseif Direction = 1 or Direction = 9
gosub SpinRight[1000]
elseif Direction = 3 or Direction = 5
gosub SpinLeft[500]
elseif Direction = 4 or Direction = 6
gosub SpinLeft[1000]
elseif Direction = 10
gosub SpinLeft[1500]
endif
endif
gosub Forward
return
’
’ Look for a clear path using Ping
’
Scanner
' Select a random scan direction to keep it interesting
ScanSeed = Random ScanSeed
if ScanSeed // 2 = 0 then
ScanStart = 0
ScanStop = 4
else
ScanStart = 5
ScanStop = 9
endif
' Look for a suitible direction using the servo scan table
Direction = 10
for x = ScanStart to ScanStop
servo Pin_Turret, ScanPos(x), 50
if x <> 2 and x <> 7 then
gosub GetPingDistance[Pin_Ping], PingDistance
if PingDistance > 45 then
Direction = x
x = ScanStop + 1
endif
endif
next
' Back to center
servo Pin_Turret, TurretCenter, 50
return Direction
’
’ Avoid Behavior
’
Avoid
if Rolling = TRUE then
gosub GetProximityData
if Gp2DistLeft > 0 and Gp2DistLeft < Leftmax then
' Something on the left, turn right a bit
gosub SpinRight[300]
elseif Gp2DistRight > 0 and Gp2DistRight < RightMax
' Something on the right, turn left a bit
gosub SpinLeft[300]
endif
endif
return
Danger
if Rolling = TRUE then
gosub GetProximityData
if Gp2DistRight < RightMin then
gosub BackAway
gosub SpinLeft[300]
elseif Gp2DistLeft < Leftmin
gosub BackAway
gosub SpinRight[300]
endif
endif
return
’ ------------------------------------------------------------------------------
’ Motor Control Subroutines
’ ------------------------------------------------------------------------------
’
’ Move Forward
’
Forward
servo p13, 520
servo p14, 500
Rolling = TRUE
return
’
’ Back up a small distance
’
BackAway
gosub Halt
gosub Backup
pause 1000
gosub Halt
return
’
’ Move Reverse
’
Backup
servo p13, -520
servo p14, -500
Rolling = TRUE
return
’
’ Stop motors
’
Halt
servo p13, 0
servo p14, 0
pause 500
Rolling = FALSE
return
’
’ Spin Right
’
SpinRight[Duration]
gosub Halt
servo p13, -520 'right servo back
servo p14, 500 'left servo forward
if Duration > 0 then
pause Duration
endif
Rolling = TRUE
return
’
’ Spin Left
’
SpinLeft[Duration]
gosub Halt
servo p13, 520 'right servo forward
servo p14, -500 'right servo back
if Duration > 0 then
pause Duration
endif
Rolling = TRUE
return
’ ------------------------------------------------------------------------------
’ Sensor Subroutines
’ ------------------------------------------------------------------------------
’
’ Get GP2D12 distance data
’
GetProximityData
gosub GetGp2Distance[Pin_Right], Gp2DistRight
pause 10
gosub GetGp2Distance[Pin_Left], Gp2DistLeft
pause 10
return
’
’ Ping Ultrasonic Distance Sensor
’
PingPin var byte ’ Ping I/O pin
PingReturn var word ’ Pint return duration
PingDist var word ’ Computed distance
GetPingDistance[PingPin]
' Initialize PING IO pin
low PingPin
' Send the Ping a 5 uS pulse
PULSOUT PingPin, 10
' Read the pulse echo return time
' BEWARE!
' The Direction parameter documentation (for ATOM Pro) appears to be reversed
' We need to use 0 (instead of 1) for a 0-1-0 pulse
' Also the documentation for the TimeoutMultiplier parameter is wrong. It is
' actually the time in uS and not a multipier.
PULSIN PingPin, 0, Done, 20000, PingReturn
' Convert to centimeters (use 889 for inches)
if PingReturn > 0 then
PingDist = (PingReturn */ 2257 ) / 2
endif
Done
return PingDist
’
’ Sharp GP2D12 Infrared Distance Sensor
’
Gp2Pin var byte
Gp2Volts var word
Gp2Index var float
Gp2Dist var word
Gp2Table bytetable 80,80,80,80,80,80,80,80,80,78, |
76,74,72,70,68,66,64,62,60,59, |
58,57,55,53,52,51,50,49,48,47, |
45,43,42,41,40,39,38,37,35,33, |
32,31,30,30,29,29,28,28,27,27, |
26,26,26,25,25,25,24,24,24,23, |
23,22,22,21,21,20,20,20,19,19, |
18,18,18,17,17,16,16,16,15,15, |
15,14,14,13,13,13,12,12,11,11, |
11,10,10,10,10,10,10,10,10,10
GetGp2Distance[Gp2Pin]
adin Gp2Pin, Gp2Volts
if Gp2Volts > 512 then
' Too close
Gp2Dist = 0
else
Gp2Index = ((TOFLOAT Gp2Volts) / 5.12) ' Limit value to <200 values
Gp2Dist = Gp2Table(TOINT Gp2Index) ' Convert A/D to measurement
endif
return Gp2Dist
[/code][/code]
) But if you’re going to spend that much, you can get quite a bit more for your money (unless it’s actually the small size that had you looking at this, which I admit is very cool)