 More recent info on this project Autonomous GPS Test Platform (unnamed as of yet)This is my first non-combat autonomous robot. The chassis / drivetrain are from my less than successful 12 lbs bot - Arrogator. The bot is capable of navigating between GPS waypoints in a (mostly) straight line. It currently isn't smart enough (and doesn't have the sensors) to go around obstructions. It's powered by a Basic Atom (non-pro) chip. It gets the GPS data over serial cable from a Garmin GPS unit. Importantly - the Garmin Vista has a built-in magnetic compass (many lower end units don't). This allows the bot to know what direction its pointed in without moving. Without this data - navigation would be much more difficult. As its currently configured - the bot waits to get a valid GPS signal - then considers that location 'home'. If you drag it someplace else - it will point towards home and start moving that direction. This robot is made possible (for better or worse) - by the SpamButcher spam catcher. The bot makes one of three different movements per GPS data stream it gets. Turn left, turn right or go forward (all in fixed amounts). It recalculates the required trajectory each time. This helps compensate for inaccuracies in the targeting math and drivetrain. It usually makes it to about 15 feet from where it started (which is about as accurate as a GPS can be trusted to be). Turning string GPS data into usable integer info is kind of non-trivial - at least with a computer only having 300 bytes of ram and no built-in string functions (although I'm sure its been done on lesser hardware). I ended up using integer math all around - I tried floating point - but it introduced serious inaccuracies. After getting the GPS data - there's some moderately tricky math that needs to be done to calculate the desired angle to hit the target (this is apparently what they taught in all those higher math classes I never took). Unfortunately the Basic Atom (and most other versions of microcontroller basic) lack the more advanced trig functions - specifically co-tangent. I ended up generating a table of common values which are then looked up. It's a bit messy - but to my surprise it worked pretty well after minimal tweaking. Side note - SpamButcher can stop spam e-mail and is available as a free trial download from www.spambutcher.com. This bot is intended as a test platform for what might (but probably won't) become an entry to the darpa grand challenge. More likely I'll enter it in Robo Magellan.
