ناوبری GPS خودکار UGV با آردوینو — دنبال کردن Waypoint‌ها (فارسی)

BOM

کد ناوبری Waypoint

#include <TinyGPS++.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
#include <Servo.h>

TinyGPSPlus gps;
QMC5883LCompass compass;
Servo steer;

// ===== Waypoint‌ها =====
const int NUM_WP = 3;
double wpLat[] = {35.712500, 35.712800, 35.713100};
double wpLng[] = {51.405200, 51.405600, 51.406000};
int currentWP = 0;

const float ARRIVAL_RADIUS = 3.0;  // متر — در این شعاع به Waypoint رسیدیم
const int   STEER_CENTER   = 90;

// فاصله بین دو نقطه GPS (متر)
float gpsDist(double lat1, double lng1, double lat2, double lng2){
  const float R = 6371000;
  float dLat = radians(lat2-lat1);
  float dLng = radians(lng2-lng1);
  float a = sin(dLat/2)*sin(dLat/2) +
            cos(radians(lat1))*cos(radians(lat2))*sin(dLng/2)*sin(dLng/2);
  return R * 2 * atan2(sqrt(a),sqrt(1-a));
}

// جهت لازم از نقطه A به B (درجه از شمال)
float gpsBearing(double lat1, double lng1, double lat2, double lng2){
  float dLng = radians(lng2-lng1);
  float y = sin(dLng)*cos(radians(lat2));
  float x = cos(radians(lat1))*sin(radians(lat2)) -
            sin(radians(lat1))*cos(radians(lat2))*cos(dLng);
  return fmod(degrees(atan2(y,x))+360, 360);
}

void setup(){
  Serial1.begin(9600);  // GPS
  compass.init();
  steer.attach(10); steer.write(STEER_CENTER);
}

void loop(){
  while(Serial1.available()) gps.encode(Serial1.read());

  if(!gps.location.isValid() || currentWP >= NUM_WP) return;

  float dist = gpsDist(gps.location.lat(), gps.location.lng(),
                       wpLat[currentWP], wpLng[currentWP]);

  if(dist < ARRIVAL_RADIUS){
    currentWP++;  // Waypoint رسیدیم
    return;
  }

  float targetBearing = gpsBearing(gps.location.lat(), gps.location.lng(),
                                   wpLat[currentWP], wpLng[currentWP]);
  compass.read();
  float heading = compass.getAzimuth();

  float error = targetBearing - heading;
  if(error > 180)  error -= 360;
  if(error < -180) error += 360;

  int servoAngle = STEER_CENTER + constrain((int)(error*0.5), -35, 35);
  steer.write(servoAngle);
  // گاز موتور ثابت: drive(150, 150)
}

چک‌لیست ناوبری

• Movable Type: فرمول‌های Haversine و Bearing
• آپارات: ناوبری GPS ربات