All,
Here is the final base code for X-Rover. The only thing that will change is the pin assignments to match the Lynxmotion Tutorials. Lynxmotion will be posting a tutorial, assembly guide, and code for you to be able to complete this project yourself. I will post new video’s in the next day or so to show the behavior of this charming little robot with the new Lynx 4WD Rover Aluminum Chassis.
Also for those of you who are interested I am using a piece of code on this bot that makes very interesting R2D2 type noises by intentionally using out of range numbers in the sound command. I was looking for something like this and was very pleased on how it came out when I wrote it myself! I will post that code snippet on stand alone thread tomorrow.
Mark
[code]’
’ X-Rover Autonomous Color Tracking Robot
’ Mark Fedasiuk 10/25/09
’ Code is nearly complete!
’ I have re-coded X-Rover to make it more generalized
’ All offsets and tuning paremters now have constants
’
enablehservo
pinp con p0
pint con p1
speaker con p9
leftd con p12
rightd con p13
atx con p11
arx con p10
baudpc con i9600
baudatom con i9600
bauddisp con i9600
tiltc con -3500 'Tilt Servo Offset
tiltd con 0 ’ Tilt Servo Dead Zone (dangerous to increase)
tiltr con 4000 ’ Tilt Range
panc con 0 ’ Pan Servo Offset
pand con 100 ’ Pan Servo Deadzone
panr con 10000 ’ Pan Range
height con 100 ’ Height Adjustment (72 centers object vertically, higher numbers lower the object in the frame) (100-120 nominal)
trackcon con 3 ’ Number of misses allowed while tracking (range 1-3)
’ Define Variables
x var byte(3) ’ X Centroid Value from CMUcam2
y var byte(3) ’ Y Centroid Value from CMUcam2
packet var byte ’ Grab a single packet of data from camera
pan var sword ’ Pan Camera
tilt var sword ’ Tilt Camera
stepp var word ’ Pan Servo Step Size
stept var word ’ Tilt Servo Step Size
stepsp var word ’ Pan Servo Step Speed
stepst var word ’ Tilt Servo Step Speed
posp var word ’ Pan Position
post var word ’ Tilt Position
rtpwr var word ’ Right Drive Motor Power
lfpwr var word ’ Left DRive Motor Power
dirp var sword ’ Pan Dirction
dirt var sword ’ Tilt Direction
val var byte ’ Variable for sound routines
tracking var byte ’ Number of times to allow non tracked object (filter)
'===================================
’ MAIN
'===================================
main
’ Initialize Variables
x = 0
y = 0
packet = 0
pan = panc
tilt = tiltc
stepp = 0
stept = 0
dirp = 1
dirt = 1
tracking = trackcon
hservo [pinp\pan]
hservo [pint\tilt]
hservo [leftd\0,rightd\0]
'Play Boot sound
for val = 2 to 5
sound speaker, [val30\val1000]
next
pause 1000 'wait for camera to boot
‘’’’’’’’’’’’’’’’’’’
‘Camera Commands
‘’’’’’’’’’’’’’’’’’’
’ set delay mode to get comms working
serout atx,baudatom,“DM 4”,13] ’ delay mode serial comms…DM value is about baud/2500 4,8,16 for 9600,19200, and 38400 baud
gosub ackno
serout atx,baudatom,“OM 0 3”,13] ’ output masinking for better tracking speed
gosub ackno
serout atx,baudatom,“NF 2”,13] ’ noise filtering default 2, 2-4 good range
gosub ackno
'serout atx,baudatom,“CR 19 32”,13] ’ used to turn off auto gain on cam
'gosub ackno
’ Play Sounds to let you know that the atom pro is properly talking to the CMUcam2
for val = 5 to 12 ’ 10 was 28
sound speaker, [val5\val(random tca)]
next
‘’’’’’’’’’’’’’’’’’’
’ Color Tracking
‘’’’’’’’’’’’’’’’’’’
'track the color red with track color command note not using command subroutine
serout atx,baudatom,"tc “,dec 120,” “,dec 240,” “,dec 0,” “,dec 40,” “,dec 0,” ",dec 40,13]
gosub ackno
‘’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’
’ Scanning MODE NOT TRACKING
‘’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’
loop
gosub Tpacket
if pan > (panc + panr) OR pan < (panc - panr) then
dirp = dirp * -1
endif
if tilt > (tiltc + tiltr) OR tilt < (tiltc - tiltr) then
dirt = dirt * -1
endif
if x(tracking) = 0 AND y(tracking) = 0 AND tracking > 0 then
tracking = tracking -1
goto loop
elseif x(tracking) = 0 AND y(tracking) = 0
'tracking = 0
if tilt > (tiltc + tiltr) then
tilt = (tiltc + tiltr)
elseif tilt < (tiltc -tiltr)
tilt = (tiltc -tiltr)
endif
pan = pan + dirp * 400
tilt = tilt + dirt * 150
hservo [pinp\pan,pint\tilt,leftd\4000,rightd-4000] ’ scan head and body and wag tail when lost track
goto loop
endif
‘’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’
’ AUTONOMOUS MODE TRACKING
‘’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’’
tracking = trackcon ’ Reset Tracking Filter
stepp = abs(x(tracking)-45)*30 ’ was 25 Pan Speed
stept = abs(y(tracking)-height)*10 ’ was 10 Tilt Speed
'get hservo pan position
if x(tracking) < 44 then
pan = (pan - stepp) min -panr
elseif x(tracking) > 46
pan = (pan + stepp) max panr
else
stepp = 0
endif
posp = abs(pan - panc)*1 max 8000 ’ turn acceleration multiplier
'serout s_out,baudpc,[sdec pan," ",sdec posp,13]
if pan > (panc + pand) then
rtpwr = -posp
lfpwr = posp
elseif pan < (panc - pand)
rtpwr = posp
lfpwr = -posp
else
rtpwr = 0
lfpwr = 0
endif
if y(tracking) < (height - 1) then
tilt = (tilt + stept) max (tiltc + tiltr)
elseif y(tracking) > (height + 1)
tilt = (tilt - stept) min (tiltc - tiltr)
else
stept = 0
endif
post = abs(tilt - tiltc)*3 max 8000 ’ drive acceleration multiplier
'serout s_out,baudpc,[sdec tilt," ",sdec post,13]
if tilt > (tiltc + tiltd) then
rtpwr = (rtpwr + post)
lfpwr = (lfpwr + post)
elseif tilt < (tiltc - tiltd)
rtpwr = (rtpwr - post)
lfpwr = (lfpwr - post)
else
rtpwr = 0
lfpwr = 0
endif
'serout s_out,baudpc,[dec posp," ",dec post,13]
hservo [pinp\pan,pint\tilt,rightd\rtpwr,leftd\lfpwr] ’ Drive the Servos while tracking object
goto loop
end
‘’’’’’’’’’’’’=================================
‘’’’’’’’’’’’’====== SUBROUTINES ==========
‘’’’’’’’’’’’’=================================
’ wait for ACK back
ackno
serin arx, baudatom, [wait(“ACK”)]
serin arx, baudatom, [packet] 'grabs the return after the ACK that confused me! this was the big fix
return
'retrieves a T type packet
Tpacket
serin arx, baudatom, [packet,dec x(tracking),dec y(tracking)]
'serout s_out,baudpc,[13,"T “,dec3 x(tracking)\3,” ",dec3 y(tracking)\3,13]
return
[/code]
