MY PID code for quadcopter not working

Hi,

I’m trying to create something like this:

But whenever i run the code, the pwm value for both motors stays the same (static at initial 1300 value)…

Pls help,

Thanks

PID_help.ino (2.23 KB)

Here is the code if you don’t want to download file:

#include <Wire.h>
#include <Servo.h>
#include <utility/imumaths.h>

Servo motor_r;
Servo motor_l;

int16_t Acc_rawX, Acc_rawY, Acc_rawZ,Gyr_rawX, Gyr_rawY, Gyr_rawZ;

float accel_angle;
float roll_angle;
float total_roll;

float elapsedTime, Time, prevTime;
int i;
float rad_to_deg = 180/3.14159265359;

float PID, pwm_l, pwm_r, error, previous_error;
float pid_p = 0;
float pid_i = 0;
float pid_d = 0;

double kp; = 3.55;
double ki; = 0.005;
double kd; = 2.05;

double thrust = 1300;
float desired_angle = 0;

void setup() {
pinMode(13, INPUT);
pinMode(12, INPUT);
pinMode(11, INPUT);

Wire.begin();
Wire.beginTransmission(0x68);
Wire.write(0x6B);
Wire.write(0);
Wire.endTransmission(true);
Serial.begin(9600);
motor_r.attach(6);
motor_l.attach(8);

Time = millis();

motor_r.writeMicroseconds(1000);
motor_l.writeMicroseconds(1000);
delay(7000);

}

void loop() {
prevTime = Time;
Time = millis();
elapsedTime = (Time - prevTime) / 1000;

Wire.beginTransmission(0x68);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(0x68,6,true);

Acc_rawX=Wire.read()<<8|Wire.read();
Acc_rawY=Wire.read()<<8|Wire.read();
Acc_rawZ=Wire.read()<<8|Wire.read();

accel_angle = atan(-1*(Acc_rawX/16384.0)/sqrt(pow((Acc_rawY/16384.0),2) + pow((Acc_rawZ/16384.0),2)))*rad_to_deg;

Wire.beginTransmission(0x68);
Wire.write(0x43);
Wire.endTransmission(false);
Wire.requestFrom(0x68,4,true);

Gyr_rawX=Wire.read()<<8|Wire.read();
Gyr_rawY=Wire.read()<<8|Wire.read();

roll_angle = Gyr_rawY/131.0;

total_roll = 0.98*(total_roll + roll_angleelapsedTime) + 0.02accel_angle;

error = total_roll - desired_angle;

pid_p = kp*error;

if(-3 <error <3){
pid_i = pid_i+(ki*error);
}

pid_d = kd*((error - previous_error)/elapsedTime);

PID = pid_p + pid_i + pid_d;

if(PID < -1000){
PID=-1000;
}

if(PID > 1000){
PID=1000;
}

pwm_l = thrust + PID;
pwm_r = thrust - PID;

if(pwm_r < 1000){
pwm_r = 1000;
}

if(pwm_r > 2000){
pwm_r = 2000;
}

if(pwm_l < 1000){
pwm_l = 1000;
}

if(pwm_l > 2000){
pwm_l = 2000;
}

motor_r.writeMicroseconds(pwm_r);
motor_l.writeMicroseconds(pwm_l);

previous_error = error;

Serial.println(pwm_r);

}

And here is the code the way it was supposed to be posted (auto-formattesd and in code tags):

#include <Wire.h>
#include <Servo.h>
#include <utility/imumaths.h>

Servo motor_r;
Servo motor_l;

int16_t Acc_rawX, Acc_rawY, Acc_rawZ, Gyr_rawX, Gyr_rawY, Gyr_rawZ;

float accel_angle;
float roll_angle;
float total_roll;

float elapsedTime, Time, prevTime;
int i;
float rad_to_deg = 180 / 3.14159265359;

float PID, pwm_l, pwm_r, error, previous_error;
float pid_p = 0;
float pid_i = 0;
float pid_d = 0;

double kp; = 3.55;
double ki; = 0.005;
double kd; = 2.05;

double thrust = 1300;
float desired_angle = 0;


void setup() {
  pinMode(13, INPUT);
  pinMode(12, INPUT);
  pinMode(11, INPUT);

  Wire.begin();
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(9600);
  motor_r.attach(6);
  motor_l.attach(8);

  Time = millis();

  motor_r.writeMicroseconds(1000);
  motor_l.writeMicroseconds(1000);
  delay(7000);

}

void loop() {
  prevTime = Time;
  Time = millis();
  elapsedTime = (Time - prevTime) / 1000;

  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true);

  Acc_rawX = Wire.read() << 8 | Wire.read();
  Acc_rawY = Wire.read() << 8 | Wire.read();
  Acc_rawZ = Wire.read() << 8 | Wire.read();

  accel_angle = atan(-1 * (Acc_rawX / 16384.0) / sqrt(pow((Acc_rawY / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;

  Wire.beginTransmission(0x68);
  Wire.write(0x43);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 4, true);
   
  Gyr_rawX = Wire.read() << 8 | Wire.read();
  Gyr_rawY = Wire.read() << 8 | Wire.read();

  roll_angle = Gyr_rawY / 131.0;

  total_roll = 0.98 * (total_roll + roll_angle * elapsedTime) + 0.02 * accel_angle;

  error = total_roll - desired_angle;

  pid_p = kp * error;

  if (-3 < error < 3) {
      pid_i = pid_i + (ki * error);  
  }

  pid_d = kd * ((error - previous_error) / elapsedTime);

  PID = pid_p + pid_i + pid_d;

  if (PID < -1000) {
      PID = -1000;
  }

  if (PID > 1000) {
      PID = 1000;
  }

  pwm_l = thrust + PID;
  pwm_r = thrust - PID;

  if (pwm_r < 1000) {
      pwm_r = 1000;
  }

  if (pwm_r > 2000) {
      pwm_r = 2000;
  }

  if (pwm_l < 1000) {
      pwm_l = 1000;
  }

  if (pwm_l > 2000) {
      pwm_l = 2000;
  }

  motor_r.writeMicroseconds(pwm_r);
  motor_l.writeMicroseconds(pwm_l);

  previous_error = error;

  Serial.println(pwm_r);
}
  if (-3 < error < 3) {

Common mistake. This is valid syntax but does NOT mean   if (-3 < error && error < 3) {

You can save a lot of code by using the constrain() function:

  PID = constrain(pid_p + pid_i + pid_d, -1000, 1000);
  pwm_l = constrain(thrust + PID, 1000, 2000);
  pwm_r = constrain(thrust - PID, 1000, 2000);