Än har jag bara en rutin som beräknar körspår baserat på redskaps bredd, punkt a och punkt b.
Du som skall köra autonomt kolla på
http://code.google.com/p/ardupilot-mega ... ionPlanner
Edit: långt inlägg
Här kommer all kod hittills, fritt att använda och modifiera, kul även om ni delar med er av ändringar och tillägg.
Tillsät bara pyserial.
main.py
Kod: Markera allt
# -*- coding: utf-8 -*-
#Importera gps biblioteket.
import gps
#Importera nmea parser med serial input
#import sernmea
import tcpgps
from math import pi
import time
import serial
#Redskaps bredd i meter
w = 2
v = w - w*2 #Spegelvänd redskapsbredd
#Skapa variabler
latA = 58.9000
lonA = 12.9000
latB = 58.2000
lonB = 12.2000
global latD
global lonD
latD = 58.2000 # för att inte rikting skall få spratt vid uppstart
lonD = 12.1000
#port och baud till styrning
ser2 = serial.Serial('com2',9600)
ser2.setDTR(level=False)
time.sleep(2)
'''
skriver ut avståndet till linjen i meter.
Positivt så är vi på höger sida, negativt på vänster sida, på A mot B
'''
def xt_main():
line = tcpgps.nmea() # Hämta nuvarande position
if line is not None: # Om position finns fortsätt
global lat
global lon
lat, lon = line # Två floats från en tuple
xt = gps.cross_track( latA, lonA, latB, lonB, lat, lon) # Räkna ut spårfel
#v = w - w*2 #Spegelvänd redskapsbredd
if xt > w * 0.7: # Om spårfel är mer än 70 procent av redskaps bredd
while xt > w * 0.7:
xt = xt - w # Flytta linje med en redskaps bredd mindre
#print xt,"\r", # Skriv ut aktuellt spårfel
return xt
elif xt < v * 0.7: # Om spårfel är mer än 70 procent av redskaps bredd
while xt < -v * 0.7:
xt = xt + w # Flytta linje med en redskaps bredd mer
#print xt,"\r", # Skriv ut aktuellt spårfel
return xt
else:
#print xt,"\r",
return xt
def riktning(): # Ta reda på i vilken riktning vi färdas i förhållande till A-B linjen.
global latD
global lonD
ang = gps.bearing(latA, lonA, latB, lonB)
deg_ab = ang * (180/pi)
#print deg
ang = gps.bearing(latD, lonD, lat, lon)# Riktning från förra varvet till där vi är nu
deg_relative = ang * (180/pi)
latD = lat # updatera old
lonD = lon
skillnad = deg_relative - deg_ab
if skillnad < -180:
skillnad = skillnad + 360
elif skillnad > 180:
skillnad = skillnad - 360
if skillnad < 90 and skillnad > -90:
return True
else:
return False
while True: # Main Loop
crosstrack = xt_main()
if crosstrack is not None:
#print crosstrack,"\r",
''' Om Vi färdas från B mot A så invertera crosstrack '''
if riktning() == False and crosstrack > 0:
crosstrack = crosstrack - crosstrack * 2
#print "Back -"
elif riktning() == False and crosstrack < 0:
crosstrack = crosstrack + crosstrack * 2
#print "Back +"
pos = (crosstrack + w ) * ( 200 / ( w + w ))
pos = "%0.0f" % (pos)
print pos,"\r","\n",
pos = int(pos)
ser2.write(pos)
gps.py
Kod: Markera allt
#https://bitbucket.org/miracle2k/pyutils/src/e00c83e5c490/pyutils/gis/geodesy.py
"""Port of the "Latitude/longitude spherical geodesy formulae & scripts"
(c) Chris Veness 2002-2009, originally in JavaScript, licensed under
LPGL:
http://www.movable-type.co.uk/scripts/latlong.html
Not all functions are included (only those I needed personally so far),
and a cross-track function, as described on the page but not included
in code, was added by myself.
Some other links I stumbled over while researching this follow below -
I hope I won't need them again:
http://stackoverflow.com/questions/1299567/how-to-calculate-distance-from-a-point-to-a-line-segment-on-a-sphere/
http://stackoverflow.com/questions/1051723/distance-from-point-to-line-great-circle-functino-not-working-right-need-help
http://williams.best.vwh.net/avform.htm
http://mail.python.org/pipermail/python-list/2005-June/328382.html
http://postgis.refractions.net/pipermail/postgis-users/2009-July/023903.html
http://www.google.com/codesearch/p?hl=en&sa=N&cd=3&ct=rc#ArccXqZgcB0/source/Common/Source/Airspace.cpp (xcsoar@sourceforge)
http://mathforum.org/library/drmath/view/51785.html
http://www.physicsforums.com/showthread.php?t=178252
"""
from math import radians, degrees, sin, cos, asin, sqrt, atan2, pi
__all__ = ('EARTH_RADIUS',
'distance_haversine', 'distance_cosine', 'bearing',
'bearing_degrees', 'destination', 'cross_track',)
#EARTH_RADIUS = R = 6371.00; # kilometers (make sure this is a float)
EARTH_RADIUS = R = 6371008.7714; # Meters (make sure this is a float)
def distance_haversine(lat1, lon1, lat2, lon2):
"""Use Haversine formula to calculate distance (in km) between two
points specified by latitude/longitude (in numeric degrees).
from: Haversine formula - R. W. Sinnott, "Virtues of the Haversine",
Sky and Telescope, vol 68, no 2, 1984
http://www.census.gov/cgi-bin/geo/gisfaq?Q5.1
"""
dlat = radians(lat2 - lat1)
dlon = radians(lon2 - lon1)
lat1 = radians(lat1)
lat2 = radians(lat2)
a = sin(dlat / 2) * sin(dlat / 2) + cos(lat1) * \
cos(lat2) * sin(dlon / 2) * sin(dlon / 2)
c = 2 * atan2(sqrt(a), sqrt(1 - a))
d = R * c
return d
def distance_cosine(lat1, lon1, lat2, lon2):
"""Use Law of Cosines to calculate distance (in km) between two
points specified by latitude/longitude (in numeric degrees).
"""
lat1 = radians(lat1)
lat2 = radians(lat2)
dlon = radians(lon2 - lon1)
return acos(sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(dlon)) * R
def bearing(lat1, lon1, lat2, lon2):
"""Calculate the (initial) bearing between two points:
http://williams.best.vwh.net/avform.htm#Crs
Will return a value in radians, different from the original.
"""
lat1 = radians(lat1)
lat2 = radians(lat2)
dlon = radians(lon2 - lon1)
y = sin(dlon) * cos(lat2)
x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dlon)
return atan2(y, x)
def bearing_degrees(lat1, lon1, lat2, lon2):
"""See ``bearing()```.
"""
radians = bearing(lat1, lon1, lat2, lon2)
return (degrees(x) + 360) % 360 # unsigned
def destination(lat, long, bearing, d):
"""Calculate destination point given start point, with initial bearing
in degrees and distance in kilometers:
http://williams.best.vwh.net/avform.htm#LL
Returns a 2-tuple (lat, long), in degrees.
"""
lat1 = radians(lat)
lon1 = radians(long)
bearing = radians(bearing)
lat2 = asin(sin(lat1) * cos(d/R) + cos(lat1) * sin(d/R) * cos(bearing))
lon2 = lon1 + atan2(sin(bearing) * sin(d/R) * cos(lat1),
cos(d/R) - sin(lat1) * sin(lat2))
lon2 = (lon2 + pi) % (2 * pi) - pi # normalize to -180...+180
# if lat2 == NaN || lon2 == NaN: return None # Hm.
return degrees(lat2), degrees(lon2)
def cross_track(latA, lonA, latB, lonB, latP, lonP):
"""Returns the distance of a point P from a great-circle path AB,
in kilometers.
Sometimes called cross track error.
>>> "%.8f" % cross_track(48.76165, 11.41947, 48.75857, 11.42501, 48.76176, 11.41595)
'0.15697753'
"""
d13 = distance_haversine(latA, lonA, latP, lonP)
brng12 = bearing(latA, lonA, latB, lonB)
brng13 = bearing(latA, lonA, latP, lonP)
dXt = asin(sin(d13 / R) * sin(brng13 - brng12)) * R
return dXt
if __name__ == '__main__':
import doctest
doctest.testmod()
tcpgps.py
Kod: Markera allt
# -*- coding: utf-8 -*-
'''
TCP klient för anslutning till rtklib av Robert JL
RJL11 på http://elektronikforumet.com/forum/
'''
import socket
def read_TCP():
TCP_IP = '127.0.0.1' # adress
TCP_PORT = 5005 # port
BUFFER_SIZE = 1024 # buffer storlek
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect((TCP_IP, TCP_PORT))
data = s.recv(BUFFER_SIZE)
return data
#print data
def nmea():
data = read_TCP()
date, time, lat, lon, height, q, ns, sdn, sde, sdu, sdne, sdeu, sdun, age, ratio = data.split() # dela strängen
lat = float(lat) # till float
lon = float(lon) # till float
#print lat, lon
return lat,lon # retunera kordinater
pyagri.ino
Kod: Markera allt
/*
Rutin för autostyrning, värde in på serie port.
*/
#include <Servo.h> // Servo för att visa vart man ska
int serv;
Servo myservo;
int sensorPin = A0; // styrvinkelgivare potensiometer 10k på analog 0
int r_1 = 30; // relä 1 på digital 30
int r_2 = 31; // relä 2 på digital 31
int val = 0; // styrvinkel
int ska = 100; // begärd styrvinkel rakt fram tills serieport igång.
int ded = 10; // glapp i mitten
int buff; // buffer
void setup()
{
pinMode(r_1, OUTPUT);
pinMode(r_2, OUTPUT);
Serial.begin(9600);
myservo.attach(9); // servo på pin 9 pwm
Serial.setTimeout(50); // Hur länge Serial.parseInt skall vänta på giltig integer
}
void loop()
{
if (Serial.available()) // kolla om det finns något inkommande
{
buff = Serial.parseInt(); // om sant läs
Serial.flush(); // Töm buffer.
if (buff > 0) // Om parseInt returnerar ett värde större än noll
{
ska = buff; // Nytt ska värde från serie port
}
serv = map(ska, 0, 200, 0, 179); // mappa in ett rc-servo
myservo.write(serv);
}
else if (vinkel() < ska - ded) // Sväng vänster
{
left();
delay(150);
Serial.write("Vanster");
}
else if (vinkel() > ska + ded) // Sväng höger
{
right();
delay(150);
Serial.write("Hoger");
}
else
{
brake(); // not break.
Serial.write("Broms");
delay(150);
}
}
int vinkel() // läs styrvinkel
{
val = analogRead(sensorPin);
val = map(val, 0, 1023, 0, 200);
return val;
}
void left() //sväng vänster
{
digitalWrite(r_1, HIGH);
digitalWrite(r_2, LOW);
}
void right() //sväng höger
{
digitalWrite(r_1, LOW);
digitalWrite(r_2, HIGH);
}
void brake() //bromsa
{
digitalWrite(r_1, LOW);
digitalWrite(r_2, LOW);
}
