drone — Esp32 Devkit C V4 simulation

An interactive Esp32 Devkit C V4 circuit simulation you can run free in your browser on Velxio, by tareq-neoaz.

Components

• Pro Dc Motor x4
• Grove Touch Sensor x2
• Mpu6050
• Ssd1306 I2C 4Pin

Sketch code

#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_SSD1306.h>
#include <math.h>

// ===== PINS =====
#define MOTOR_FL 25
#define MOTOR_FR 26
#define MOTOR_RL 27
#define MOTOR_RR 14
#define TOUCH_START 32
#define TOUCH_STOP  33

// ===== OBJECTS =====
Adafruit_MPU6050 mpu;
Adafruit_SSD1306 display(128, 64, &Wire, -1);

// ===== PID =====
float kP = 2.0, kI = 0.0, kD = 0.5;
float integralX = 0, lastErrorX = 0;
float integralY = 0, lastErrorY = 0;
unsigned long lastTime = 0;

// ===== STATE =====
int throttle = 0;
bool armed = false;

float computePID(float target, float current, float &integral, float &lastError, float dt) {
  float error = target - current;
  float P = kP * error;
  integral += error * dt;
  float I = kI * integral;
  float D = kD * (error - lastError) / dt;
  lastError = error;
  return P + I + D;
}

void setup() {
  Serial.begin(115200);
  Wire.begin(21, 22);

  pinMode(MOTOR_FL, OUTPUT);
  pinMode(MOTOR_FR, OUTPUT);
  pinMode(MOTOR_RL, OUTPUT);
  pinMode(MOTOR_RR, OUTPUT);
  pinMode(TOUCH_START, INPUT);
  pinMode(TOUCH_STOP, INPUT);

  if (!mpu.begin()) {
    Serial.println("MPU6050 not found!");
    while (1);
  }

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.println("Drone Ready");
  display.display();
  delay(1000);

  lastTime = millis();
}

void loop() {
  bool startPressed = digitalRead(TOUCH_START) == HIGH;
  bool stopPressed  = digitalRead(TOUCH_STOP)  == HIGH;

  if (startPressed && !armed) {
    armed = true;
    throttle = 150;
  }
  if (stopPressed) {
    armed = false;
    throttle = 0;
  }

  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  float angleX = atan2(a.acceleration.y, a.acceleration.z) * 180.0 / PI;
  float angleY = atan2(a.acceleration.x, a.acceleration.z) * 180.0 / PI;

  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  lastTime = now;

  float pidRoll  = computePID(0, angleX, integralX, lastErrorX, dt);
  float pidPitch = computePID(0, angleY, integralY, lastErrorY, dt);

  int motorFL = throttle - pidPitch + pidRoll;
  int motorFR = throttle - pidPitch - pidRoll;
  int motorRL = throttle + pidPitch + pidRoll;
  int motorRR = throttle + pidPitch - pidRoll;

  motorFL = constrain(motorFL, 0, 255);
  motorFR = constrain(motorFR, 0, 255);
  motorRL = constrain(motorRL, 0, 255);
  motorRR = constrain(motorRR, 0, 255);

  if (!armed) {
    motorFL = motorFR = motorRL = motorRR = 0;
  }

  analogWrite(MOTOR_FL, motorFL);
  analogWrite(MOTOR_FR, motorFR);
  analogWrite(MOTOR_RL, motorRL);
  analogWrite(MOTOR_RR, motorRR);

  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("Status: ");
  display.println(armed ? "ARMED" : "IDLE");
  display.setCursor(0, 16);
  display.print("AX:"); display.println(angleX, 1);
  display.setCursor(0, 32);
  display.print("AY:"); display.println(angleY, 1);
  dis

More projects by tareq-neoaz

• wireless drone — Esp32 Devkit C V4
• Indicator — Esp32 Devkit C V4
• clock — Esp32 Devkit C V4
• Project I — Esp32 Devkit C V4
• Project 1.2 — Esp32 Devkit C V4