Untitled
unknown
c_cpp
a year ago
2.6 kB
6
Indexable
#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); } } }
Editor is loading...
Leave a Comment