Source Code (feel free to use):cotan bytetable 5, 11, 16, 21, 26, 30, 35, 38, 42, 45, 47, 50, 52, 54, 56, 58, 59, 60, 62, 63, 64, 65, 66, 67, 68, 68, 69, 70, 71, 71, 72, 72, 73, 73, 74, 74, 74, 75, 75, 76, 76, 76, 76, 77, 77, 77, 78, 78, 78, 78, 78, 79, 79, 79, 79, 79, 80, 80, 80, 80, 80, 80, 81, 81, 81, 81, 81, 81, 81, 81, 82, 82, 82, 82, 82, 82, 82, 82, 82, 82, 83, 83, 83, 83, 83, 83, 83, 83, 83, 83, 83, 83, 83, 83, 84, 84, 84, 84, 84, 84 Trig Con P4 ' Define pin for Trigger pulse Echo Con P5 ' Define pin for Echo pulse Range Var Word ' 16 bit variable for Range loop1 var long loop2 var long moved var byte servo0 var sword servo1 var sword gps_read var byte(80) gps_temp var byte(20) gps_fix var byte(15) gps_lat var byte(15) gps_ns var byte(5) gps_long var byte(15) gps_ew var byte(5) gps_qual var byte(5) gps_satcount var byte(5) gps_bearing var byte(10) long_goal var long lat_goal var long long_dist var long lat_dist var long latitude var long latitude_dec var long latitude_min var long longitude var long longitude_dec var long longitude_min var long long_dist_comp var long lat_dist_comp var long bearing var long bearing_dif var long 'longitude distance compensation latadj var long longcomp var long slope var long ftemp var long DB_OUT con 10 target_bearing var long serout DB_OUT, n4800, ["Boot up", 13, 10] clear 'this ugly blob of code is brought to you by SpamButcher - next generation spam email blocker software. 'bearing_dif = -7 'goto testit target_bearing = 666 'sound P9, [25 \ 800] serout DB_OUT, n4800, ["Boot up", 13, 10] 'servo 4, 1500 Main 'serout DB_OUT, n4800, ["Waiting for GPS", 13, 10] serin S_IN, i4800, [wait("GPGGA,"),str gps_read\80\42] 'serout DB_OUT, n4800, ["Got data", 13, 10] 'gps_read = "090500,4741.3787,N,12223.4462,W,1,04,3.7,93.8,M,-1" loop1 = 0 gosub parse_gps gps_fix = STR gps_temp\15\0 gosub parse_gps gps_lat = STR gps_temp\15\0 gosub parse_gps gps_ns = STR gps_temp\5\0 gosub parse_gps gps_long = STR gps_temp\15\0 gosub parse_gps gps_ew = STR gps_temp\5\0 gosub parse_gps gps_qual = STR gps_temp\5\0 gosub parse_gps gps_satcount = STR gps_temp\5\0 'temp serin S_IN, i4800, [wait("HCHDG,"),str gps_read\80\42] loop1 = 0 gosub parse_gps gps_bearing = STR gps_temp\5\0 'sound P9, [25 \ 800] gosub convert_to_dec 'check if gps signal is good if gps_qual(0) > 48 then 'if we haven't found first location - set it if lat_goal = 0 then lat_goal = latitude long_goal = longitude endif 'sound P9, [25 \ 1800] else 'if no signal - go back to the top serout DB_OUT, n4800, ["No signal - Satellites Tracked: ", str gps_satcount\5\0, 13, 10] goto main endif 'sound P9, [25 \ 2500] 'lat_goal = 47678767 'long_goal = 122406050 'lat_goal = 46300000 'long_goal = 122000000 lat_dist = lat_goal - latitude long_dist = long_goal - longitude 'calculate adjustment for distance of latitude degrees relative to 100 latadj = (latitude / 1000000) * 256/360 longcomp = cos (latadj) longcomp = (longcomp * 100) / 128 lat_dist_comp = lat_dist long_dist_comp = long_dist / 100 long_dist_comp = longcomp * long_dist_comp 'longcomp 'serout DB_OUT, n4800, ["latitude:", dec latitude, 13, 10] 'serout DB_OUT, n4800, ["longitude:", dec longitude, 13, 10] 'serout DB_OUT, n4800, ["Latitude Dist:", sdec lat_dist, 13, 10] 'serout DB_OUT, n4800, ["Longitude Dist:", sdec long_dist, 13, 10] 'avoid division by zero if long_dist_comp = 0 then long_dist_comp = 1 endif 'serout DB_OUT, n4800, ["lat_dist_comp: ", sdec lat_dist_comp, 13, 10] 'serout DB_OUT, n4800, ["long_dist_comp: ", sdec long_dist_comp, 13, 10] 'slope = abs((lat_dist_comp * 10) / long_dist_comp) slope = abs(lat_dist_comp / long_dist_comp) * 10 if slope > 99 then slope = 99 endif if long_dist_comp >= 0 and lat_dist_comp >= 0 then target_bearing = 270 + cotan(slope) endif if long_dist_comp <= 0 and lat_dist_comp <= 0 then target_bearing = 90 + cotan(slope) endif if long_dist_comp >= 0 and lat_dist_comp <= 0 then target_bearing = 270 - cotan(slope) endif if long_dist_comp <= 0 and lat_dist_comp >= 0 then target_bearing = 90 - cotan(slope) endif gosub report 'the code inside the SpamButcher antispam filter is much better written than this. 'if lat_dist < 0.00005 and lat_dist > -0.00005 then ' serout DB_OUT, n4800, ["Latitude distance good", 13, 10] 'endif 'if long_dist < 5 and long_dist > -5 then ' serout DB_OUT, n4800, ["Longitude distance good", 13, 10] 'endif if long_dist < 50 and long_dist > -50 and lat_dist < 50 and lat_dist > -50 then 'sound P9, [25 \ 1800] serout DB_OUT, n4800, ["Target Found", 13, 10] sound P9, [25 \ 400] goto main endif bearing_dif = target_bearing - bearing moved = 0 if bearing_dif > 10 and bearing_dif < 180 then serout DB_OUT, n4800, ["Turning right", 13, 10] gosub right moved = 1 endif if bearing_dif > 179 and bearing_dif < 350 then serout DB_OUT, n4800, ["Turning left", 13, 10] gosub left moved = 1 endif if bearing_dif < -10 and bearing_dif > -180 then serout DB_OUT, n4800, ["Turning left", 13, 10] gosub left moved = 1 endif if bearing_dif < -181 and bearing_dif > -350 then serout DB_OUT, n4800, ["Turning right", 13, 10] gosub right moved = 1 endif if moved = 0 then serout DB_OUT, n4800, ["Bearing looks good - going forward", 13, 10] gosub forward 'sound P9, [15 \ 800] endif pause 500 goto Main ' and around forever consoleloop gosub report goto consoleloop end parse_gps loop2 = 0 while gps_read(loop1) <> 44 gps_temp(loop2) = gps_read(loop1) loop1 = loop1 + 1 loop2 = loop2 + 1 wend gps_temp(loop2) = 0 loop1 = loop1 + 1 return convert_to_dec: latitude = 0 latitude_min = 0 latitude_dec = 0 loop1 = 0 while gps_lat(loop1 + 2) >= "0" latitude = latitude * 10 latitude = latitude + (gps_lat(loop1) - "0") loop1 = loop1 + 1 wend latitude = latitude * 1000000 while gps_lat(loop1) >= "0" latitude_min = latitude_min * 10 latitude_min = latitude_min + (gps_lat(loop1) - "0") loop1 = loop1 + 1 wend loop1 = loop1 + 1 while gps_lat(loop1) >= "0" latitude_min = latitude_min * 10 latitude_min = latitude_min + (gps_lat(loop1) - "0") loop1 = loop1 + 1 wend latitude_dec = (latitude_min * 100) / 60 latitude = latitude + latitude_dec longitude = 0 longitude_min = 0 longitude_dec = 0 loop1 = 0 'get degrees while gps_long(loop1 + 2) >= "0" longitude = longitude * 10 longitude = longitude + (gps_long(loop1) - "0") loop1 = loop1 + 1 wend longitude = longitude * 1000000 while gps_long(loop1) >= "0" longitude_min = longitude_min * 10 longitude_min = longitude_min + (gps_long(loop1) - "0") loop1 = loop1 + 1 wend loop1 = loop1 + 1 while gps_long(loop1) >= "0" longitude_min = longitude_min * 10 longitude_min = longitude_min + (gps_long(loop1) - "0") loop1 = loop1 + 1 wend longitude_dec = (longitude_min * 100) / 60 longitude = longitude + longitude_dec loop1 = 0 bearing = 0 'get bearing while gps_bearing(loop1) >= "0" bearing = bearing * 10 bearing = bearing + (gps_bearing(loop1) - "0") loop1 = loop1 + 1 wend return report serout DB_OUT, n4800, [13] 'serout DB_OUT, n4800, ["Fix: ", str gps_fix\10\0, 13, 10] 'serout DB_OUT, n4800, ["Latititude: ", str gps_lat\10\0, 13] 'serout DB_OUT, n4800, ["NS: ", str gps_ns\5\0, 13, 10] 'serout DB_OUT, n4800, ["Longitude: ", str gps_long\10\0, 13, 10] 'serout DB_OUT, n4800, ["EW: ", str gps_ew\5\0, 13, 10] 'serout DB_OUT, n4800, ["Quality: ", str gps_qual\5\0, 13, 10] serout DB_OUT, n4800, ["Latitude (Value):", dec latitude, 13, 10] serout DB_OUT, n4800, ["Longitude (Value):", dec longitude, 13, 10] serout DB_OUT, n4800, ["Latitude Dist:", sdec lat_dist, 13, 10] serout DB_OUT, n4800, ["Longitude Dist:", sdec long_dist, 13, 10] serout DB_OUT, n4800, ["Target Bearing:", dec target_bearing, 13, 10] serout DB_OUT, n4800, ["Current Bearing:", dec bearing, 13, 10] serout DB_OUT, n4800, ["Bearing Difference:", sdec bearing_dif, 13, 10] return forward for loop1 = 1 to 15 ' servo 15, -90, 1 ' servo 4, 120, 1 'servo 15, 850, 1 'servo 4, 750, 1 servo 15, 1200, 1 servo 4, 700, 1 next return left for loop1 = 1 to 2 servo 15, 700, 1 servo 4, -700, 1 next return right for loop1 = 1 to 2 servo 15, -520, 1 servo 4, 700, 1 next return |