Cannot integrate PID in my Quadcopter? Is this the right way?

Projects Programming
1

I am working in my quadcopter. Right now I am just testing for take- off and Roll angle. I tried to integrate PID library in my code but it keeps generating 100 and my quadcopter goes blasting. Right now I am just giving 1,1,1 for kp,ki and kd. I know this is not right but like I said, I am just playing. Here is my Code. ..

#define CHNL_HIGH 1900
#define CHNL_LOW 1100

#define THROTTLE_CHNL 2
#define CW1_IN 9
#define CW2_IN 10
#define CW3_IN 11
#define CW4_IN 12
#define ARM_DEG 21
#define THROTTLE_FLAG 1
Servo servoCW1;
Servo servoCW2;
Servo servoCW3;
Servo servoCW4;

int servoCW1_DEG = 0;
int servoCW2_DEG = 0;
volatile uint8_t bUpdateFlagsShared;
volatile uint16_t unThrottleInShared;
volatile int throttleOffset;
uint32_t ulThrottleStart;

float ypr[3]; // yaw pitch roll

//Define Variables we'll be connecting to
double Input_roll,rollOffset,Setpoint_roll;

PID myPID(&Input_roll, &rollOffset, &Setpoint_roll,1,1,1, DIRECT);

// Set the FreeIMU object
FreeIMU my3IMU = FreeIMU();

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

delay(5);
my3IMU.init(); // the parameter enable or disable fast mode

//initialize the variables we're linked to

Setpoint_roll = -5.00;

//turn the PID on
myPID.SetMode(AUTOMATIC);
myPID.SetOutputLimits(-100.00, 100.00);

servoCW1.attach(CW1_IN);
servoCW2.attach(CW2_IN);

servoCW3.attach(CW3_IN);
servoCW4.attach(CW4_IN);

Serial.println("GET IT");
arm();
attachInterrupt(0, calcThrottle,CHANGE);

delay(5);
}

void loop()
{

static uint16_t unThrottleIn;
static uint8_t bUpdateFlags;

// check shared update flags to see if any channels have a new signal
if(bUpdateFlagsShared)
{
noInterrupts();

bUpdateFlags = bUpdateFlagsShared;

if(bUpdateFlags & THROTTLE_FLAG)
{
unThrottleIn = unThrottleInShared;
}

bUpdateFlagsShared = 0;

interrupts(); // we have local copies of the inputs, so now we can turn interrupts back on

}

if(bUpdateFlags & THROTTLE_FLAG)

{
throttleChange(unThrottleIn);

}
my3IMU.getYawPitchRoll(ypr);
Input_roll = ypr[2];
myPID.Compute();
rollChange(rollOffset);
bUpdateFlags = 0;
}

void rollChange(double rollOffset)
{
int valCW1 = servoCW1.readMicroseconds();
int valCW2 = servoCW2.readMicroseconds();
Serial.print("offset: ");
Serial.println(rollOffset);
valCW1 = valCW1 + rollOffset;
valCW2 = valCW2 - rollOffset;
Serial.print("valCW1: ");
Serial.println(valCW1);

Serial.print("valCW2: ");
Serial.println(valCW2);
Serial.println("");
Serial.println("");

servoCW1.writeMicroseconds((int)valCW1);
servoCW2.writeMicroseconds((int)valCW2);
}
void arm()
{

servoCW1.write(ARM_DEG);
servoCW2.write(ARM_DEG);

servoCW3.write(ARM_DEG);
servoCW4.write(ARM_DEG);

}

void throttleChange(int cha_val)
{

servoCW1.writeMicroseconds(cha_val);
servoCW2.writeMicroseconds(cha_val);

servoCW3.writeMicroseconds(cha_val);
servoCW4.writeMicroseconds(cha_val);

}

void calcThrottle()
{
if(digitalRead(THROTTLE_CHNL) == HIGH)
{
ulThrottleStart = micros();
}else
{

unThrottleInShared = (uint16_t)(micros() - ulThrottleStart);
bUpdateFlagsShared |= THROTTLE_FLAG;
}
}

I am getting an rollOffset from the PID controller. And action is taken in the rollchange() function. My setpoint is -5.00.

Problem: The PID outputs 100 most of the time. And, I don't think it is helping to reach my setpoint but it is really destroying it. So my question is am I really using the output from PID the right way.

2 
PID myPID(&Input_roll, &rollOffset, &Setpoint_roll,1,1,1, DIRECT);

1.0, 1.0, and 1.0 are not good initial values for kP, kI, and kD.

        int valCW1 = servoCW1.readMicroseconds();
        int valCW2 =  servoCW2.readMicroseconds();

Can't remember what you last wrote, huh? Not good.

        servoCW1.writeMicroseconds((int)valCW1);
        servoCW2.writeMicroseconds((int)valCW2);

I've never understood the need to cast an int to an int. Can you explain why you need to?

3

PaulS:

PID myPID(&Input_roll, &rollOffset, &Setpoint_roll,1,1,1, DIRECT);

1.0, 1.0, and 1.0 are not good initial values for kP, kI, and kD.

        int valCW1 = servoCW1.readMicroseconds();

int valCW2 =  servoCW2.readMicroseconds();



Can't remember what you last wrote, huh? Not good.

servoCW1.writeMicroseconds((int)valCW1);
        servoCW2.writeMicroseconds((int)valCW2);



I've never understood the need to cast an int to an int. Can you explain why you need to?

You are right about readMicroseconds()......
And In Docs for Servo: the value of the parameter in microseconds (int)
So, it says the writeMicroseconds param must be in Int

4

So, it says the writeMicroseconds param must be in Int

Yeah? So? What is the type of the argument?

A cast may be necessary of the type you have is not the same as that that a function expects, and the type can not be safely converted. For instance, the delay() function takes a value that is unsigned long. An int easily fits in an unsigned long, so no cast is needed.

If the function took an int, and you have an unsigned long, a cast is required. This puts the burden on you to validate that the cast will be successful.

You still need to comment on, or change, your kP, kI, and kD parameters.

5

PaulS:
A cast may be necessary of the type you have is not the same as that that a function expects, and the type can not be safely converted. For instance, the delay() function takes a value that is unsigned long. An int easily fits in an unsigned long, so no cast is needed.

Ahaaaaaa.. I see what ur saying ur right... And, yes For the PID constants...... I didn't know what to start off with soo just did 1,1,1 but at some point I will use the PID tuning library to get the dynamic constants...

6

I didn't know what to start off with soo just did 1,1,1

Do you understand what each of the parameters means, in applying a correction to get the actual value closer to the desired value?

If you do, you'll see that 1.0 might not be reasonable. Those values are a bit like trying to boil a teaspoon of water using a blast furnace.

7

PaulS:
If you do, you'll see that 1.0 might not be reasonable. Those values are a bit like trying to boil a teaspoon of water using a blast furnace.

Hmmm awesome Example. I thought I did understand but I didn't think it would have that much impact like your example..... So i will get into that then...