init

.gitignore

@@ -0,0 +1 @@
+secrets.h

Auto_Robotik_Datenfusion_Final.ino

@@ -0,0 +1,88 @@
+#include <Arduino.h>
+#include <avr/wdt.h>
+
+#include "spm4648.h"
+#include "MPU6050.h"
+
+#include "globals.h"
+//#include "driveCircle.h"
+//#include "driveRegler.h"
+#include "drivefusion.h"
+
+
+bool lastSwitch = true;
+uint32_t mainLoopDelay = 16000;
+uint32_t mainLoopStart = 0;
+
+void driveRemote() {
+  if(spm.hasData()) {
+    setMotor(spm.data[2]*0.3);
+    setServo(spm.data[1]);
+    Serial.print("Manual: \t");
+    Serial.print(gasVal);
+    Serial.print(" \t");
+    Serial.print(servoVal);
+    Serial.print(" \t");
+    Serial.print(spm.data[2]*0.2);
+    Serial.print("\t");
+    Serial.println(spm.data[1]);
+  }
+}
+
+
+void setup() {
+
+  Serial.begin(115200);
+  Serial1.begin(115200);
+  Serial2.begin(38400);
+  Serial3.begin(9600);
+
+  Serial.println("boot");
+  mpu.setUpMPU();
+  mpu.callibrateGyroValues();
+  initServos();
+  Serial.println("setup finished");
+
+  wdt_enable(WDTO_250MS);
+}
+
+void loop() {
+  wdt_reset();
+  spm.read(Serial1);
+
+  
+  mpu.readAndProcessGyroData();
+  
+  mpu.readAndProcessAccelData();
+  mpu.printData();
+  
+  if(spm.isConnected()) {
+    bool sw = spm.data[4] > 0;
+    if(sw == false && lastSwitch == true) {
+      initAuto();
+    }
+    if(sw) {
+      msAuto = millis();
+      driveRemote();
+    } else {
+      driveAuto();
+    }
+    lastSwitch = sw;
+    
+  } else {
+    //safe mode
+    Serial.println(F("No connection!"));
+    setMotor(0);
+    setServo(0);
+  }
+  
+  mainLoopDelay = micros() - mainLoopStart;
+  
+  if(mainLoopDelay < 10000)
+    delayMicroseconds(10000 - mainLoopDelay); //max. 16ms
+
+  mainLoopDelay = micros() - mainLoopStart;
+  //Serial.println(mainLoopDelay/1000.0);
+
+  mainLoopStart = micros();
+}

ESC.cpp

@@ -0,0 +1,52 @@
+/* Electronic Speed Controller (ESC) - Library */
+
+/*
+	
+*/
+
+#include "ESC.h"
+
+// < Constructor >
+/* Sets the proper pin to output.
+*/
+ESC::ESC(byte ESC_pin, int outputMin, int outputMax, int armVal)
+{
+	oPin = ESC_pin;
+	oMin = outputMin;
+	oMax = outputMax;
+	oArm = armVal;
+}
+
+// < Destructor >
+ESC::~ESC()
+{
+	// Nothing to destruct
+}
+
+// calib
+/* Calibrate the maximum and minimum PWM signal the ESC is expecting
+*/
+void ESC::calib(void)
+{
+	myESC.attach(oPin);  			// attaches the ESC on pin oPin to the ESC object
+	myESC.writeMicroseconds(oMax);
+		delay(ESC_CAL_DELAY);
+	myESC.writeMicroseconds(oMin);
+		delay(ESC_CAL_DELAY);
+	arm();
+}
+
+void ESC::arm(void)
+{
+	myESC.attach(oPin);  			// attaches the ESC on pin oPin to the ESC object
+	myESC.writeMicroseconds (oArm);
+}
+void ESC::stop(void)
+{
+	myESC.writeMicroseconds (ESC_STOP_PULSE);
+}
+void ESC::speed(int outputESC)
+{
+	oESC = constrain(outputESC, oMin, oMax);
+	myESC.writeMicroseconds(oESC);
+}

ESC.h

@@ -0,0 +1,47 @@
+/* Electronic Speed Controller (ESC) - Library */
+
+/*
+
+*/
+
+#ifndef ESC_Library
+#define ESC_Library
+
+#if (ARDUINO >= 100)
+	#include "Arduino.h"
+#else
+	#include "WProgram.h"
+#endif
+
+#include <Servo.h>				// Including the Servo library
+#define ESC_CAL_DELAY	(8000)	// Calibration delay (milisecond)
+#define ESC_STOP_PULSE	(500)	//
+
+class ESC
+{
+	public:
+		ESC(byte ESC_pin, int outputMin = 1000, int outputMax = 2000, int armVal = 500);
+		~ESC();
+		void calib(void);
+		void arm(void);
+		void stop(void);
+		void speed(int ESC_val);
+	
+	private:
+	// < Local attributes >
+		// Hardware
+		byte oPin;			// ESC output Pin
+		
+		// Calibration
+		int oMin = 1000; 
+		int oMax = 2000;
+		int oESC = 1000;
+		int oArm = 500;
+		Servo myESC;		// create servo object to control an ESC
+
+
+};
+
+#endif
+
+/* Electronic Speed Controller (ESC) - Library */

MPU6050.cpp

