Autonomous Test Platform

This is the next version of my Autonomous Test Platform. It's intent is to be a fairly rugged indoor / outdoor platform for testing various robotics technologies. It may ultimately get entered into Robo Magellan - a navigation contest for robots.

It currently has (or will have soon)
Garmin E-trex GPS
Basic Atom (28-pin)
Mini-atom BotBoard

It doesn't do too much yet. The GPS navigation was already working in a previous incarnation - so that much should be pretty easy.

It will soon have SRF04 sonar sensors. If I'm feeling really ambitious, it may get a robotic arm and video camera.

The drive motors are Black & Decker screwdrivers ($9.99 at Kmart).

It can go about 4 mph and can crawl over objects up to about 4" tall.

'Basic Atom GPS Navigation Code
'This source code brought to you by SpamButcher anti-spam software.
'SpamButcher is an inexpensive spam email blocking solution for end-users.

'Feel free to use for your own purposes (and at your own risk)

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 code sample is brought to you by the SpamButcher antispam filter. 

'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


'Be sure that the software that drives SpamButcher's email filtering software is written to tighter standards 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