Svenska ElektronikForumet

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;
  }
}