Drone Arduino Code

Hello everyone! Welcome to Arduino Geek. Today we will discuss about the Arduino code for Drone. So let's get started.

Drone Arduino Code - 

Here is an example Arduino code for a quadcopter drone that uses an MPU6050 sensor for stabilization and control:

Arduino Code -

// Include libraries

#include <Wire.h>

#include <I2Cdev.h>

#include <MPU6050.h>

// Define motor pins

const int motor1Pin = 3;

const int motor2Pin = 5;

const int motor3Pin = 6;

const int motor4Pin = 9;

// Declare MPU6050 object

MPU6050 mpu;

// Initialize variables

int16_t ax, ay, az;

int16_t gx, gy, gz;

float roll, pitch, yaw;

float rollSetpoint, pitchSetpoint, yawSetpoint;

float pidRollInput, pidPitchInput, pidYawInput;

float pidRollOutput, pidPitchOutput, pidYawOutput;

float pidRollSetpoint = 0, pidPitchSetpoint = 0, pidYawSetpoint = 0;

float pidRollKp = 2, pidPitchKp = 2, pidYawKp = 2;

float pidRollKi = 0.01, pidPitchKi = 0.01, pidYawKi = 0.01;

float pidRollKd = 0.001, pidPitchKd = 0.001, pidYawKd = 0.001;

// PID variables

unsigned long pidRollLastTime, pidPitchLastTime, pidYawLastTime;

float pidRollErrorSum, pidPitchErrorSum, pidYawErrorSum;

float pidRollLastError, pidPitchLastError, pidYawLastError;

// PID limits

const float pidRollMaxOutput = 255;

const float pidRollMinOutput = -255;

const float pidPitchMaxOutput = 255;

const float pidPitchMinOutput = -255;

const float pidYawMaxOutput = 255;

const float pidYawMinOutput = -255;

// Setup function

void setup() {

  // Set motor pins as outputs

  pinMode(motor1Pin, OUTPUT);

  pinMode(motor2Pin, OUTPUT);

  pinMode(motor3Pin, OUTPUT);

  pinMode(motor4Pin, OUTPUT);

  // Initialize MPU6050

  Wire.begin();

  mpu.initialize();

  mpu.setFullScaleGyroRange(2000);

  mpu.setFullScaleAccelRange(2);

  mpu.setDLPFMode(6);

}

// Loop function

void loop() {

  // Read sensor values

  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Convert sensor values to degrees

  roll = atan2(ay, az) * 180 / M_PI;

  pitch = atan2(-ax, sqrt(ay * ay + az * az)) * 180 / M_PI;

  yaw = atan2(gx, sqrt(gy * gy + gz * gz)) * 180 / M_PI;

  // Calculate PID inputs

  pidRollInput = roll - pidRollSetpoint;

  pidPitchInput = pitch - pidPitchSetpoint;

  pidYawInput = yaw - pidYawSetpoint;

  // Calculate PID outputs

  unsigned long pidRollNow = millis();

  float pidRollTimeChange = (float)(pidRollNow - pidRollLastTime);

  pidRollErrorSum += pidRollInput * pidRollTimeChange;

  pidRollLastError = pidRollInput;

  pidRollLastTime = pidRollNow;

  pidRollOutput = pidRollKp * pidRollInput + pidRollKi * pidRollErrorSum + pidRollKd * (pidRollInput - pidRollLastError);

  pidRollOutput = constrain(pidRollOutput, pidRollMinOutput, pidRollMaxOutput);

  unsigned long pidPitchNow = millis();

  float pidPitchTimeChange = (float)(pidPitchNow - pidPitchLastTime);

  pidPitchErrorSum += pidPitchInput * pidPitchTimeChange;

  pidPitchLastError = pidPitchInput;

  pidPitchLastTime = pidPitchNow;

  pidPitchOutput = pidPitchKp * pidPitchInput + pidPitchKi * pidPitchErrorSum + pidPitchKd * (pidPitchInput - pidPitchLastError);

  pidPitchOutput = constrain(pidPitchOutput, pidPitchMinOutput, pidPitchMaxOutput);

  unsigned long pidYawNow = millis();

  float pidYawTimeChange = (float)(pidYawNow - pidYawLastTime);

  pidYawErrorSum += pidYawInput * pidYawTimeChange;

  pidYawLastError = pidYawInput;

  pidYawLastTime = pidYawNow;

  pidYawOutput = pidYawKp * pidYawInput + pidYawKi * pidYawErrorSum + pidYawKd * (pidYawInput - pidYawLastError);

  pidYawOutput = constrain(pidYawOutput, pidYawMinOutput, pidYawMaxOutput);

  // Apply motor outputs

  analogWrite(motor1Pin, 1000 + pidRollOutput - pidPitchOutput + pidYawOutput);

  analogWrite(motor2Pin, 1000 - pidRollOutput - pidPitchOutput - pidYawOutput);

  analogWrite(motor3Pin, 1000 - pidRollOutput + pidPitchOutput + pidYawOutput);

  analogWrite(motor4Pin, 1000 + pidRollOutput + pidPitchOutput - pidYawOutput);

  }