@@ -0,0 +1,124 @@
+#include "MPU6050.h"
+
+MPU6050::MPU6050() {
+  
+}
+
+void MPU6050::getAngularVelocity() {
+  gyroX = gyroXPresent / 131.0;
+  gyroY = gyroYPresent / 131.0; 
+  gyroZ = gyroZPresent / 131.0;
+}
+void MPU6050::calculateAngle() {  
+  winkelX = winkelX + ((timePresent - timePast)*(gyroXPresent + gyroXPast - 2*gyroXCalli)) * 0.00000382;
+  winkelY = winkelY + ((timePresent - timePast)*(gyroYPresent + gyroYPast - 2*gyroYCalli)) * 0.00000382;
+  winkelZ = winkelZ + ((timePresent - timePast)*(gyroZPresent + gyroZPast - 2*gyroZCalli)) * 0.00000382;
+}
+void MPU6050::printData() {
+  Serial.print("3-Achsen-Gyroskop              ");
+  Serial.print("\tX=");
+  Serial.print(gyroX);
+  Serial.print("°/s");
+  Serial.print("\tY=");
+  Serial.print(gyroY);
+  Serial.print("°/s");
+  Serial.print("\tZ=");
+  Serial.print(gyroZ);
+  Serial.print("°\r\n");
+
+  Serial.print("Winkel der Achsen              ");
+  Serial.print("\tX=");
+  Serial.print(winkelX);
+  Serial.print("° ");
+  Serial.print("\tY=");
+  Serial.print(winkelY);
+  Serial.print("° ");
+  Serial.print("\tZ=");
+  Serial.print(winkelZ);
+  Serial.print("°\r\n");
+    
+  Serial.print("3-Achsen-Beschleunigungsmesser ");
+  Serial.print("\tX=");
+  Serial.print(beschleunigungX);
+  Serial.print("m/s² ");
+  Serial.print("\tY=");
+  Serial.print(beschleunigungY);
+  Serial.print("m/s² ");
+  Serial.print("\tZ=");
+  Serial.print(beschleunigungZ);
+  Serial.print("m/s²\r\n");
+}
+
+void MPU6050::setUpMPU() {
+  // power management
+  Wire.beginTransmission(0b1101000);
+  Wire.write(0x6B);
+  Wire.write(0b00000000);
+  Wire.endTransmission();
+
+  // configure gyro
+  Wire.beginTransmission(0b1101000);
+  Wire.write(0x1B);
+  Wire.write(0b00000000);
+  Wire.endTransmission();
+
+  // configure accelerometer
+  Wire.beginTransmission(0b1101000);
+  Wire.write(0x1C);
+  Wire.write(0b00000000);
+  Wire.endTransmission();  
+}
+
+void MPU6050::callibrateGyroValues() {
+    for (int i=0; i<5000; i++) {
+      getGyroValues();
+      gyroXCalli = gyroXCalli + gyroXPresent;
+      gyroYCalli = gyroYCalli + gyroYPresent;
+      gyroZCalli = gyroZCalli + gyroZPresent;
+    }
+    gyroXCalli = gyroXCalli/5000;
+    gyroYCalli = gyroYCalli/5000;
+    gyroZCalli = gyroZCalli/5000;
+}
+
+void MPU6050::readAndProcessAccelData() {
+  Wire.beginTransmission(0b1101000); 
+  Wire.write(0x3B); 
+  Wire.endTransmission();
+  Wire.requestFrom(0b1101000,6); 
+  while(Wire.available() < 6);
+  accelX = Wire.read()<<8|Wire.read(); 
+  accelY = Wire.read()<<8|Wire.read(); 
+  accelZ = Wire.read()<<8|Wire.read(); 
+  MPU6050::processAccelData();
+}
+
+void MPU6050::processAccelData() {
+  beschleunigungX = (accelX/16384.0)*9.81;
+  beschleunigungY = (accelY/16384.0)*9.81; 
+  beschleunigungZ = (accelZ/16384.0)*9.81;
+}
+
+void MPU6050::readAndProcessGyroData() {
+  gyroXPast = gyroXPresent;
+  gyroYPast = gyroYPresent;
+  gyroZPast = gyroZPresent;
+  timePast = timePresent;
+  timePresent = millis();
+  
+  MPU6050::getGyroValues();
+  MPU6050::getAngularVelocity();
+  MPU6050::calculateAngle();
+}
+
+void MPU6050::getGyroValues() {
+  Wire.beginTransmission(0b1101000); 
+  Wire.write(0x43);
+  Wire.endTransmission();
+  Wire.requestFrom(0b1101000,6);
+  while(Wire.available() < 6);
+  gyroXPresent = Wire.read()<<8|Wire.read();
+  gyroYPresent = Wire.read()<<8|Wire.read();
+  gyroZPresent = Wire.read()<<8|Wire.read();
+}
+ 

MPU6050.h

@@ -0,0 +1,30 @@
+#ifndef MPU6050_h
+#define MPU6050_h
+
+#include "Arduino.h"
+#include <Wire.h>
+
+class MPU6050 {
+  public:
+    MPU6050(void);
+    long accelX, accelY, accelZ;
+    float beschleunigungX, beschleunigungY, beschleunigungZ;
+    float gyroX, gyroY, gyroZ;
+    
+    void getAngularVelocity(void);
+    void calculateAngle(void);
+    void printData(void);
+    void setUpMPU();
+
+    
+    void callibrateGyroValues(void);
+    void readAndProcessAccelData(void);
+    void processAccelData(void);
+    void readAndProcessGyroData(void);
+    void getGyroValues(void);
+
+    long gyroXCalli, gyroYCalli, gyroZCalli,gyroXPresent,gyroYPresent,gyroZPresent,gyroXPast,gyroYPast,gyroZPast,timePast,timePresent;
+    float winkelX,winkelY,winkelZ;
+};
+
+#endif

PID_v1.cpp

@@ -0,0 +1,223 @@
+/**********************************************************************************************
+ * Arduino PID Library - Version 1.2.1
+ * by Brett Beauregard <br3ttb@gmail.com> brettbeauregard.com
+ *
+ * This Library is licensed under the MIT License
+ **********************************************************************************************/
+
+#if ARDUINO >= 100
+  #include "Arduino.h"
+#else
+  #include "WProgram.h"
+#endif
+
+#include "PID_v1.h"
+
+/*Constructor (...)*********************************************************
+ *    The parameters specified here are those for for which we can't set up
+ *    reliable defaults, so we need to have the user set them.
+ ***************************************************************************/
+PID::PID(double* Input, double* Output, double* Setpoint,
+        double Kp, double Ki, double Kd, int POn, int ControllerDirection)
+{
+    myOutput = Output;
+    myInput = Input;
+    mySetpoint = Setpoint;
+    inAuto = false;
+
+    PID::SetOutputLimits(0, 255);				//default output limit corresponds to
+												//the arduino pwm limits
+
+    SampleTime = 100;							//default Controller Sample Time is 0.1 seconds
+
+    PID::SetControllerDirection(ControllerDirection);
+    PID::SetTunings(Kp, Ki, Kd, POn);
+
+    lastTime = millis()-SampleTime;
+}
+
+/*Constructor (...)*********************************************************
+ *    To allow backwards compatability for v1.1, or for people that just want
+ *    to use Proportional on Error without explicitly saying so
+ ***************************************************************************/
+
+PID::PID(double* Input, double* Output, double* Setpoint,
+        double Kp, double Ki, double Kd, int ControllerDirection)
+    :PID::PID(Input, Output, Setpoint, Kp, Ki, Kd, P_ON_E, ControllerDirection)
+{
+
+}
+
+
+/* Compute() **********************************************************************
+ *     This, as they say, is where the magic happens.  this function should be called
+ *   every time "void loop()" executes.  the function will decide for itself whether a new
+ *   pid Output needs to be computed.  returns true when the output is computed,
+ *   false when nothing has been done.
+ **********************************************************************************/
+bool PID::Compute()
+{
+   if(!inAuto) return false;
+   unsigned long now = millis();
+   unsigned long timeChange = (now - lastTime);
+   if(timeChange>=SampleTime)
+   {
+      /*Compute all the working error variables*/
+      double input = *myInput;
+      double error = *mySetpoint - input;
+      double dInput = (input - lastInput);
+      outputSum+= (ki * error);
+
+      /*Add Proportional on Measurement, if P_ON_M is specified*/
+      if(!pOnE) outputSum-= kp * dInput;
+
+      if(outputSum > outMax) outputSum= outMax;
+      else if(outputSum < outMin) outputSum= outMin;
+
+      /*Add Proportional on Error, if P_ON_E is specified*/
+	   double output;
+      if(pOnE) output = kp * error;
+      else output = 0;
+
+      /*Compute Rest of PID Output*/
+      output += outputSum - kd * dInput;
+
+	    if(output > outMax) output = outMax;
+      else if(output < outMin) output = outMin;
+	    *myOutput = output;
+
+      /*Remember some variables for next time*/
+      lastInput = input;
+      lastTime = now;
+	    return true;
+   }
+   else return false;
+}
+
+/* SetTunings(...)*************************************************************
+ * This function allows the controller's dynamic performance to be adjusted.
+ * it's called automatically from the constructor, but tunings can also
+ * be adjusted on the fly during normal operation
+ ******************************************************************************/
+void PID::SetTunings(double Kp, double Ki, double Kd, int POn)
+{
+   if (Kp<0 || Ki<0 || Kd<0) return;
+
+   pOn = POn;
+   pOnE = POn == P_ON_E;
+
+   dispKp = Kp; dispKi = Ki; dispKd = Kd;
+
+   double SampleTimeInSec = ((double)SampleTime)/1000;
+   kp = Kp;
+   ki = Ki * SampleTimeInSec;
+   kd = Kd / SampleTimeInSec;
+
+  if(controllerDirection ==REVERSE)
+   {
+      kp = (0 - kp);
+      ki = (0 - ki);
+      kd = (0 - kd);
+   }
+}
+
+/* SetTunings(...)*************************************************************
+ * Set Tunings using the last-rembered POn setting
+ ******************************************************************************/
+void PID::SetTunings(double Kp, double Ki, double Kd){
+    SetTunings(Kp, Ki, Kd, pOn); 
+}
+
+/* SetSampleTime(...) *********************************************************
+ * sets the period, in Milliseconds, at which the calculation is performed
+ ******************************************************************************/
+void PID::SetSampleTime(int NewSampleTime)
+{
+   if (NewSampleTime > 0)
+   {
+      double ratio  = (double)NewSampleTime
+                      / (double)SampleTime;
+      ki *= ratio;
+      kd /= ratio;
+      SampleTime = (unsigned long)NewSampleTime;
+   }
+}
+
+/* SetOutputLimits(...)****************************************************
+ *     This function will be used far more often than SetInputLimits.  while
+ *  the input to the controller will generally be in the 0-1023 range (which is
+ *  the default already,)  the output will be a little different.  maybe they'll
+ *  be doing a time window and will need 0-8000 or something.  or maybe they'll
+ *  want to clamp it from 0-125.  who knows.  at any rate, that can all be done
+ *  here.
+ **************************************************************************/
+void PID::SetOutputLimits(double Min, double Max)
+{
+   if(Min >= Max) return;
+   outMin = Min;
+   outMax = Max;
+
+   if(inAuto)
+   {
+	   if(*myOutput > outMax) *myOutput = outMax;
+	   else if(*myOutput < outMin) *myOutput = outMin;
+
+	   if(outputSum > outMax) outputSum= outMax;
+	   else if(outputSum < outMin) outputSum= outMin;
+   }
+}
+
+/* SetMode(...)****************************************************************
+ * Allows the controller Mode to be set to manual (0) or Automatic (non-zero)
+ * when the transition from manual to auto occurs, the controller is
+ * automatically initialized
+ ******************************************************************************/
+void PID::SetMode(int Mode)
+{
+    bool newAuto = (Mode == AUTOMATIC);
+    if(newAuto && !inAuto)
+    {  /*we just went from manual to auto*/
+        PID::Initialize();
+    }
+    inAuto = newAuto;
+}
+
+/* Initialize()****************************************************************
+ *	does all the things that need to happen to ensure a bumpless transfer
+ *  from manual to automatic mode.
+ ******************************************************************************/
+void PID::Initialize()
+{
+   outputSum = *myOutput;
+   lastInput = *myInput;
+   if(outputSum > outMax) outputSum = outMax;
+   else if(outputSum < outMin) outputSum = outMin;
+}
+
+/* SetControllerDirection(...)*************************************************
+ * The PID will either be connected to a DIRECT acting process (+Output leads
+ * to +Input) or a REVERSE acting process(+Output leads to -Input.)  we need to
+ * know which one, because otherwise we may increase the output when we should
+ * be decreasing.  This is called from the constructor.
+ ******************************************************************************/
+void PID::SetControllerDirection(int Direction)
+{
+   if(inAuto && Direction !=controllerDirection)
+   {
+	    kp = (0 - kp);
+      ki = (0 - ki);
+      kd = (0 - kd);
+   }
+   controllerDirection = Direction;
+}
+
+/* Status Funcions*************************************************************
+ * Just because you set the Kp=-1 doesn't mean it actually happened.  these
+ * functions query the internal state of the PID.  they're here for display
+ * purposes.  this are the functions the PID Front-end uses for example
+ ******************************************************************************/
+double PID::GetKp(){ return  dispKp; }
+double PID::GetKi(){ return  dispKi;}
+double PID::GetKd(){ return  dispKd;}
+int PID::GetMode(){ return  inAuto ? AUTOMATIC : MANUAL;}
+int PID::GetDirection(){ return controllerDirection;}

