Hello!
i've got a problem with my quadcopter project.
As you can see in the first picture (where the green is the ProcessValue from MPU6050 and the blue is the outsignal from the PID) the PIDs outsignal is fucking up.
And on the other picture the ProcessValue is generated by the code right under "valueread(pitch, roll, height);" (in the loop function) and the valueread function disabled by change it to valueread(pitchu, roll, height);" it doesn't fuck up.
Any clues why??
I've taken the MPU6050 code from Jeff Rowberg's example code https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino, so I'm not 100% sure how that works.
#include <Servo.h>
#include <PID_v1.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_REALACCEL
//Gyro stuff
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container
VectorInt16 aa; // [x, y, z] accel sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
// === INTERRUPT DETECTION ROUTINE ===
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
Servo motorfront;
Servo motorback;
Servo motorright;
Servo motorleft;
int braap;
int first=0;
double pitch = 0;
double roll;
double pospitchkvot;
double negpitchkvot;
double firstnegpitchkvot;
double posrollkvot;
double negrollkvot;
double pitchkvot;
double notpitchkvot;
double rollkvot;
double notrollkvot;
double height;
double heightfilter;
double heightout;
double heightoutf;
double SPheight=4280;
double mfront;
double mback;
double mright;
double mleft;
PID pospitchPID(&pitch, &pospitchkvot, 0, 1, 0, 0, REVERSE);
PID negpitchPID(&pitch, &firstnegpitchkvot, 0, 1, 0, 0, DIRECT);
PID posrollPID(&roll, &posrollkvot, 0, 0.002, 0, 0, DIRECT);
PID negrollPID(&roll, &negrollkvot, 0, 0.002, 0, 0, REVERSE);
PID heightPID(&heightfilter, &heightout, &SPheight, 0.002, 0, 0, DIRECT);
void setup() {
//Start I2C and Serial
Wire.begin();
Serial.begin(115200);
while(!Serial);
// initialize
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
error();
}
//Set up all motors
motorfront.attach(3);
motorback.attach(5);
motorright.attach(6);
motorleft.attach(9);
delay(2000);
//Set output limit so it'll calculate right
pospitchPID.SetOutputLimits(0,100 );
negpitchPID.SetOutputLimits(0,1);
posrollPID.SetOutputLimits(1,2);
negrollPID.SetOutputLimits(0,1);
heightPID.SetOutputLimits(60,170);
//Set LED-pin as output
pinMode(13, OUTPUT);
//Call motor begin function to initiate the motors
Serial.println("MOTORS STARTING!!");
Motor_begin();
//Activate PID regulators
heightPID.SetMode(AUTOMATIC);
pospitchPID.SetMode(AUTOMATIC);
negpitchPID.SetMode(AUTOMATIC);
posrollPID.SetMode(AUTOMATIC);
negrollPID.SetMode(AUTOMATIC);
}
void loop() {
//double pitchu;
valueread(pitch, roll, height);
/*
if (first == 0 && pitch < 100){
pitch ++;
}
else if(first == 0 && pitch == 100){
first = 1;
}
else if (first == 1 && pitch > 0){
pitch --;
}
else if(first == 1 && pitch == 0){
first = 0;
}
/*
braap = braap++;
//Compute PID regulators
heightPID.Compute();
pospitchPID.Compute();
negpitchPID.Compute();
posrollPID.Compute();
negrollPID.Compute();
//Flip negative quota
negpitchkvot = (firstnegpitchkvot - 0) * (0 - 1) / (1 - 0) + 1;
//Calculate quota
pitchkvot = pospitchkvot * negpitchkvot;
rollkvot = posrollkvot * negrollkvot;
notpitchkvot = (1 - pitchkvot) + 1;
notrollkvot = (1 - rollkvot) + 1;
//saftey variable
heightoutf = heightout;
//test
heightoutf = 80;
//Multiply quota and height speed
mfront = pitchkvot * heightoutf;
mback = notpitchkvot * heightoutf;
mright = rollkvot * heightoutf;
mleft = notrollkvot * heightoutf;
motorfront.write(mfront);
motorback.write(mback);
Serial.print(braap);
Serial.print(";");
Serial.print(pospitchkvot);
Serial.print(";");
Serial.print(negpitchkvot);
Serial.print(";");
Serial.println(pitch);
}
void Motor_begin() {
//Show Motors are starting by blinking LED
digitalWrite(13, HIGH);
//Get motors to go to on mode
motorfront.write(60);
motorback.write(60);
motorright.write(60);
motorleft.write(60);
delay(500);
//Blink
digitalWrite(13, LOW);
delay(500);
//Blink
digitalWrite(13, HIGH);
//Get motors to go to drive mode
motorfront.write(70);
motorback.write(70);
motorright.write(70);
motorleft.write(70);
delay(500);
//Blink
digitalWrite(13, LOW);
delay(500);
//Blink
digitalWrite(13, HIGH);
//Test motors
for(int x=60; x<=80; x++)
{
motorfront.write(x);
motorback.write(x);
motorright.write(x);
motorleft.write(x);
delay(100);
}
//Blink
digitalWrite(13, LOW);
//Go back to 0 speed
motorfront.write(60);
motorback.write(60);
motorright.write(60);
motorleft.write(60);
delay(500);
//Blink
digitalWrite(13, HIGH);
delay(500);
//End of test, shut LED off
digitalWrite(13, LOW);
}
double valueread(double & x,double & z,double & y) {
// if programming failed, don't try to do anything
if (!dmpReady) {error();}
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
//Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL
// display Euler angles in degrees
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
x = ypr[1] * 180/M_PI;
z = ypr[2] * 180/M_PI;
#endif
#ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
y = aaReal.z;
#endif
}
}
void error() {
while(0==0) {
digitalWrite(13, HIGH);
Serial.println("ERROR!!!!");
}
}
