Автопилот

#include <Average.h>
#include <SoftwareSerial.h>
#include <PWMServo.h>
#include <Wire.h>
#include <HMC5883L.h>
#include "TinyGPS++.h"
 
int compassavg = 50;
PWMServo rudder;
 
int maxspeed = 40;
int waypointdist = 8;
//INITIALLY 4 DOWN
 
int prevangle = 90;
int accuracy = 4;
 
int rudderspeed = 10;
 
int lock = 12;
int distance = 7;
int data = 5;
 
int gpsavg = 10;
 
int ruddertrim = -2;
int compasstrim = 180;
 
int ang = 0;
 
static int r = 6371;
const float Pi = 3.14159;
 
HMC5883L compass;
int error = 0;
 
TinyGPSPlus gps;
 
unsigned long startTime;
unsigned long otherTime;
 
SoftwareSerial ss(5, 4);
 
void setup() {
  Serial.begin(4800);
  rudder.attach(9);
  Wire.begin();
  compass = HMC5883L();
  error = compass.SetScale(0.88);
  
  error = compass.SetMeasurementMode(Measurement_Continuous);
  
  ss.begin(9600);      
  
}
 
void loop() {
Serial.println(GetBearing());
  startTime = millis();
  float bearing = RealBearing(30,60,GetBearing());  //49 -123
  int heading = 0;
  RudderTurn(RudderAngle(bearing,heading));
  Serial.println(millis() - startTime);
  delay(200);
  
}
 
static void KeepHeading(int heading) {
  float bearing = RealBearing(30,60,GetBearing());  //49 -123
  RudderTurn(RudderAngle(bearing,heading));
  delay(200);
}
 
static void GoByPath(float lat2, float long2){
  gps.encode(ss.read());
  while (1 > 0){
    if (millis() > 5000 && gps.charsProcessed() < 10)
    {
      digitalWrite(data, HIGH);
    }
    else {
      digitalWrite(data, LOW);
    }
    while (ss.available() > 0){
      gps.encode(ss.read());
      if (gps.location.isUpdated())
      {
pathagain:
        if (gps.speed.mps() > maxspeed){ 
          delay(100);
          goto pathagain;
        }
 
        int latslice = gpsavg;
        int lati;
        Average<float> lataverage(latslice);
        for (int lati = 0; lati < latslice; lati = lati + 1) {
       
        lataverage.push(gps.location.lat()); 
        }
        float lat1 = (lataverage.mean());    // mean среднее арифметическое
 
 
        int longslice = gpsavg;
        int longi;
       Average<float> longaverage(longslice);
        for (longi = 0; longi < longslice; longi = longi + 1) {
          longaverage.push (gps.location.lng());
         
        }
        float long1 = (longaverage.mean());
 
        
        float heading = GetHeading(lat1, long1, lat2, long2);
        while (1 > 0){
          if (GetDistanceInM(lat1, long1, lat2, long2) > accuracy){
            float heading = GetHeading(lat1, long1, lat2, long2);
            long1=GetPosition(lat1, long1, waypointdist, heading, 2);
            lat1=GetPosition(lat1, long1, waypointdist, heading, 1);
            
            GoToWaypoint(lat1, long1);
            //delay(200);
          }
          else {
           
            return; 
          }
        }
      }
    }
  }
}
 
static void GoToWaypoint(float lat2, float long2){
  gps.encode(ss.read());
  while (1 > 0){
    while (ss.available() > 0){
      gps.encode(ss.read());
      if (gps.location.isUpdated())
      {
        digitalWrite(lock, HIGH);
        float lat1, long1;
        lat1 = gps.location.lat();
        long1 = gps.location.lng();
        if (GetDistanceInM(lat1, long1, lat2, long2) < (accuracy/1.9)){
          
          digitalWrite(distance, HIGH);
          delay(500);
          digitalWrite(distance, LOW);
          return;
        }
        else {
 
gpsagain:
 
          int latslice = gpsavg;
          int lati;
          Average<float> lataverage(latslice);
        for (int lati = 0; lati < latslice; lati = lati + 1) {
       
        lataverage.push(gps.location.lat()); 
        }
        float lat1 = (lataverage.mean());    // mean среднее арифметическое
 
          int longslice = gpsavg;
          int longi;
 Average<float> longaverage(longslice);
        for (longi = 0; longi < longslice; longi = longi + 1) {
          longaverage.push (gps.location.lng());
         
        }
        float long1 = (longaverage.mean());
 
          if (gps.speed.knots() > maxspeed){ 
            delay(100);
            goto gpsagain;
          }
          delay(20); 
 
          int slice = 20;
          int i;
                 Average<int> average(slice);  
          for (i = 0; i < slice; i = i + 1) {
                    average.push(GetBearing());  //(int)
          }
          float bearing = average.mode();           // mode  это число, которое повторяется чаще, чем любой другой
         // int average[slice];
         // for (i = 0; i < slice; i = i + 1) {
          //  average[i] = (int)GetBearing();
          //}
       //   float bearing = mode(average, slice);
          float heading = GetHeading(lat1, long1, lat2, long2);
          bearing = RealBearing(lat1, long1, bearing);
          RudderTurn(RudderAngle(bearing, heading));
 
          
        }
        
      }
      digitalWrite(lock, LOW);
    }
  }
  return;
}
 
 
static void RudderTurn(int angle){
  otherTime = millis();
  while (angle > prevangle) {
    prevangle++;
    rudder.write(prevangle); 
    delay(rudderspeed);  
  }
  while (angle < prevangle) {
    prevangle--;
    rudder.write(prevangle); 
    delay(rudderspeed);  
  }
  otherTime = millis() - otherTime;
  if (otherTime < 800) {
    delay(800 - otherTime);
  }
  prevangle = angle;
}
  static float GetPosition(float lat1, float long1, float distance, float heading, int option)
{
  
  distance = distance / 1000;
  lat1 = ConvertToRadians(lat1);
  long1 = ConvertToRadians(long1);
  heading = ConvertToRadians(heading);
  float lat2 = asin(sin(lat1) * cos(distance / r) +
    cos(lat1) * sin(distance / r) * cos(heading));
  float long2 = long1 + atan2(sin(heading) * sin(distance / r) * cos(lat1),
  cos(distance / r) - sin(lat1) * sin(lat2));
  if (option == 1)
  {
    return ConvertToDegrees(lat2);
  }
  else
  {
    return ConvertToDegrees(long2);
  }
}
 