PID_v1.h

@@ -0,0 +1,89 @@
+#ifndef PID_v1_h
+#define PID_v1_h
+#define LIBRARY_VERSION	1.2.1
+
+class PID
+{
+
+
+  public:
+
+  //Constants used in some of the functions below
+  #define AUTOMATIC	1
+  #define MANUAL	0
+  #define DIRECT  0
+  #define REVERSE  1
+  #define P_ON_M 0
+  #define P_ON_E 1
+
+  //commonly used functions **************************************************************************
+    PID(double*, double*, double*,        // * constructor.  links the PID to the Input, Output, and 
+        double, double, double, int, int);//   Setpoint.  Initial tuning parameters are also set here.
+                                          //   (overload for specifying proportional mode)
+
+    PID(double*, double*, double*,        // * constructor.  links the PID to the Input, Output, and 
+        double, double, double, int);     //   Setpoint.  Initial tuning parameters are also set here
+	
+    void SetMode(int Mode);               // * sets PID to either Manual (0) or Auto (non-0)
+
+    bool Compute();                       // * performs the PID calculation.  it should be
+                                          //   called every time loop() cycles. ON/OFF and
+                                          //   calculation frequency can be set using SetMode
+                                          //   SetSampleTime respectively
+
+    void SetOutputLimits(double, double); // * clamps the output to a specific range. 0-255 by default, but
+										                      //   it's likely the user will want to change this depending on
+										                      //   the application
+	
+
+
+  //available but not commonly used functions ********************************************************
+    void SetTunings(double, double,       // * While most users will set the tunings once in the 
+                    double);         	    //   constructor, this function gives the user the option
+                                          //   of changing tunings during runtime for Adaptive control
+    void SetTunings(double, double,       // * overload for specifying proportional mode
+                    double, int);         	  
+
+	void SetControllerDirection(int);	  // * Sets the Direction, or "Action" of the controller. DIRECT
+										  //   means the output will increase when error is positive. REVERSE
+										  //   means the opposite.  it's very unlikely that this will be needed
+										  //   once it is set in the constructor.
+    void SetSampleTime(int);              // * sets the frequency, in Milliseconds, with which 
+                                          //   the PID calculation is performed.  default is 100
+										  
+										  
+										  
+  //Display functions ****************************************************************
+	double GetKp();						  // These functions query the pid for interal values.
+	double GetKi();						  //  they were created mainly for the pid front-end,
+	double GetKd();						  // where it's important to know what is actually 
+	int GetMode();						  //  inside the PID.
+	int GetDirection();					  //
+
+  private:
+	void Initialize();
+	
+	double dispKp;				// * we'll hold on to the tuning parameters in user-entered 
+	double dispKi;				//   format for display purposes
+	double dispKd;				//
+    
+	double kp;                  // * (P)roportional Tuning Parameter
+    double ki;                  // * (I)ntegral Tuning Parameter
+    double kd;                  // * (D)erivative Tuning Parameter
+
+	int controllerDirection;
+	int pOn;
+
+    double *myInput;              // * Pointers to the Input, Output, and Setpoint variables
+    double *myOutput;             //   This creates a hard link between the variables and the 
+    double *mySetpoint;           //   PID, freeing the user from having to constantly tell us
+                                  //   what these values are.  with pointers we'll just know.
+			  
+	unsigned long lastTime;
+	double outputSum, lastInput;
+
+	unsigned long SampleTime;
+	double outMin, outMax;
+	bool inAuto, pOnE;
+};
+#endif

TinyGPS.cpp

