Issues regarding pre-programmed Arduino micro Quadcopter flight

Started by sainatarajan, December 22, 2018, 02:05:30 PM

Previous topic - Next topic

0 Members and 1 Guest are viewing this topic.

Print
Go Down Pages1
User actions

sainatarajan

December 22, 2018, 02:05:30 PM
I'm having little trouble in order to control the Arduino Micro Quadcopter that we are building. This quadcopter is built for a programmed flight sequence. It has an MPU6050 as the IMU, 4 coreless motors, 4 MOSFETs and an Arduino Nano as the chip. We use PWM to control the speeds of the motors.

We are building this drone to perform a sequence of movements. Let's say : Going up for sometime, then going left, then going right, and atlast landing. I'm using a complementary filter in order to fuse the values of accelerometer angles and gyro angles.

The code for it is :


void recordRegisters() {
Wire.beginTransmission(0b1101000);
    Wire.write(0x3B);
    Wire.endTransmission();
    Wire.requestFrom(0b1101000, 14); 
   
    while (Wire.available() < 14);

    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 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 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 updateMotorSpeeds() {
// Assume that the throttle is always constant for now.
front_left_motor_speed= (throttle - pitch - roll - yaw);
front_right_motor_speed= (throttle + pitch - roll + yaw);
back_left_motor_speed= (throttle - pitch + roll + yaw);
back_right_motor_speed= (throttle + pitch + roll - yaw);

}

void goUp() {
Serial.println("Going up......");
for(; throttle <= 200; throttle++) {
calcComplementaryFilter();
                computePID();
controlMotorAlgorithm(); // assigns motor values
writePWMToAllMotors(); // does analogWrite() based on values set before
delay(25);
}
}


With due credits to the author of the above code, I'm able to get the values of roll, pitch and yaw. I'm using the updateMotorSpeeds() function to assign the proper values for the 4 motors. The problems I'm are facing are :

1. Are the motor mixing values correct ? (from the updateMotorSpeeds() function)
2. I'm not sure whether to use a PID controller for stabilization of the quadcopter?
3. What should be the motor values for the sequence of movements that I had mentioned above?
4. Also I am calculating complementary filter and computing PID values every time I increase my throttle by 1 in the goUp() function. Am I right in doing so?

Dharmik

#1
December 22, 2018, 05:23:41 PM
look interesting project. if you have any video after flashing this code then it might help further. have you assigned pinout to the motors? also

controlMotorAlgorithm(); // assigns motor values
writePWMToAllMotors()

these two functions code seems missing above.

saikat

#2
December 22, 2018, 09:02:03 PM
Firstly post complete code ....
Eg : how do you assign variable throttle , pitch , yaw  roll etc...

Secondly to get proper motor pwm value , put standard transmitter receiver - fly the programmed
sequence and record each motor pwm values using avr input capture interrupts
Then you can use those values to programme your processor

sainatarajan

#3
December 24, 2018, 02:33:15 PM


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
Go Up Pages1
User actions