void mdhdrone::initPIDController() {
rollPID.begin(); // initialize the PID instance
pitchPID.begin(); // initialize the PID instance
yawPID.begin(); // initialize the PID instance
rollPID.setpoint(0); // The "goal" the PID controller tries to "reach"
pitchPID.setpoint(0); // The "goal" the PID controller tries to "reach"
yawPID.setpoint(0); // The "goal" the PID controller tries to "reach"
rollPID.tune(1, 0.1, 0.5);
pitchPID.tune(1, 0.1, 0.5);
yawPID.tune(1, 0.1, 0.5);
rollPID.limit(0, 255);
pitchPID.limit(0, 255);
yawPID.limit(0, 255);
}
void mdhdrone::setupMPU() {
Wire.beginTransmission(0b1101000);
Wire.write(0x6B);
Wire.write(0b00000000);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1B);
Wire.write(0x08);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1C);
Wire.write(0x10);
Wire.endTransmission();
Wire.beginTransmission(0b1101000);
Wire.write(0x1A);
Wire.write(0x03);
Wire.endTransmission();
}
void mdhdrone::recordRegisters() {
Wire.beginTransmission(0b1101000);
Wire.write(0x3B);
Wire.endTransmission();
Wire.requestFrom(0b1101000, 14);
//Serial.println("Pre while loop");
while (Wire.available() < 14);
//Serial.println("Post while loop");
accelX = Wire.read() << 8 | Wire.read();
accelY = Wire.read() << 8 | Wire.read();
accelZ = Wire.read() << 8 | Wire.read();
temperature = Wire.read() << 8 | Wire.read();
gyroX = Wire.read() << 8 | Wire.read();
gyroY = Wire.read() << 8 | Wire.read();
gyroZ = Wire.read() << 8 | Wire.read();
if (cal_int == 2000)
{
gyroX -= gyro_x_cal;
gyroY -= gyro_y_cal;
gyroZ -= gyro_z_cal;
}
}
void mdhdrone::controlMotorAlgorithm() {
front_left_motor_speed= (int)(throttle - pitchPIDOutput - rollPIDOutput + yawPIDOutput);
front_right_motor_speed= (int)(throttle - pitchPIDOutput + rollPIDOutput - yawPIDOutput);
back_left_motor_speed= (int)(throttle + pitchPIDOutput - rollPIDOutput - yawPIDOutput);
back_right_motor_speed= (int)(throttle + pitchPIDOutput + rollPIDOutput + yawPIDOutput);
/*I am mapping it because sometimes the values goes out of range*/
front_left_motor_speed= map(front_left_motor_speed, -15, 280, 0, 255);
front_right_motor_speed= map(front_right_motor_speed, -15, 280, 0, 255);
back_left_motor_speed= map(back_left_motor_speed, -15, 280, 0, 255);
back_right_motor_speed= map(back_right_motor_speed, -15, 280, 0, 255);
}
void mdhdrone::calcComplementaryFilter() {
recordRegisters();
gyroX = gyroX / 65.5;
gyroY = gyroY / 65.5;
gyroZ = gyroZ / 65.5;
accelX = accelX / 4096.0;
accelY = accelY / 4096.0;
accelZ = accelZ / 4096.0;
double dt = (double)(micros() - timer) / 1000000;
timer = micros();
rollAngle = atan2(accelY, accelZ) * 180 / PI;
pitchAngle = atan2(accelX, sqrt(accelY * accelY + accelZ * accelZ)) * 180 / PI;
roll = 0.99 * (roll + gyroX * dt) + 0.01 * rollAngle;
pitch = 0.99 * (pitch + gyroY * dt) + 0.01 * pitchAngle;
yaw = gyroZ;
}
void mdhdrone::calibrateGyroscope() {
for (cal_int = 1; cal_int <= 2000; cal_int++)
{
recordRegisters();
gyro_x_cal += gyroX;
gyro_y_cal += gyroY;
gyro_z_cal += gyroZ;
}
gyro_x_cal /= 2000;
gyro_y_cal /= 2000;
gyro_z_cal /= 2000;
}
void mdhdrone::startTimer() {
timer= micros();
}
void mdhdrone::goUp() {
Serial.println("Going up......");
for(; throttle <= 255; throttle++) {
mdhdrone::calcComplementaryFilter();
rollPIDOutput= rollPID.compute(roll);
pitchPIDOutput= pitchPID.compute(pitch);
yawPIDOutput= yawPID.compute(yaw);
mdhdrone::controlMotorAlgorithm();
mdhdrone::printMotorValues();
mdhdrone::writePWMToAllMotors();
delay(25);
}
}
void mdhdrone::comeDown() {
Serial.println("Coming down......");
for(; throttle > 1; throttle--) {
mdhdrone::calcComplementaryFilter();
rollPIDOutput= rollPID.compute(roll);
pitchPIDOutput= pitchPID.compute(pitch);
yawPIDOutput= yawPID.compute(yaw);
mdhdrone::controlMotorAlgorithm();
mdhdrone::printMotorValues();
mdhdrone::writePWMToAllMotors();
delay(50);
}
}
void mdhdrone::writePWMToAllMotors() {
analogWrite(front_left_pin, front_left_motor_speed);
analogWrite(front_right_pin, front_right_motor_speed);
analogWrite(back_left_pin, back_left_motor_speed);
analogWrite(back_right_pin, back_right_motor_speed);
}
Thanks for your suggestions..
I have posted the entire code without the getter and setter functions.
The mini-drone tries to lift but falls from my hand when we were testing it out in the ground. I can definitely feel a lot of thrust from the motors and it lifts up for a second and then traverses an inverted U path to the ground. What can be the reason for this?
Print