@@ -0,0 +1,429 @@
+/*
+TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
+Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers.
+Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson.
+Precision improvements suggested by Wayne Holder.
+Copyright (C) 2008-2013 Mikal Hart
+All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#include "TinyGPS.h"
+
+#define _GPRMC_TERM   "GPRMC"
+#define _GPGGA_TERM   "GPGGA"
+
+TinyGPS::TinyGPS()
+  :  _time(GPS_INVALID_TIME)
+  ,  _date(GPS_INVALID_DATE)
+  ,  _latitude(GPS_INVALID_ANGLE)
+  ,  _longitude(GPS_INVALID_ANGLE)
+  ,  _altitude(GPS_INVALID_ALTITUDE)
+  ,  _speed(GPS_INVALID_SPEED)
+  ,  _course(GPS_INVALID_ANGLE)
+  ,  _hdop(GPS_INVALID_HDOP)
+  ,  _numsats(GPS_INVALID_SATELLITES)
+  ,  _last_time_fix(GPS_INVALID_FIX_TIME)
+  ,  _last_position_fix(GPS_INVALID_FIX_TIME)
+  ,  _parity(0)
+  ,  _is_checksum_term(false)
+  ,  _sentence_type(_GPS_SENTENCE_OTHER)
+  ,  _term_number(0)
+  ,  _term_offset(0)
+  ,  _gps_data_good(false)
+#ifndef _GPS_NO_STATS
+  ,  _encoded_characters(0)
+  ,  _good_sentences(0)
+  ,  _failed_checksum(0)
+#endif
+{
+  _term[0] = '\0';
+}
+
+//
+// public methods
+//
+
+bool TinyGPS::encode(char c)
+{
+  bool valid_sentence = false;
+
+#ifndef _GPS_NO_STATS
+  ++_encoded_characters;
+#endif
+  switch(c)
+  {
+  case ',': // term terminators
+    _parity ^= c;
+  case '\r':
+  case '\n':
+  case '*':
+    if (_term_offset < sizeof(_term))
+    {
+      _term[_term_offset] = 0;
+      valid_sentence = term_complete();
+    }
+    ++_term_number;
+    _term_offset = 0;
+    _is_checksum_term = c == '*';
+    return valid_sentence;
+
+  case '$': // sentence begin
+    _term_number = _term_offset = 0;
+    _parity = 0;
+    _sentence_type = _GPS_SENTENCE_OTHER;
+    _is_checksum_term = false;
+    _gps_data_good = false;
+    return valid_sentence;
+  }
+
+  // ordinary characters
+  if (_term_offset < sizeof(_term) - 1)
+    _term[_term_offset++] = c;
+  if (!_is_checksum_term)
+    _parity ^= c;
+
+  return valid_sentence;
+}
+
+#ifndef _GPS_NO_STATS
+void TinyGPS::stats(unsigned long *chars, unsigned short *sentences, unsigned short *failed_cs)
+{
+  if (chars) *chars = _encoded_characters;
+  if (sentences) *sentences = _good_sentences;
+  if (failed_cs) *failed_cs = _failed_checksum;
+}
+#endif
+
+//
+// internal utilities
+//
+int TinyGPS::from_hex(char a) 
+{
+  if (a >= 'A' && a <= 'F')
+    return a - 'A' + 10;
+  else if (a >= 'a' && a <= 'f')
+    return a - 'a' + 10;
+  else
+    return a - '0';
+}
+
+unsigned long TinyGPS::parse_decimal()
+{
+  char *p = _term;
+  bool isneg = *p == '-';
+  if (isneg) ++p;
+  unsigned long ret = 100UL * gpsatol(p);
+  while (gpsisdigit(*p)) ++p;
+  if (*p == '.')
+  {
+    if (gpsisdigit(p[1]))
+    {
+      ret += 10 * (p[1] - '0');
+      if (gpsisdigit(p[2]))
+        ret += p[2] - '0';
+    }
+  }
+  return isneg ? -ret : ret;
+}
+
+// Parse a string in the form ddmm.mmmmmmm...
+unsigned long TinyGPS::parse_degrees()
+{
+  char *p;
+  unsigned long left_of_decimal = gpsatol(_term);
+  unsigned long hundred1000ths_of_minute = (left_of_decimal % 100UL) * 100000UL;
+  for (p=_term; gpsisdigit(*p); ++p);
+  if (*p == '.')
+  {
+    unsigned long mult = 10000;
+    while (gpsisdigit(*++p))
+    {
+      hundred1000ths_of_minute += mult * (*p - '0');
+      mult /= 10;
+    }
+  }
+  return (left_of_decimal / 100) * 1000000 + (hundred1000ths_of_minute + 3) / 6;
+}
+
+#define COMBINE(sentence_type, term_number) (((unsigned)(sentence_type) << 5) | term_number)
+
+// Processes a just-completed term
+// Returns true if new sentence has just passed checksum test and is validated
+bool TinyGPS::term_complete()
+{
+  if (_is_checksum_term)
+  {
+    byte checksum = 16 * from_hex(_term[0]) + from_hex(_term[1]);
+    if (checksum == _parity)
+    {
+      if (_gps_data_good)
+      {
+#ifndef _GPS_NO_STATS
+        ++_good_sentences;
+#endif
+        _last_time_fix = _new_time_fix;
+        _last_position_fix = _new_position_fix;
+
+        switch(_sentence_type)
+        {
+        case _GPS_SENTENCE_GPRMC:
+          _time      = _new_time;
+          _date      = _new_date;
+          _latitude  = _new_latitude;
+          _longitude = _new_longitude;
+          _speed     = _new_speed;
+          _course    = _new_course;
+          break;
+        case _GPS_SENTENCE_GPGGA:
+          _altitude  = _new_altitude;
+          _time      = _new_time;
+          _latitude  = _new_latitude;
+          _longitude = _new_longitude;
+          _numsats   = _new_numsats;
+          _hdop      = _new_hdop;
+          break;
+        }
+
+        return true;
+      }
+    }
+
+#ifndef _GPS_NO_STATS
+    else
+      ++_failed_checksum;
+#endif
+    return false;
+  }
+
+  // the first term determines the sentence type
+  if (_term_number == 0)
+  {
+    if (!gpsstrcmp(_term, _GPRMC_TERM))
+      _sentence_type = _GPS_SENTENCE_GPRMC;
+    else if (!gpsstrcmp(_term, _GPGGA_TERM))
+      _sentence_type = _GPS_SENTENCE_GPGGA;
+    else
+      _sentence_type = _GPS_SENTENCE_OTHER;
+    return false;
+  }
+
+  if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0])
+    switch(COMBINE(_sentence_type, _term_number))
+  {
+    case COMBINE(_GPS_SENTENCE_GPRMC, 1): // Time in both sentences
+    case COMBINE(_GPS_SENTENCE_GPGGA, 1):
+      _new_time = parse_decimal();
+      _new_time_fix = millis();
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 2): // GPRMC validity
+      _gps_data_good = _term[0] == 'A';
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 3): // Latitude
+    case COMBINE(_GPS_SENTENCE_GPGGA, 2):
+      _new_latitude = parse_degrees();
+      _new_position_fix = millis();
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 4): // N/S
+    case COMBINE(_GPS_SENTENCE_GPGGA, 3):
+      if (_term[0] == 'S')
+        _new_latitude = -_new_latitude;
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 5): // Longitude
+    case COMBINE(_GPS_SENTENCE_GPGGA, 4):
+      _new_longitude = parse_degrees();
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 6): // E/W
+    case COMBINE(_GPS_SENTENCE_GPGGA, 5):
+      if (_term[0] == 'W')
+        _new_longitude = -_new_longitude;
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 7): // Speed (GPRMC)
+      _new_speed = parse_decimal();
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 8): // Course (GPRMC)
+      _new_course = parse_decimal();
+      break;
+    case COMBINE(_GPS_SENTENCE_GPRMC, 9): // Date (GPRMC)
+      _new_date = gpsatol(_term);
+      break;
+    case COMBINE(_GPS_SENTENCE_GPGGA, 6): // Fix data (GPGGA)
+      _gps_data_good = _term[0] > '0';
+      break;
+    case COMBINE(_GPS_SENTENCE_GPGGA, 7): // Satellites used (GPGGA)
+      _new_numsats = (unsigned char)atoi(_term);
+      break;
+    case COMBINE(_GPS_SENTENCE_GPGGA, 8): // HDOP
+      _new_hdop = parse_decimal();
+      break;
+    case COMBINE(_GPS_SENTENCE_GPGGA, 9): // Altitude (GPGGA)
+      _new_altitude = parse_decimal();
+      break;
+  }
+
+  return false;
+}
+
+long TinyGPS::gpsatol(const char *str)
+{
+  long ret = 0;
+  while (gpsisdigit(*str))
+    ret = 10 * ret + *str++ - '0';
+  return ret;
+}
+
+int TinyGPS::gpsstrcmp(const char *str1, const char *str2)
+{
+  while (*str1 && *str1 == *str2)
+    ++str1, ++str2;
+  return *str1;
+}
+
+/* static */
+float TinyGPS::distance_between (float lat1, float long1, float lat2, float long2) 
+{
+  // returns distance in meters between two positions, both specified 
+  // as signed decimal-degrees latitude and longitude. Uses great-circle 
+  // distance computation for hypothetical sphere of radius 6372795 meters.
+  // Because Earth is no exact sphere, rounding errors may be up to 0.5%.
+  // Courtesy of Maarten Lamers
+  float delta = radians(long1-long2);
+  float sdlong = sin(delta);
+  float cdlong = cos(delta);
+  lat1 = radians(lat1);
+  lat2 = radians(lat2);
+  float slat1 = sin(lat1);
+  float clat1 = cos(lat1);
+  float slat2 = sin(lat2);
+  float clat2 = cos(lat2);
+  delta = (clat1 * slat2) - (slat1 * clat2 * cdlong); 
+  delta = sq(delta); 
+  delta += sq(clat2 * sdlong); 
+  delta = sqrt(delta); 
+  float denom = (slat1 * slat2) + (clat1 * clat2 * cdlong); 
+  delta = atan2(delta, denom); 
+  return delta * 6372795; 
+}
+
+float TinyGPS::course_to (float lat1, float long1, float lat2, float long2) 
+{
+  // returns course in degrees (North=0, West=270) from position 1 to position 2,
+  // both specified as signed decimal-degrees latitude and longitude.
+  // Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
+  // Courtesy of Maarten Lamers
+  float dlon = radians(long2-long1);
+  lat1 = radians(lat1);
+  lat2 = radians(lat2);
+  float a1 = sin(dlon) * cos(lat2);
+  float a2 = sin(lat1) * cos(lat2) * cos(dlon);
+  a2 = cos(lat1) * sin(lat2) - a2;
+  a2 = atan2(a1, a2);
+  if (a2 < 0.0)
+  {
+    a2 += TWO_PI;
+  }
+  return degrees(a2);
+}
+
+const char *TinyGPS::cardinal (float course)
+{
+  static const char* directions[] = {"N", "NNE", "NE", "ENE", "E", "ESE", "SE", "SSE", "S", "SSW", "SW", "WSW", "W", "WNW", "NW", "NNW"};
+
+  int direction = (int)((course + 11.25f) / 22.5f);
+  return directions[direction % 16];
+}
+
+// lat/long in MILLIONTHs of a degree and age of fix in milliseconds
+// (note: versions 12 and earlier gave this value in 100,000ths of a degree.
+void TinyGPS::get_position(long *latitude, long *longitude, unsigned long *fix_age)
+{
+  if (latitude) *latitude = _latitude;
+  if (longitude) *longitude = _longitude;
+  if (fix_age) *fix_age = _last_position_fix == GPS_INVALID_FIX_TIME ? 
+   GPS_INVALID_AGE : millis() - _last_position_fix;
+}
+
+// date as ddmmyy, time as hhmmsscc, and age in milliseconds
+void TinyGPS::get_datetime(unsigned long *date, unsigned long *time, unsigned long *age)
+{
+  if (date) *date = _date;
+  if (time) *time = _time;
+  if (age) *age = _last_time_fix == GPS_INVALID_FIX_TIME ? 
+   GPS_INVALID_AGE : millis() - _last_time_fix;
+}
+
+void TinyGPS::f_get_position(float *latitude, float *longitude, unsigned long *fix_age)
+{
+  long lat, lon;
+  get_position(&lat, &lon, fix_age);
+  *latitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lat / 1000000.0);
+  *longitude = lat == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : (lon / 1000000.0);
+}
+
+void TinyGPS::crack_datetime(int *year, byte *month, byte *day, 
+  byte *hour, byte *minute, byte *second, byte *hundredths, unsigned long *age)
+{
+  unsigned long date, time;
+  get_datetime(&date, &time, age);
+  if (year) 
+  {
+    *year = date % 100;
+    *year += *year > 80 ? 1900 : 2000;
+  }
+  if (month) *month = (date / 100) % 100;
+  if (day) *day = date / 10000;
+  if (hour) *hour = time / 1000000;
+  if (minute) *minute = (time / 10000) % 100;
+  if (second) *second = (time / 100) % 100;
+  if (hundredths) *hundredths = time % 100;
+}
+
+float TinyGPS::f_altitude()    
+{
+  return _altitude == GPS_INVALID_ALTITUDE ? GPS_INVALID_F_ALTITUDE : _altitude / 100.0;
+}
+
+float TinyGPS::f_course()
+{
+  return _course == GPS_INVALID_ANGLE ? GPS_INVALID_F_ANGLE : _course / 100.0;
+}
+
+float TinyGPS::f_speed_knots() 
+{
+  return _speed == GPS_INVALID_SPEED ? GPS_INVALID_F_SPEED : _speed / 100.0;
+}
+
+float TinyGPS::f_speed_mph()   
+{ 
+  float sk = f_speed_knots();
+  return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPH_PER_KNOT * sk; 
+}
+
+float TinyGPS::f_speed_mps()   
+{ 
+  float sk = f_speed_knots();
+  return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_MPS_PER_KNOT * sk; 
+}
+
+float TinyGPS::f_speed_kmph()  
+{ 
+  float sk = f_speed_knots();
+  return sk == GPS_INVALID_F_SPEED ? GPS_INVALID_F_SPEED : _GPS_KMPH_PER_KNOT * sk; 
+}
+
+const float TinyGPS::GPS_INVALID_F_ANGLE = 1000.0;
+const float TinyGPS::GPS_INVALID_F_ALTITUDE = 1000000.0;
+const float TinyGPS::GPS_INVALID_F_SPEED = -1.0;

