Untitled

#include <TinyGPS++.h>
#include <SoftwareSerial.h>
#include <SwitecX25.h>
#include <TinyGPS.h>
static const uint32_t GPSBaud = 9600;
SwitecX25 motor1(219*3,5,6,7,8);
TinyGPSPlus gpsp;
TinyGPS gps;
SoftwareSerial ss(0, 1);

float flat, flon,lat1,lon1;
unsigned long age;
bool newData = false;
//test
const double EARTH_RADIUS_KM = 6371.0; // Earth's radius in kilometers

// Function to convert degrees to radians
double toRadians(double degrees) {
  return degrees * PI / 180.0;
}

// Function to calculate the distance using the Haversine formula
double calculateDistance(double lat1, double lon1, double lat2, double lon2) {
  double dlat = toRadians(lat2 - lat1);
  double dlon = toRadians(lon2 - lon1);

  double a = sin(dlat / 2) * sin(dlat / 2) +
             cos(toRadians(lat1)) * cos(toRadians(lat2)) *
             sin(dlon / 2) * sin(dlon / 2);

  double c = 2 * atan2(sqrt(a), sqrt(1 - a));
  double distance = EARTH_RADIUS_KM * c;

  return distance;
}


void gps_data_update(){
    for (unsigned long start = millis(); millis() - start < 1000;)
  {
    while (ss.available())
    {
      char c = ss.read();
      // Serial.write(c); // uncomment this line if you want to see the GPS data flowing
      if (gps.encode(c)) // Did a new valid sentence come in?
        newData = true;
    }
  }

  if (newData)
  {
    gps.f_get_position(&flat, &flon, &age);
    Serial.print("LAT=");
    Serial.println(flat == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flat, 6);
    Serial.print(" LON=");
    Serial.println(flon == TinyGPS::GPS_INVALID_F_ANGLE ? 0.0 : flon, 6);

}}

//koniec testu, zmienilem tinygps++ na gpsp - jak chujowo to wyjebac tinygps i zmienic plusa na gps
void setup(){
  Serial.begin(9600);
  ss.begin(GPSBaud);
  motor1.currentStep = 50;
  motor1.setPosition(0);
  delay(5);
  while (motor1.currentStep > 0) {
    if (motor1.currentStep < 10){
      delay(50);
      motor1.update();
    }
    else{
      motor1.update();
    }
    }
  for (int i = 0; i<657; i++){
    motor1.setPosition(i);
    motor1.updateBlocking();
  };
  for (int i = 657; i>0; i--){
    motor1.setPosition(i);
    motor1.updateBlocking();
  };

  
}

void loop() {
  while (ss.available() > 0){
    gpsp.encode(ss.read());
    if (gpsp.location.isUpdated()){
        motor1.setPosition(gpsp.speed.kmph() * 4.7);
        motor1.updateBlocking();
        gps_data_update();
        float dis=calculateDistance(lat1,lon1,flat, flon);
        Serial.print("distance:");
        Serial.println(dis);
      }
    }
  }