Arduino Code
include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>
#include <TinyGPS++.h>
#include <SoftwareSerial.h>s
// -----------------------
// IMU Class
// -----------------------
class IMU {
private:
Adafruit_MPU6050 mpu;
float gyroX_offset = 0;
float gyroY_offset = 0;
float gyroZ_offset = 0;
float accelX_offset = 0;
float accelY_offset = 0;
const float alpha = 0.98;
const float deadband = 0.02;
float pitch = 0;
float roll = 0;
unsigned long last_time = 0;
public:
float ax, ay, az;
float gx_deg, gy_deg, gz_deg;
bool begin() {
if (![Link]()) return false;
[Link](MPU6050_RANGE_8_G);
[Link](MPU6050_RANGE_500_DEG);
[Link](MPU6050_BAND_21_HZ);
last_time = millis();
return true;
}
void calibrate(int samples = 3000, int delay_ms = 2) {
sensors_event_t a, g, temp;
float gx_sum = 0, gy_sum = 0, gz_sum = 0;
float ax_sum = 0, ay_sum = 0;
for (int i = 0; i < samples; i++) {
[Link](&a, &g, &temp);
gx_sum += [Link].x;
gy_sum += [Link].y;
gz_sum += [Link].z;
ax_sum += [Link].x;
ay_sum += [Link].y;
delay(delay_ms);
}
gyroX_offset = gx_sum / samples;
gyroY_offset = gy_sum / samples;
gyroZ_offset = gz_sum / samples;
accelX_offset = ax_sum / samples;
accelY_offset = ay_sum / samples;
}
void update() {
sensors_event_t a, g, temp;
[Link](&a, &g, &temp);
float gx = [Link].x - gyroX_offset;
float gy = [Link].y - gyroY_offset;
float gz = [Link].z - gyroZ_offset;
gx_deg = gx * 57.2958;// 180/3.1416
gy_deg = gy * 57.2958;
gz_deg = gz * 57.2958;
if (abs(gx_deg) < deadband) gx_deg = 0;
if (abs(gy_deg) < deadband) gy_deg = 0;
if (abs(gz_deg) < deadband) gz_deg = 0;
ax = [Link].x - accelX_offset;
ay = [Link].y - accelY_offset;
az = [Link].z;
float dt = (millis() - last_time) / 1000.0;
last_time = millis();
float pitch_acc = atan2(-ax, sqrt(ay * ay + az * az)) * 57.2958;
float roll_acc = atan2(ay, az) * 57.2958;
pitch = alpha * (pitch + gx_deg * dt) + (1 - alpha) * pitch_acc;
roll = alpha * (roll + gy_deg * dt) + (1 - alpha) * roll_acc;
}
float getPitch() { return pitch; }
float getRoll() { return roll; }
};
// -----------------------
// Component Pins
// -----------------------
const int LED = 8;
const int Buzzer = 13;
Servo servo1, servo2, servo3, servo4;
const int servoPins[4] = {9, 10, 11, 12}; //
9=front,10=left,11=back,12=right
const int servo_center = 90;
const int max_servo_rate = 5;
// GPS Module Pins
const int GPS_RX = 7;
const int GPS_TX = 6;
// PID gains
float Kp_pitch = 2.0, Ki_pitch = 0.5, Kd_pitch = 0.5, Kff_pitch = 0.8;
float Kp_roll = 2.0, Ki_roll = 0.5, Kd_roll = 0.5, Kff_roll = 0.8;
// -----------------------
// Global Variables
// -----------------------
IMU imu;
float pitch_integral = 0, roll_integral = 0;
float last_pitch_error = 0, last_roll_error = 0;
float yaw = 0;
TinyGPSPlus gps;
SoftwareSerial gpsSerial(GPS_RX, GPS_TX);
// -----------------------
// LED & Buzzer Functions
// -----------------------
void LED_Light() {
digitalWrite(LED, HIGH);
}
void LED_Blink() {
for (int i = 0; i < 5; i++) {
digitalWrite(LED, HIGH); delay(100);
digitalWrite(LED, LOW); delay(100);
}
}
void Buzzer_sound() {
for (int i = 0; i <= 5; i++) {
digitalWrite(Buzzer, HIGH); delay(100);
digitalWrite(Buzzer, LOW); delay(100);
}
}
// -----------------------
// Setup
// -----------------------
void setup() {
[Link](115200);
pinMode(LED, OUTPUT);
pinMode(Buzzer, OUTPUT);
Buzzer_sound();
LED_Blink();
[Link](9600); // GPS baud rate
if (![Link]()) {
[Link]("IMU initialization failed!");
while (1);
}
[Link]();
[Link](servoPins[0]);
[Link](servoPins[1]);
[Link](servoPins[2]);
[Link](servoPins[3]);
[Link](servo_center);
[Link](servo_center);
[Link](servo_center);
[Link](servo_center);
Buzzer_sound();
}
// -----------------------
// Loop
// -----------------------
void loop() {
LED_Light();
// Update IMU
[Link]();
float dt = 0.05; // 50ms loop
yaw += imu.gz_deg * dt;
// -----------------------
// PID control for pitch & roll
// -----------------------
float pitch_setpoint = 0; // target vertical
float roll_setpoint = 0;
// Pitch PID
float pitch_error = pitch_setpoint - [Link]();
pitch_integral += pitch_error * dt;
pitch_integral = constrain(pitch_integral, -20, 20);
float pitch_derivative = (pitch_error - last_pitch_error) / dt;
last_pitch_error = pitch_error;
float pitch_output = Kp_pitch*pitch_error + Ki_pitch*pitch_integral +
Kd_pitch*pitch_derivative + Kff_pitch*imu.gx_deg;
// Roll PID
float roll_error = roll_setpoint - [Link]();
roll_integral += roll_error * dt;
roll_integral = constrain(roll_integral, -20, 20);
float roll_derivative = (roll_error - last_roll_error) / dt;
last_roll_error = roll_error;
float roll_output = Kp_roll*roll_error + Ki_roll*roll_integral +
Kd_roll*roll_derivative + Kff_roll*imu.gy_deg;
// -----------------------
// Servo updates (stabilization)
// -----------------------
static int servo1_last = servo_center;
static int servo2_last = servo_center;
static int servo3_last = servo_center;
static int servo4_last = servo_center;
int servo_front = constrain(servo_center + pitch_output, 0, 180); // front
int servo_back = constrain(servo_center - pitch_output, 0, 180); // back
int servo_left = constrain(servo_center + roll_output, 0, 180); // left
int servo_right = constrain(servo_center - roll_output, 0, 180); // right
// Smooth servo movement
servo1_last = constrain(servo_front, servo1_last - max_servo_rate,
servo1_last + max_servo_rate);
servo3_last = constrain(servo_back, servo3_last - max_servo_rate,
servo3_last + max_servo_rate);
servo2_last = constrain(servo_left, servo2_last - max_servo_rate,
servo2_last + max_servo_rate);
servo4_last = constrain(servo_right, servo4_last - max_servo_rate,
servo4_last + max_servo_rate);
[Link](servo1_last);
[Link](servo3_last);
[Link](servo2_last);
[Link](servo4_last);
// -----------------------
// Read GPS
// -----------------------
while ([Link]() > 0) {
[Link]([Link]());
}
double lat = 0.0, lon = 0.0;
bool gps_valid = false;
if ([Link]()) {
lat = [Link]();
lon = [Link]();
gps_valid = true;
}
// Google Maps link or GPS not available
String gmap_link = gps_valid ?
"[Link] + String(lat, 6) + "," + String(lon,
6) :
"GPS fix not yet";
// -----------------------
// Send serial data
// -----------------------
[Link]([Link]()); [Link](",");
[Link]([Link]()); [Link](",");
[Link](yaw); [Link](",");
[Link]([Link]); [Link](",");
[Link]([Link]); [Link](",");
[Link]([Link]); [Link](",");
[Link](lat); [Link](",");
[Link](lon); [Link](",");
[Link](gmap_link);
delay(50);
}