I am doing a Quadcopter project and i need help!, i have done most of the part, reading sensors, controlling motors etc. I am totally stuck at the stabilization algorithm part. I tried PID but no go as i dont know how to set the output to servo speed. I tried my own code, that is given below
bool errState(int type) // returns true if the copter is not level
{
if (type == PITCHPVE) // Positive pitch
{
if (getDeg(ypr[1]) > getDeg(statYPR[1]) + 3)
return true;
else
return false;
}
if (type == PITCHNVE) // Negative Pitch
{
if (getDeg(ypr[1]) < getDeg(statYPR[1]) - 3)
return true;
else
return false;
}
if (type == ROLLPVE) // +ve Roll
{
if (getDeg(ypr[2]) < getDeg(statYPR[2]) - 3)
return true;
else
return false;
}
if (type == ROLLNVE)// -ve Roll
{
if (getDeg(ypr[2]) < getDeg(statYPR[2]) - 3)
return true;
else
return false;
}
}
//comSpd holds the last common value written to all the motors using writeMicroseconds()
//escSpeed holds current speeds of each motor
// getIMUData() sets yaw, pitch and roll readings from imu in ypr[];
void autoLevel(int code, float error)
{
switch (code)
{
case PITCH:
{
// runs for +ve pitch
if (error > 0)
{
while (errState(PITCHPVE))
{
if(escSpeed[0] > comSpd-100 && escSpeed[2] < comSpd + 100)
{
esc[0].writeMicroseconds(--escSpeed[0]);
esc[2].writeMicroseconds(++escSpeed[2]);
}
getIMUData(false);
}
}
if (error < 0)
{
while (errState(PITCHNVE))
{
//getIMUData(false);
if(escSpeed[2] > comSpd-100 && escSpeed[0] < comSpd + 100)
{
esc[2].writeMicroseconds(--escSpeed[2]);
esc[0].writeMicroseconds(++escSpeed[0]);
}
getIMUData(false);
}
}
break;
}
case ROLL:
{
if (error > 0)
{
while (errState(ROLLPVE))
{
if(escSpeed[1] > comSpd-100 && escSpeed[3] < comSpd + 100)
{
esc[1].writeMicroseconds(--escSpeed[1]);
esc[3].writeMicroseconds(++escSpeed[3]);
}
getIMUData(false);
}
}
if (error < 0)
{
while (errState(ROLLNVE))
{
if(escSpeed[3] > comSpd-100 && escSpeed[1] < comSpd + 100)
{
esc[3].writeMicroseconds(--escSpeed[3]);
esc[1].writeMicroseconds(++escSpeed[1]);
}
getIMUData(false);
}
}
break;
}
case YAW:
{
break;
}
default:
return;
}
return;
}
I didnt code for YAW yet. thought to finish the PITCH and ROLL first.
The quad copter oscillates violently when it lifts off, it pitches, it rolls simultaneously in either direction, but theoretically the code should be correct right?
I need some help urgently.
The components
Arduino Mega 2560
Sensors:
MPU6050
HMC5883L
Connectivity:
ESP8266
I have not used any radio controller or KK boards.
the fan speeds are sent to arduino via the wifi module as a HTTP request with an android app and it sets the speed of all the motors accordingly.