JOYSTICK IMU — ESP32 simulation

An interactive ESP32 circuit simulation you can run free in your browser on Velxio, by josephkinuthia541.

Components

Sketch code

#include <Wire.h>

// ===============================
// JOYSTICK PINS
// ===============================
const int throttlePin = 34;
const int rollPin     = 35;
const int pitchPin    = 32;
const int yawPin      = 33;

// ===============================
// MOTOR PINS
// ===============================
const int M1 = 25;
const int M2 = 26;
const int M3 = 27;
const int M4 = 14;

// ===============================
// JOYSTICK SETTINGS
// ===============================
const int CENTER = 2047;
const int DEADZONE = 300;

// ===============================
// IMU
// ===============================
const byte IMU_ADDRESS = 0x6A;

const byte CTRL1_XL = 0x10;
const byte CTRL2_G  = 0x11;

const byte OUTX_L_G  = 0x22;
const byte OUTX_L_XL = 0x28;

// ===============================
// ATTITUDE
// ===============================
float roll = 0.0;
float pitch = 0.0;

// ===============================
// DESIRED ATTITUDE
// ===============================
float desiredRoll = 0.0;
float desiredPitch = 0.0;

// Maximum commanded angle
const float MAX_ANGLE = 30.0;

// ===============================
// PID GAINS
// ===============================
float rollKp = 2.0;
float rollKi = 0.05;
float rollKd = 0.8;

float pitchKp = 2.0;
float pitchKi = 0.05;
float pitchKd = 0.8;

// ===============================
// PID VARIABLES
// ===============================
float rollError = 0.0;
float rollIntegral = 0.0;

float pitchError = 0.0;
float pitchIntegral = 0.0;

// ===============================
// TIME
// ===============================
unsigned long previousTime = 0;


// =================================================
// SETUP
// =================================================

void setup()
{
  Serial.begin(115200);

  // I2C
  Wire.begin(21, 22);

  // Motor pins
  pinMode(M1, OUTPUT);
  pinMode(M2, OUTPUT);
  pinMode(M3, OUTPUT);
  pinMode(M4, OUTPUT);

  // ===============================
  // CONFIGURE IMU
  // ===============================

  // Accelerometer:
  // 104 Hz, +/-4G
  writeRegister(CTRL1_XL, 0x48);

  // Gyroscope:
  // 104 Hz, +/-245 dps
  writeRegister(CTRL2_G, 0x40);

  previousTime = micros();

  Serial.println("================================");
  Serial.println("STAGE 10C");
  Serial.println("JOYSTICK CONTROLLED PID");
  Serial.println("================================");
}


// =================================================
// MAIN LOOP
// =================================================

void loop()
{
  // =================================================
  // 1. READ JOYSTICKS
  // =================================================

  int throttleRaw = analogRead(throttlePin);
  int rollRaw     = analogRead(rollPin);
  int pitchRaw    = analogRead(pitchPin);
  int yawRaw      = analogRead(yawPin);


  // =================================================
  // 2. CONVERT JOYSTICK VALUES
  // =================================================

  // THROTTLE:
  // UP   = 255
  // DOWN = 0

  int throttleCommand =
    throttleToPWM

More projects by josephkinuthia541