Hey,
I am trying to calculate PID for a quadcopter (roll and pitch). Data input is from MPU6050.
When I put the .Compute(); into setup, the compiler doesn´t show any errors.
If I put it into loop, the compiler shows these errors:
PID:92: error: 'PIDroll' was not declared in this scope
PID:93: error: 'PIDpitch' was not declared in this scope
What am I doing wrong? Thanks a lot.
PS. I have a bit of servo code that´s not used yet.
/*================================================================================
INIT
================================================================================*/
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Include Libraries
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <PID_v1.h>
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Define Input Pins
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
#define voltmeter A0
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Define Output Pins
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
#define x1_pin 6
#define i1_pin 9
#define x2_pin 10
#define i2_pin 5
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Define Constants
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
double pidrp=3.55;
double pidri=0.005;
double pidrd=2.05;
double pidpp=3.55;
double pidpi=0.005;
double pidpd=2.05;
double error_pd = 0;
double error_rd = 0;
double pid_ro = 0;
double pid_po = 0;
double pitch = 0;
double roll = 0;
double yaw = 0;
//MPU6050 constants
unsigned long timer = 0;
float timeStep = 0.01;
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Create Objects
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
Servo X1; //Create servos
Servo I1;
Servo X2;
Servo I2;
MPU6050 mpu;
//Create MPU6050 object
/*================================================================================
SETUP
================================================================================*/
void setup()
{
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
MPU6050 setup
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
Serial.begin(115200);
while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
{
Serial.println("MPU6050 not responding");
delay(500);
}
mpu.calibrateGyro();
mpu.setThreshold(3);
/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
PID setup
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
PID PIDroll(&roll,&pid_ro,&error_rd, pidrp, pidri, pidrd,P_ON_M, DIRECT); //Create roll and pitch PID
PID PIDpitch(&pitch,&pid_po,&error_pd, pidpp, pidpi, pidpd,P_ON_M, DIRECT); //Create roll and pitch PID
PIDroll.SetMode(AUTOMATIC);
PIDpitch.SetMode(AUTOMATIC);
PIDroll.Compute();
PIDpitch.Compute();
}
void loop()
{
timer = millis();
Vector norm = mpu.readNormalizeGyro();
pitch = pitch + norm.YAxis * timeStep;
roll = roll + norm.XAxis * timeStep;
yaw = yaw + norm.ZAxis * timeStep;
Serial.print(roll);
Serial.print("|--------|");
Serial.print(pitch);
Serial.print("|--------|");
Serial.print(pid_ro);
Serial.print("|--------|");
Serial.print(pid_po);
Serial.print("|--------|");
Serial.println(yaw);
PIDroll.Compute();
PIDpitch.Compute();
}