TinyGPS.h

@@ -0,0 +1,157 @@
+/*
+TinyGPS - a small GPS library for Arduino providing basic NMEA parsing
+Based on work by and "distance_to" and "course_to" courtesy of Maarten Lamers.
+Suggestion to add satellites(), course_to(), and cardinal(), by Matt Monson.
+Precision improvements suggested by Wayne Holder.
+Copyright (C) 2008-2013 Mikal Hart
+All rights reserved.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, write to the Free Software
+Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+
+#ifndef TinyGPS_h
+#define TinyGPS_h
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#include <stdlib.h>
+
+#define _GPS_VERSION 13 // software version of this library
+#define _GPS_MPH_PER_KNOT 1.15077945
+#define _GPS_MPS_PER_KNOT 0.51444444
+#define _GPS_KMPH_PER_KNOT 1.852
+#define _GPS_MILES_PER_METER 0.00062137112
+#define _GPS_KM_PER_METER 0.001
+// #define _GPS_NO_STATS
+
+class TinyGPS
+{
+public:
+  enum {
+    GPS_INVALID_AGE = 0xFFFFFFFF,      GPS_INVALID_ANGLE = 999999999, 
+    GPS_INVALID_ALTITUDE = 999999999,  GPS_INVALID_DATE = 0,
+    GPS_INVALID_TIME = 0xFFFFFFFF,		 GPS_INVALID_SPEED = 999999999, 
+    GPS_INVALID_FIX_TIME = 0xFFFFFFFF, GPS_INVALID_SATELLITES = 0xFF,
+    GPS_INVALID_HDOP = 0xFFFFFFFF
+  };
+
+  static const float GPS_INVALID_F_ANGLE, GPS_INVALID_F_ALTITUDE, GPS_INVALID_F_SPEED;
+
+  TinyGPS();
+  bool encode(char c); // process one character received from GPS
+  TinyGPS &operator << (char c) {encode(c); return *this;}
+
+  // lat/long in MILLIONTHs of a degree and age of fix in milliseconds
+  // (note: versions 12 and earlier gave lat/long in 100,000ths of a degree.
+  void get_position(long *latitude, long *longitude, unsigned long *fix_age = 0);
+
+  // date as ddmmyy, time as hhmmsscc, and age in milliseconds
+  void get_datetime(unsigned long *date, unsigned long *time, unsigned long *age = 0);
+
+  // signed altitude in centimeters (from GPGGA sentence)
+  inline long altitude() { return _altitude; }
+
+  // course in last full GPRMC sentence in 100th of a degree
+  inline unsigned long course() { return _course; }
+
+  // speed in last full GPRMC sentence in 100ths of a knot
+  inline unsigned long speed() { return _speed; }
+
+  // satellites used in last full GPGGA sentence
+  inline unsigned short satellites() { return _numsats; }
+
+  // horizontal dilution of precision in 100ths
+  inline unsigned long hdop() { return _hdop; }
+
+  void f_get_position(float *latitude, float *longitude, unsigned long *fix_age = 0);
+  void crack_datetime(int *year, byte *month, byte *day, 
+    byte *hour, byte *minute, byte *second, byte *hundredths = 0, unsigned long *fix_age = 0);
+  float f_altitude();
+  float f_course();
+  float f_speed_knots();
+  float f_speed_mph();
+  float f_speed_mps();
+  float f_speed_kmph();
+
+  static int library_version() { return _GPS_VERSION; }
+
+  static float distance_between (float lat1, float long1, float lat2, float long2);
+  static float course_to (float lat1, float long1, float lat2, float long2);
+  static const char *cardinal(float course);
+
+#ifndef _GPS_NO_STATS
+  void stats(unsigned long *chars, unsigned short *good_sentences, unsigned short *failed_cs);
+#endif
+
+private:
+  enum {_GPS_SENTENCE_GPGGA, _GPS_SENTENCE_GPRMC, _GPS_SENTENCE_OTHER};
+
+  // properties
+  unsigned long _time, _new_time;
+  unsigned long _date, _new_date;
+  long _latitude, _new_latitude;
+  long _longitude, _new_longitude;
+  long _altitude, _new_altitude;
+  unsigned long  _speed, _new_speed;
+  unsigned long  _course, _new_course;
+  unsigned long  _hdop, _new_hdop;
+  unsigned short _numsats, _new_numsats;
+
+  unsigned long _last_time_fix, _new_time_fix;
+  unsigned long _last_position_fix, _new_position_fix;
+
+  // parsing state variables
+  byte _parity;
+  bool _is_checksum_term;
+  char _term[15];
+  byte _sentence_type;
+  byte _term_number;
+  byte _term_offset;
+  bool _gps_data_good;
+
+#ifndef _GPS_NO_STATS
+  // statistics
+  unsigned long _encoded_characters;
+  unsigned short _good_sentences;
+  unsigned short _failed_checksum;
+  unsigned short _passed_checksum;
+#endif
+
+  // internal utilities
+  int from_hex(char a);
+  unsigned long parse_decimal();
+  unsigned long parse_degrees();
+  bool term_complete();
+  bool gpsisdigit(char c) { return c >= '0' && c <= '9'; }
+  long gpsatol(const char *str);
+  int gpsstrcmp(const char *str1, const char *str2);
+};
+
+#if !defined(ARDUINO) 
+// Arduino 0012 workaround
+#undef int
+#undef char
+#undef long
+#undef byte
+#undef float
+#undef abs
+#undef round 
+#endif
+
+#endif

driveCircle.h

@@ -0,0 +1,117 @@
+#ifndef DRIVE_CIRCLE_H
+#define DRIVE_CIRCLE_H
+
+#include <Arduino.h>
+
+#include "globals.h"
+
+float messung = 0;
+unsigned long msPrint = 0;
+float degStart = 0;
+int oldState = -1;
+
+
+void initAuto() {
+  
+}
+
+void driveAuto() {
+  int state = 0;
+  uint32_t elapsed = millis() - msAuto;
+  
+  if(elapsed > 1000) state = 1;// fahr los
+  if(elapsed > 6000) state = 2; //messung
+  if(elapsed > 36000) state = 3; // größerer kreis
+  if(elapsed > 41000) state = 4; //messung
+  if(elapsed > 71000) state = 5; // größerer kreis
+  if(elapsed > 76000) state = 6; //messung
+  if(elapsed > 106000) state = 7; //stop
+  
+  if(state == 2 || state == 4 || state == 6)
+    messung = mpu.winkelZ - degStart;
+
+  if(millis() - msPrint > 200) {
+    Serial.print("Auto: \t");
+    Serial.print(gasVal);
+    Serial.print(" \t");
+    Serial.print(servoVal);
+    Serial.print(" \t");
+    Serial.print(elapsed/1000);
+    Serial.print("s \tstate=");
+    Serial.print(state);
+    Serial.print(" \t");
+    Serial.print(mpu.winkelZ);
+    Serial.print("° \t");
+    Serial.print(messung);
+    Serial.print("° \t");
+    Serial.print(messung / 30);
+    Serial.print("°/s \t");
+    Serial.println();
+
+    Serial3.print("Auto: \t");
+    Serial3.print(gasVal);
+    Serial3.print(" \t");
+    Serial3.print(servoVal);
+    Serial3.print(" \t");
+    Serial3.print(elapsed/1000);
+    Serial3.print("s \tstate=");
+    Serial3.print(state);
+    Serial3.print(" \t");
+    Serial3.print(mpu.winkelZ);
+    Serial3.print("° \t");
+    Serial3.print(messung);
+    Serial3.print("° \t");
+    Serial3.print(messung / 30);
+    Serial3.print("°/s \t");
+    Serial3.println();
+    msPrint = millis();
+  }
+
+
+  if(oldState == state) {
+    return;
+  }
+
+  float spd = 0.11;
+  
+  switch(state) {
+    case 0:
+      setMotor(0);
+      setServo(0);
+      break;
+    case 1:
+      setMotor(spd);
+      setServo(1);
+      break;
+    case 2:
+      setMotor(spd);
+      setServo(1);
+      degStart = mpu.winkelZ;
+      break;
+    case 3:
+      setMotor(spd);
+      setServo(0.8);
+      break;
+    case 4:
+      setMotor(spd);
+      setServo(0.8);
+      degStart = mpu.winkelZ;
+      break;
+    case 5:
+      setMotor(spd);
+      setServo(0.6);
+      break;
+    case 6:
+      setMotor(spd);
+      setServo(0.6);
+      degStart = mpu.winkelZ;
+      break;
+    case 7:
+      setMotor(0);
+      setServo(0);
+      break;
+  }  
+  oldState = state;
+}
+
+#endif

driveRegler.h

@@ -0,0 +1,122 @@
+#ifndef DRIVE_REGLER_H
+#define DRIVE_REGLER_H
+
+#include "Arduino.h"
+#include "globals.h"
+#include "secrets.h"
+
+float sollRichtung, istRichtung;
+double pidIst, pidSoll, output;
+double Kp=1, Ki=50, Kd=0.11;
+float flat, flon;
+unsigned long age;
+unsigned long start = 0;
+float Longitudealt, Latitudealt, dist;
+
+unsigned long chars = 0;
+unsigned short sentences = 0, failed = 0;
+
+PID myPID(&pidIst, &output, &pidSoll, Kp, Ki, Kd, DIRECT);
+TinyGPS gps;
+
+const char header[] = "Mode \tGas \tServo \tistW° \tsollW° \tLenk \tLat \tLng \tDist \tAge";
+
+void initAuto() {
+  myPID.SetMode(AUTOMATIC);
+  sollRichtung = 0;
+
+  Serial3.println(header);
+  Serial.println(header);
+}
+
+void driveAuto() {
+  bool newdata = false;
+    
+  while(Serial2.available()) {
+    char c = Serial2.read();
+    if (gps.encode(c)) {
+      newdata = true;
+    }
+  }
+
+  if (newdata || (millis() - start > 1000)) {
+    gps.f_get_position(&flat, &flon, &age);
+    gps.stats(&chars, &sentences, &failed);
+
+    Serial.print("Auto: \t");
+
+    if(age > 1000){
+      Serial.print("valid sentences = "); 
+      Serial.print(sentences);
+
+      Serial.print("  Satellit wird gesucht...");
+      istRichtung = sollRichtung;
+
+    } 
+    else {
+      dist = gps.distance_between(flat, flon, s_lat, s_lon);
+      Serial.print("Latitude: ");
+      Serial.print(flat,10);
+      Serial.print(" \tLongitude: ");
+      Serial.print(flon,10);
+      Serial.print(" \tDist: ");
+      Serial.print(dist);
+      Serial.print(" \tTime: ");
+      Serial.print(age);
+      Serial.print("ms. ");
+      sollRichtung = gps.course_to(flat, flon, s_lat, s_lon); // Koordinaten zum Sportplatz !
+      if (Latitudealt != flat && Longitudealt != flon) {
+        istRichtung = gps.course_to(Latitudealt, Longitudealt, flat, flon);
+        Longitudealt = flon;
+        Latitudealt = flat;
+      }
+      //istRichtung = gps.f_course(); // Korrigieren der Lenkung zum Ziel
+    }
+    start = millis();
+    Serial3.print("Auto: \t");
+    Serial3.print(gasVal);
+    Serial3.print(" \t");
+    Serial3.print(servoVal);
+    Serial3.print(" \t");
+    Serial3.print(istRichtung);
+    Serial3.print(" \t");
+    Serial3.print(sollRichtung);
+    Serial3.print(" \t");
+    Serial3.print(output);
+
+    Serial3.print(" \t");
+    Serial3.print(flat,10);
+    Serial3.print(" \t");
+    Serial3.print(flon,10);
+    Serial3.print(" \t");
+    Serial3.print(dist);
+    Serial3.print(" \t");    
+    Serial3.print(age);
+    Serial3.println();
+
+    Serial.print(gasVal);
+    Serial.print(" \t");
+    Serial.print(servoVal);
+    Serial.print(" \t");
+    Serial.print(istRichtung);
+    Serial.print("\t");
+    Serial.print(sollRichtung);
+    Serial.print("\t");
+    Serial.println(output);
+    
+  }
+
+  //istRichtung = mpu.winkelZ;
+  //sollRichtung = spm.data[1] * 180;
+  
+
+
+  if(spm.hasData()) {
+    setMotor(spm.data[2]*0.2);
+    output = (((int)sollRichtung - (int)istRichtung + 180) % 360 - 180) / 180.0 * 0.3;
+    setServo(output);
+  }
+}
+
+
+#endif

drivefusion.h

@@ -0,0 +1,154 @@
+#ifndef DRIVE_REGLER_H
+#define DRIVE_REGLER_H
+
+#include "Arduino.h"
+#include "globals.h"
+#include "secrets.h"
+
+float sollRichtung, istRichtung;
+double pidIst, pidSoll, output;
+double Kp=1, Ki=50, Kd=0.11;
+float flat, flon;
+unsigned long age;
+unsigned long start = 0;
+float Longitudealt, Latitudealt, dist, bestwert;
+
+unsigned long chars = 0;
+unsigned short sentences = 0, failed = 0;
+
+PID myPID(&pidIst, &output, &pidSoll, Kp, Ki, Kd, DIRECT);
+TinyGPS gps;
+
+const char header[] = "Mode \tGas \tServo \tistW° \tsollW° \tLenk \tLat \tLng \tDist \tAge \tDatenfusion";
+
+void initAuto() {
+  myPID.SetMode(AUTOMATIC);
+  sollRichtung = 0;
+
+  Serial3.println(header);
+  Serial.println(header);
+}
+
+void driveAuto() {
+  bool newdata = false;
+    
+  while(Serial2.available()) {
+    char c = Serial2.read();
+    if (gps.encode(c)) {
+      newdata = true;
+    }
+  }
+
+  if (newdata || (millis() - start > 1000)) {
+    gps.f_get_position(&flat, &flon, &age);
+    gps.stats(&chars, &sentences, &failed);
+
+    Serial.print("Auto: \t");
+
+    if(age > 1000){
+      Serial.print("valid sentences = "); 
+      Serial.print(sentences);
+
+      Serial.print("  Satellit wird gesucht...");
+
+    } 
+    else {
+      dist = gps.distance_between(flat, flon, s_lat, s_lon);
+      Serial.print("Latitude: ");
+      Serial.print(flat,10);
+      Serial.print(" \tLongitude: ");
+      Serial.print(flon,10);
+      Serial.print(" \tDist: ");
+      Serial.print(dist);
+      Serial.print(" \tTime: ");
+      Serial.print(age);
+      Serial.print("ms. ");
+      sollRichtung = 359 - gps.course_to(flat, flon, s_lat, s_lon); // Koordinaten zum Sportplatz !
+      if (Latitudealt != flat && Longitudealt != flon) {
+        istRichtung = 359 - gps.course_to(Latitudealt, Longitudealt, flat, flon);
+        Longitudealt = flon;
+        Latitudealt = flat;
+
+        const float s1 = 0.31;
+        const float s2 = 0; // 0.95
+        
+        bestwert = (mpu.winkelZ) * (s2*s2 / (s1*s1 + s2*s2)) + istRichtung * (s1*s1 / (s1*s1 + s2*s2));
+        
+        mpu.winkelZ = bestwert;
+      }
+      //istRichtung = gps.f_course(); // Korrigieren der Lenkung zum Ziel
+    }
+    start = millis();
+    Serial3.print("Auto: \t");
+    Serial3.print(gasVal);
+    Serial3.print(" \t");
+    Serial3.print(servoVal);
+    Serial3.print(" \t");
+    Serial3.print(istRichtung);
+    Serial3.print(" \t");
+    Serial3.print(sollRichtung);
+    Serial3.print(" \t");
+    Serial3.print(output);
+
+    Serial3.print(" \t");
+    Serial3.print(flat,10);
+    Serial3.print(" \t");
+    Serial3.print(flon,10);
+    Serial3.print(" \t");
+    Serial3.print(dist);
+    Serial3.print(" \t");    
+    Serial3.print(age);
+    Serial3.print(" \t");    
+    Serial3.print(bestwert);
+    Serial3.print(" \t");    
+    Serial3.print(mpu.winkelZ);
+    Serial3.print(" \t");    
+    Serial3.print(failed);
+    Serial3.println();
+
+    Serial.print(gasVal);
+    Serial.print(" \t");
+    Serial.print(servoVal);
+    Serial.print(" \t");
+    Serial.print(istRichtung);
+    Serial.print("\t");
+    Serial.print(sollRichtung);
+    Serial.print("\t");
+    Serial.println(output);
+    
+  }
+
+  //istRichtung = mpu.winkelZ;
+  //sollRichtung = spm.data[1] * 180;
+
+  if(spm.hasData()) {
+    setMotor(spm.data[2]*0.3);
+    int winkelDiff = sollRichtung - mpu.winkelZ; // -360 - 360
+    // 0 - 180 -> rechts fahren
+    // -180 - 0 -> links
+    // 180 - 360 -> links
+    // -180 - -360 -> rechts
+
+    winkelDiff += 180;
+    //180 hizufuegen
+    // 180 - 360 -> rechts fahren
+    // -0 - 180 -> links
+    // 360 - 360+180 -> links
+    // 0 - -180 -> rechts
+
+    winkelDiff = (winkelDiff+360) % 360;
+    //0 - 360 modulo nur positiv
+    // 180 - 360 -> rechts fahren
+    // -0 - 180 -> links
+    // 0 - 180 -> links
+    // 360 - 180 -> rechts
+
+    output = 1.0 * (winkelDiff - 180) / 180.0;
+    // -1 - +1
+    
+    setServo(output);
+  }
+}
+
+
+#endif

globals.cpp

@@ -0,0 +1,32 @@
+#include "globals.h"
+
+int gasVal = 0, servoVal = 0;
+unsigned long msAuto = 0;
+
+Servo myServo;
+//ESC myGas(GAS_PIN, 1000, 2000, 500);
+Servo myGas;
+
+MPU6050 mpu;
+Spm4648 spm;
+
+void initServos() {
+  myServo.attach(SERVO_PIN);
+  myGas.attach(GAS_PIN);
+  //myGas.calib();
+  //myGas.arm();
+  setServo(0);
+  setMotor(0);
+}
+
+void setMotor(float val) {
+  val = constrain(val, -1, 1);
+  //gasVal = (val + 1.5) * 500; // 1000 .. 2000
+  gasVal = (val + 1) * 90;
+  myGas.write(gasVal);
+}
+void setServo(float val) {
+  val = constrain(val, -1, 1);
+  servoVal = 180 - (val + 0.08 + 1) * 90; // 180 .. 0
+  myServo.write(servoVal);
+}

globals.h

@@ -0,0 +1,27 @@
+#ifndef GLOBALS_H
+#define GLOBALS_h
+#include "TinyGPS.h"
+#include "Arduino.h"
+#include "spm4648.h"
+#include "MPU6050.h"
+#include "Servo.h"
+#include "ESC.h"
+#include "PID_v1.h"
+#include "math.h"
+
+
+#define GAS_PIN 6
+#define SERVO_PIN 3
+
+
+extern int gasVal, servoVal;
+extern unsigned long msAuto;
+
+void initServos();
+void setMotor(float val);
+void setServo(float val);
+
+extern MPU6050 mpu;
+extern Spm4648 spm;
+
+#endif

spm4648.cpp

@@ -0,0 +1,96 @@
+
+#include "spm4648.h"
+
+Spm4648::Spm4648() {
+}
+
+void Spm4648::read(Stream &port) {
+  if(micros() - timeout > 4000) { //interval is 21ms
+    p = 0;
+  }
+  while(port.available()>0) {
+    timeout = micros();
+    int8_t tmp = port.read();
+    if(p<length) {
+      buf[p++] = tmp;
+      if(p == length) {
+        Spm4648::parse();
+        newData = 1;
+        break;
+      }
+    } else {
+      while(port.available()>0)
+        port.read();
+      Serial.print("SPM: unexpected byte");
+
+      break;
+    }
+  }
+}
+    
+float Spm4648::getVal(byte x) {
+  for(byte i=0; i<idCount; i++) {
+    if(ids[i] == x) {
+      float raw = (vals[i] - MINVAL) * 2.0 / (MAXVAL-MINVAL) - 1;
+      if(raw > 20)
+        raw = -1;
+      return constrain(raw, -1, 1);
+    }
+  }
+  return 0;
+}
+void Spm4648::listVal() {
+  for(byte i=0; i<idCount; i++) {
+    Serial.print(ids[i], HEX);
+    Serial.print(F(": "));
+    Serial.print(vals[i]);
+    Serial.print(F("  \t"));
+  }
+  Serial.println();
+}
+
+bool Spm4648::hasData() {
+  if(newData) {
+    newData = 0;
+    return true;
+  }
+  return false;
+}
+bool Spm4648::isConnected() {
+  return micros() - timeout < 500000UL;
+}
+
+void Spm4648::parse() {
+  for(byte i=0; i<length-1; i+=2) {
+    byte id = buf[i] & 0b11111000;
+    uint16_t val = (buf[i]&7) << 8 | buf[i+1];
+    Spm4648::setVal(id, val);
+  }
+  data[0] = Spm4648::getVal(0x00);//ly
+  data[1] = Spm4648::getVal(0x18);//lx
+  data[2] = Spm4648::getVal(0x10);//ry
+  data[3] = Spm4648::getVal(0x08);//rx
+  data[4] = Spm4648::getVal(0x20);//sw
+}
+
+void Spm4648::setVal(byte x, uint16_t val) {
+  for(byte i=0; i<idCount; i++) {
+    if(ids[i] == x) {
+      //limit div size
+      if(val > MAXSTEP + vals[i])
+        vals[i] += MAXSTEP;
+      else if(vals[i] >= MAXSTEP && val < vals[i] - MAXSTEP)
+        vals[i] -= MAXSTEP;
+      else
+        vals[i] = val;
+      
+      return;
+    }
+  }
+  
+  if(idCount >= CHANNELS)
+    return;
+  
+  ids[idCount] = x;
+  vals[idCount++] = val;
+}

spm4648.h

@@ -0,0 +1,38 @@
+#ifndef Spm4648_h
+#define Spm4648_h
+
+#include "Arduino.h"
+
+#define CHANNELS 10
+
+#define MINVAL 306
+#define MAXVAL 1738
+
+#define MAXSTEP ((MAXVAL-MINVAL) / 10)
+
+class Spm4648 {
+  public:
+    Spm4648(void);
+    void read(Stream &port);
+    float getVal(byte x);
+    void listVal();
+    bool hasData();
+    bool isConnected();
+
+    float data[5];
+    uint8_t ids[CHANNELS];
+    uint16_t vals[CHANNELS];
+    uint8_t idCount = 0;
+    bool newData;
+
+  private:
+    void parse();
+    void setVal(byte x, uint16_t val);
+
+    uint8_t p = 0;
+    uint32_t timeout;
+    static const char length = 16;
+    uint8_t buf[length];
+};
+
+#endif