10b PID — ESP32 simulation

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

Components

Sketch code

#include <Wire.h>


// =====================================================
// LSM6DS3 I2C ADDRESS
// =====================================================

#define LSM6DS3_ADDR 0x6A


// =====================================================
// LSM6DS3 REGISTERS
// =====================================================

#define WHO_AM_I     0x0F
#define CTRL1_XL     0x10
#define CTRL2_G      0x11

#define OUTX_L_G     0x22
#define OUTX_L_XL    0x28


// =====================================================
// JOYSTICK INPUTS
// =====================================================

// JOYSTICK 1

const int ROLL_PIN     = 35;
const int THROTTLE_PIN = 34;


// JOYSTICK 2

const int PITCH_PIN = 32;
const int YAW_PIN   = 33;


// =====================================================
// MOTOR OUTPUTS
// =====================================================

const int MOTOR1_PIN = 25;
const int MOTOR2_PIN = 26;
const int MOTOR3_PIN = 27;
const int MOTOR4_PIN = 14;


// =====================================================
// JOYSTICK SETTINGS
// =====================================================

const int CENTER = 2047;

const int DEADZONE = 300;


// =====================================================
// ATTITUDE VARIABLES
// =====================================================

float roll = 0.0;

float pitch = 0.0;


// =====================================================
// TIME
// =====================================================

unsigned long previousTime;


// =====================================================
// PID GAINS
// =====================================================

// ROLL PID

float rollKp = 2.0;
float rollKi = 0.05;
float rollKd = 0.8;


// PITCH PID

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


// =====================================================
// PID VARIABLES
// =====================================================

// Roll

float rollError = 0.0;

float rollIntegral = 0.0;

float previousRollError = 0.0;


// Pitch

float pitchError = 0.0;

float pitchIntegral = 0.0;

float previousPitchError = 0.0;


// =====================================================
// WRITE TO REGISTER
// =====================================================

void writeRegister(byte reg, byte value)
{
  Wire.beginTransmission(LSM6DS3_ADDR);

  Wire.write(reg);

  Wire.write(value);

  Wire.endTransmission();
}


// =====================================================
// READ ONE BYTE
// =====================================================

byte readRegister(byte reg)
{
  Wire.beginTransmission(LSM6DS3_ADDR);

  Wire.write(reg);

  Wire.endTransmission(false);

  Wire.requestFrom(LSM6DS3_ADDR, 1);

  return Wire.read();
}


// =====================================================
// READ TWO BYTES
// =====================================================

int16_t read16(byte reg)
{
  Wire.beginTransmission(LSM6DS3_ADDR);

  Wire.write(reg);

  Wire.endTransmission(false);

  Wire.requestFrom(LSM6DS3_ADDR, 2)

More projects by josephkinuthia541