autostyrning med styrvinkel feedback, visning av begärd styrvinkel med rc-servo, status dioder för bredd och punkter.
Anslutning genom tcp till rtklib i tcpgps.py
anslutnign till arduino med serieport. Nu kan jag koncentrera mig på att bygga in allt i lämplig låda och montera rattmotor på en av traktorerna.
Koden:
Kod: Markera allt
# -*- coding: utf-8 -*-
import tcpgps
import serial
import time
import gps
from math import pi
#Redskaps bredd i meter
w = 2.0
v = w - w*2 #Spegelvänd redskapsbredd
#Skapa variabler
latA = 58.1000
lonA = 12.0000
latB = 58.0000
lonB = 12.9000
global latD
global lonD
latD = 58.0000 # för att inte rikting skall få spratt vid uppstart
lonD = 12.0000
ser = serial.Serial('com35',9600) # serieport till arduino
ser.setDTR(level=False)
time.sleep(2)
text = "hej"
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å globala 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
def arduino():
buffer = ''
if ser.inWaiting > 0:
buffer = buffer + ser.read(ser.inWaiting())
if '\n' in buffer:
lines = buffer.split('\n') # Splits buffer block into lines at linefeed
last_received = lines[-2]
buffer = lines[-1]
#print last_received #debug
if last_received is not None:
return last_received
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)
if pos > 0 and pos < 200:
ser.write(pos)
ser.write("\n")
ser.write("\r")
temp = arduino()
if temp is not None:
text = temp
if 'SetA' in text:
latA = lat
lonA = lon
response = int(400)
ser.write(response)
ser.write("\n")
ser.write("\r")
text = "Hej"
print "Set-A"
if 'SetB' in text:
latB = lat
lonB = lon
response = int(500)
ser.write(response)
ser.write("\n")
ser.write("\r")
text = "Hej"
print "Set-B"
if 'With' in text:
text = text.strip("With")
w = float(text) * 0.01
v = w - w*2 #Spegelvänd redskapsbredd
print w
text = "Hej"
response = int(300)
ser.write(response)
ser.write("\n")
ser.write("\r")
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 = 8989 # 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
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()
Kod: Markera allt
/*
Rutin för autostyrning, värde in på serie port. 1 till 200
*/
#include <TM1638.h>
#include <Servo.h> // Servo för att visa vart man ska
int serv;
Servo myservo;
// define a module on data pin 40, clock pin 41 and strobe pin 42
TM1638 module1(40, 41, 42);
// http://dx.com/p/8x-digital-tube-8x-key-8x-double-color-led-module-81873
// else google TM1638
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 = 30; // glapp i mitten
int buff; // buffer
int del = 250;
int with = 0;
int bt1 = 0; // Knapp 1, från höger till vänster
int bt2 = 0;
int bt3 = 0;
int bt4 = 0;
int currentWith = 0;
boolean ledStateA = false;
boolean ledStateB = false;
boolean ledStateWith = false;
boolean ledStateStatus = false;
long previousMillis1 = 0;
long previousMillis = 0;
long interval = 250;
long intervalS = 250;
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()
{
unsigned long currentMillis1 = millis();
if (Serial.available()) // kolla om det finns något inkommande
{
buff = Serial.parseInt(); // om sant läs
Serial.flush(); // Töm buffer.
if (buff == 300)
{
withResponde();
}
else if (buff == 400)
{
SetAResponde();
}
else if (buff == 500)
{
SetBResponde();
}
else if (buff > 0 && buff < 200) // Om parseInt returnerar ett värde större än noll
{
if (buff != ska)
{
statusBlink();
}
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
{
if(currentMillis1 - previousMillis1 > intervalS)
{
previousMillis1 = currentMillis1;
left();
}
}
else if (vinkel() > ska + ded) // Sväng höger
{
if(currentMillis1 - previousMillis1 > intervalS)
{
previousMillis1 = currentMillis1;
right();
}
}
else
{
if(currentMillis1 - previousMillis1 > intervalS)
{
previousMillis1 = currentMillis1;
brake();
}
}
interface(); // kör interface rutinen.
}
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);
}
void interface() // interface rutin.
{
unsigned long currentMillis = millis();
if (module1.getButtons() == 0b10000000)
{
if(currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
bt1 = bt1 + 1;
if (bt1 > 9)
{
bt1 = 0;
}
}
}
else if (module1.getButtons() == 0b01000000)
{
if(currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
bt2 = bt2 + 1;
if (bt2 > 9)
{
bt2 = 0;
}
}
}
else if (module1.getButtons() == 0b00100000)
{
if(currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
bt3 = bt3 + 1;
if (bt3 > 9)
{
bt3 = 0;
}
}
}
else if (module1.getButtons() == 0b00010000)
{
if(currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
bt4 = bt4 + 1;
if (bt4 > 9)
{
bt4 = 0;
}
}
}
currentWith = bt1*1 + bt2*10 + bt3*100 + bt4*1000;
if (currentWith != with)
{
with = currentWith;
module1.setDisplayToDecNumber(with, 4, false);
Serial.print ("With");
Serial.print (with);
Serial.print ("\n");
}
if (module1.getButtons() == 0b00000001)
{
if (currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
Serial.print ("SetA");
Serial.print ("\n");
}
}
else if (module1.getButtons() == 0b00000100)
{
if (currentMillis - previousMillis > interval)
{
previousMillis = currentMillis;
Serial.print ("SetB");
Serial.print ("\n");
}
}
}
void withResponde() // växla diod 6 och 7 när host programmet svarar att uppdaterad bredd tagits emot.
{
if (ledStateWith == false)
{
module1.setLED(TM1638_COLOR_GREEN, 7);
module1.setLED(TM1638_COLOR_NONE, 6);
ledStateWith = true;
}
else
{
if (ledStateWith == true)
{
module1.setLED(TM1638_COLOR_NONE, 7);
module1.setLED(TM1638_COLOR_GREEN, 6);
ledStateWith = false;
}
}
}
void SetAResponde()
{
if (ledStateA == false)
{
module1.setLED(TM1638_COLOR_GREEN, 0);
module1.setLED(TM1638_COLOR_NONE, 1);
ledStateA = true;
}
else
{
module1.setLED(TM1638_COLOR_NONE, 0);
module1.setLED(TM1638_COLOR_GREEN, 1);
ledStateA = false;
}
}
void SetBResponde()
{
if (ledStateB == false)
{
module1.setLED(TM1638_COLOR_GREEN, 2);
module1.setLED(TM1638_COLOR_NONE, 3);
ledStateB = true;
}
else
{
module1.setLED(TM1638_COLOR_NONE, 2);
module1.setLED(TM1638_COLOR_GREEN, 3);
ledStateB = false;
}
}
void statusBlink()
{
if (ledStateStatus == false)
{
module1.setLED(TM1638_COLOR_GREEN, 4);
ledStateStatus = true;
}
else
{
module1.setLED(TM1638_COLOR_RED, 4);
ledStateStatus = false;
}
}
Har lite dåligt med kommentarer i koden men den borde gå att läsa.
Det vore kul att höra lite hur det går för era projekt med gps också.