static int RudderAngle(float bearing, float heading){
  heading = (int)heading % 360;
  bearing = (int)bearing % 360;
  if (heading == bearing)
  {
    return 90;
  }
  int o1 = (int)heading - (int)bearing;
  int o2 = (int)bearing - (int)heading;
  int dir = ((360-((int)heading - (int)bearing)) % 360);
  if (dir < 180) {
    if (dir > 30)
    {
      return 60;
    }
    else
    {
      return 90 - dir / 2;
    }
  }
  else if (dir >= 180)
  {
    if ((360 - dir) > 30)
    {
      return 120;
    }
    else
    {
      return 90 + (360 - dir) / 2;
    }
  }
  return 90;
}
 
static float GetBearing(){
  int i;
  Average <int> average(compassavg);
  for (i = 0; i < compassavg; i = i + 1) {
    MagnetometerRaw raw = compass.ReadRawAxis();
    MagnetometerScaled scaled = compass.ReadScaledAxis();
    int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)
    float heading = atan2(scaled.YAxis, scaled.XAxis);
    if(heading < 0)
      heading += 2*Pi;
    if(heading > 2*Pi)
      heading -= 2*Pi;
    float headingDegrees = (heading * 180/M_PI); 
    headingDegrees = fmod((headingDegrees + compasstrim),360);  // 1111112222222222222222222222222222222222222224444444444444444444443333333333333333333333355555555555555555555444444444444444 
    average.push(headingDegrees);
  }
  return (average.mode());
 
  //return mode(average, compassavg);
}
 
 
static float RealBearing(float lat1, float long1, float bearing)
{
  
  float magdif = GetHeading(lat1, long1, 75.7667, -99.7833);
  return (fmod((bearing + magdif),360));
}
static float GetHeading(float lat1, float long1, float lat2, float long2)
{
  
  lat1 = ConvertToRadians(lat1);
  lat2 = ConvertToRadians(lat2);
  long1 = ConvertToRadians(long1);
  long2 = ConvertToRadians(long2);
  float dLon = long2 - long1;
  float y = sin(dLon) * cos(lat2);
  float x = cos(lat1) * sin(lat2) - sin(lat1) * cos(lat2) * cos(dLon);
  return ConvertToStaticDegrees(atan2(y, x));
}
static float GetDistanceInM(float lat1, float long1, float lat2, float long2)
{
  
  lat1 = ConvertToRadians(lat1);
  lat2 = ConvertToRadians(lat2);
  long1 = ConvertToRadians(long1);
  long2 = ConvertToRadians(long2);
  float dLat = lat2 - lat1;
  float dLon = long2 - long1;
  float a = haversin(dLat) + cos(lat1) * cos(lat2) * haversin(dLon);
  float c = 2 * atan2(sqrt(a), sqrt(1 - a));
  return r * c * 1000; // Distance in M
}
static float ConvertToDegrees(float rad)
{
  float degrees = rad / (Pi / 180);
  if (degrees <= -180) {
    return degrees + 360;
  }
  else {
    return degrees;
  }
}
static float ConvertToStaticDegrees(float rad)
{
  float degrees = rad / (Pi / 180);
  if (degrees <= 0) {
    return degrees + 360;
  }
  else {
    return degrees;
  }
}
static float ConvertToRadians(float angle)
{
  return (Pi / 180) * angle;
}
static float haversin(float input)
{
  return (sin(input / 2)) * (sin(input / 2));
}
 
static void RoutineDone(){
  RudderTurn(0);
  delay(100);
  RudderTurn(180);
  delay(100);
}