This is the code to implement a quadcopter. PID controller tuning is necessary.
Dependencies: PwmIn mbed MPU6050IMU
Diff: main.cpp
- Revision:
- 0:6038ca525e44
- Child:
- 1:2021161b8fd2
diff -r 000000000000 -r 6038ca525e44 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 08 00:06:09 2019 +0000 @@ -0,0 +1,439 @@ + +#include "mbed.h" +#include "rtos.h" +#include "MPU6050.h" +#include "PwmIn.h" + +//Functions + void PID1(void const *argument); //PID Control + void PeriodicInterruptISR(void); //Periodic Timer Interrupt Serive Routine + //Processes and Threads + osThreadId PiControlId; // Thread ID + osThreadDef(PID1, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority + + void receiver_data(); +//------------------------------------------------------------------------------------------------------------- + +//System Definitions: + #define PWM_Period 20000 //in micro-seconds + #define ESC_ARM 1000 // this set the PWM signal to 50% duty cycle for a signal period of 2000 micro seconds + #define dt 0.005 // periodic interrupt each 20 ms... 50 Hz refresh rate of quadcopter motor speeds + + + // Controller Gains, and boundaries + //Roll + #define uRollMax 300 + //Pitch + #define uPitchMax 300 + // Throttle + #define MaxThrottle 1600 // 1600 micro seconds + #define MinThrottle 1100 // 1100 micro seconds + // Yaw + #define uYawMax 200 + +//------------------------------------------------------------------------------------------------------------- + +//Port Configuration: + // These are PWM In's that define the system's desired points and other control commands + // Note: the ports of the following PWM input pins have to be interrupt enabled (Port A and D are the only + // interrupt enabled ports for KL25z). + PwmIn channel1(PTA5); // Roll + PwmIn channel2(PTA13); // {itch + PwmIn channel3(PTA16); // Throttle + PwmIn channel4(PTA17); // Yaw + + //PWM Outputs + // Configuration of motors: + // Motor4 Motor1 + // . ^ . + // . | . + // . | . + // . . + // Motor3 Motor2 + PwmOut PWM_motor1(PTB3); //PWM Motor1 + PwmOut PWM_motor3(PTB1); //PWM Motor3 + PwmOut PWM_motor2(PTB2); //PWM Motor2 + PwmOut PWM_motor4(PTB0); //PWM Motor4 + + // MPU6050 gyroscope and accelerometer + // Note the library MPU6050.h must be modified to match the chosen I2C pins + // defined in the hardware design + MPU6050 mpu6050; + + //Timer Interrupts + Timer t; + Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt + Serial pc(USBTX, USBRX); // tx, rx +//------------------------------------------------------------------------------------------------------------- + +//Gloabal Variables Definitions: + + // PID + //Throttle + float desiredThrottle; + //Roll + float eRoll, desiredRoll, desRoll, actualRoll, eDerRoll, elastRoll, eIntRoll, uRoll; + float KpRoll, KdRoll, KiRoll; + //Pitch + float ePitch, desiredPitch, desPitch, actualPitch, eDerPitch, elastPitch, eIntPitch, uPitch; + float KpPitch, KdPitch, KiPitch; + + //Yaw + float eYaw, desiredYaw, desYaw, actualYaw, eDerYaw, elastYaw, eIntYaw, uYaw; + float KpYaw, KdYaw, KiYaw; + + // control speeds of the motors + float v1, v2, v3, v4; + + // Gyro Stuff + float sum = 0; + uint32_t sumCount = 0; + + + float RollCalibration, PitchCalibration, YawCalibration; + float actRoll, actPitch, actYaw; + +//------------------------------------------------------------------------------------------------------------- + +int main() { + + // System setup + // this sets up the periodic interrupt, the UART baud rate communication with the PC, as well as + // setting up the PWM signal period of each motor. + pc.baud(9600); // Set max uart baud rate + pc.printf("We have good serial communication! :D\n"); + + //Initializing Threads for interrupts + PiControlId = osThreadCreate(osThread(PID1), NULL); //Create Thread for PID1 + PeriodicInt.attach(&PeriodicInterruptISR, dt); //Periodic timer interrupt every dt seconds as defined above + + //PWM Period + PWM_motor1.period_us(PWM_Period); // This sets the PWM period to [PWM_Period] + PWM_motor2.period_us(PWM_Period); // + PWM_motor3.period_us(PWM_Period); // + PWM_motor4.period_us(PWM_Period); // + + // this sets all motor PWM signals to 50% to arm the esc's for operation + PWM_motor1.pulsewidth_us(2000); //Set the Pulsewidth output + PWM_motor2.pulsewidth_us(2000); //Set the Pulsewidth output + PWM_motor3.pulsewidth_us(2000); //Set the Pulsewidth output + PWM_motor4.pulsewidth_us(2000); //Set the Pulsewidth output + + wait(10); + PWM_motor1.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output + PWM_motor2.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output + PWM_motor3.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output + PWM_motor4.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output + + KpRoll = .55; + KdRoll = 0; + KiRoll = 0; + + KpPitch = KpRoll; + KdPitch = KdRoll; + KiPitch = KiRoll; + + //Set up I2C + i2c.frequency(400000); // use fast (400 kHz) I2C + t.start(); + // Read the WHO_AM_I register, this is a good test of communication + uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 + pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); + + if (whoami == 0x68){ // WHO_AM_I should always be 0x68 + + pc.printf("MPU6050 is online..."); + wait(1); + mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values + pc.printf("x-axis self test: acceleration trim within : "); + pc.printf("%f \n\r", SelfTest[0]); + pc.printf("y-axis self test: acceleration trim within : "); + pc.printf("%f \n\r", SelfTest[1]); + pc.printf("z-axis self test: acceleration trim within : "); + pc.printf("%f \n\r", SelfTest[2]); + pc.printf("x-axis self test: gyration trim within : "); + pc.printf("%f \n\r", SelfTest[3]); + pc.printf("y-axis self test: gyration trim within : "); + pc.printf("%f \n\r", SelfTest[4]); + pc.printf("z-axis self test: gyration trim within : "); + pc.printf("%f \n\r", SelfTest[5]); + + wait(1); + + if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f){ + mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration + mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers + mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature + + wait(2); + } + else { + pc.printf("Device did not the pass self-test!\n\r"); + } + } + + else{ + pc.printf("Could not connect to MPU6050: \n\r"); + pc.printf("%#x \n", whoami); + while(1) ; // Loop forever if communication doesn't happen + } + + wait(10); // wait 10 seconds to make sure esc's are armed and ready to go + + // counter for mpu calibration + int counter = 0; + //Loop Forever + while (1) { + + + // If data ready bit set, all data registers have new data + if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt + mpu6050.readAccelData(accelCount); // Read the x/y/z adc values + mpu6050.getAres(); + + // Now we'll calculate the accleration value into actual g's + ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set + ay = (float)accelCount[1]*aRes - accelBias[1]; + az = (float)accelCount[2]*aRes - accelBias[2]; + + mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values + mpu6050.getGres(); + + // Calculate the gyro value into actual degrees per second + gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set + gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; + gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; + + tempCount = mpu6050.readTempData(); // Read the x/y/z adc values + temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade + } + + Now = t.read_us(); + deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update + lastUpdate = Now; + + sum += deltat; + sumCount++; + + if(lastUpdate - firstUpdate > 10000000.0f) { + beta = 0.04; // decrease filter gain after stabilized + zeta = 0.015; // increasey bias drift gain after stabilized + } + + // Pass gyro rate as rad/s + mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); + + + // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. + // In this coordinate system, the positive z-axis is down toward Earth. + // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise. + // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. + // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. + // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. + // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be + // applied in the correct order which for this configuration is yaw, pitch, and then roll. + // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. + yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]); + pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); + roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]); + actPitch = pitch * 180.0f / PI; + actYaw = yaw * 180.0f / PI; + actRoll = roll * 180.0f / PI; + + if (counter < 800){ + counter++; + RollCalibration = actRoll; + PitchCalibration= actPitch; + YawCalibration = actYaw; + } + else{ + + actRoll = actRoll - RollCalibration; + actualRoll = actPitch - PitchCalibration; + actualYaw = actYaw - YawCalibration; + + if(actRoll > 180){ + actualPitch=-(actRoll-360); + } + if(actRoll < -180){ + actualPitch=-(actRoll+360); + } + if ((actRoll > -180) && actRoll < 180) { + actualPitch=-actRoll; + } + + + // Measuring receiver values + desRoll = 1000000 * channel1.pulsewidth(); // desired point for roll + desPitch = 1000000 * channel2.pulsewidth(); // desired point for pitch + desiredThrottle = 1000000 * channel3.pulsewidth(); // desired point for throttle + desYaw = 1000000 * channel4.pulsewidth(); // desired point for yaw + + // The desired roll, pitch and yaw has a range between 1000 to 2000 microseconds + // we will map these values to limit the desired input between -45deg to +45deg for roll and pitch + // As for yaw, the desired input will range between -180 to 180 deg + // + // The equaion below maps the PWM value to a desired deg value, then it is shifter by some amount of degree so that + // the 1000 us is equivalent to -45deg, and the 2000 us point is equivalent to 45deg. + // The same logic is used for the Yaw equation + desiredRoll = (90 * (abs(desRoll -1000))/1000) - 45; + desiredPitch= (90 * (abs(desPitch-1000))/1000) - 45; + desiredYaw = (360* (abs(desYaw -1000))/1000) - 180; + } + // pc.printf("%f %f %f\n\r", actualPitch, desiredPitch, ePitch); + // pc.printf("%f %f %f %f\n\r ", uRoll, uPitch, eRoll, ePitch); + + pc.printf("%f %f %f %f\n\r", v1, v2, v3, v4); + // pc.printf("%f %f %f %f %f %f \n\r\n\r\n\r", actualRoll, desiredRoll, actualPitch, desiredPitch, actualYaw, eYaw ); + /* + pc.printf("%f \n\r", v1); + pc.printf("%f \n\r", v2); + pc.printf("%f \n\r", v3); + pc.printf("%f \n\r \n\r", v4); + + pc.printf("error in roll %f \n\r \n\r", eRoll); + pc.printf("error in pitch %f \n\r \n\r", ePitch); + pc.printf("error in yaw %f \n\r \n\r", eYaw);*/ + /* + pc.printf("Pulse width Roll value %f \n\r",desRoll); + pc.printf("Pulse width Pitch value %f \n\r",desPitch); + pc.printf("Pulse width Throttle value %f \n\r",desiredThrottle); + pc.printf("Pulse width Yaw value %f \n\r",desYaw); + */ + /* + pc.printf("Desired Roll: %f \n\r", desiredRoll); + pc.printf("Desired Pitch: %f \n\r", desiredPitch); + pc.printf("Desired Throttle: %f \n\r", desiredThrottle); + pc.printf("Desired Yaw: %f \n\r", desiredYaw); + */ + myled= !myled; + count = t.read_ms(); + sum = 0; + sumCount = 0; + + //wait(.5); + + } + + + +} + + + +// Updated on: Sept 19 2019 +// Koceila Cherfouh +// Deescription +// periodic time interrupt that controls the speed of the motor based on the gyro and accelerometer readings using a PID controller. +// +// Update and whats left to do: +// make sure all variables are declared. define max values, and gain values for each PID controllers. +// define the motor speed assignment based on the system's kinematic model +// test and tune the controller gain + +// ******** Periodic Thread ******** +void PID1(void const *argument){ + + while(true){ + osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. + + if (desiredThrottle > 1050){ // 1050 micro seconds will serve as the activation throttle + // 1- PID roll + eRoll= desiredRoll - actualRoll; + eDerRoll = eRoll - elastRoll; // derivative of error + uRoll = (KpRoll * eRoll) + (KdRoll * eDerRoll) + (KiRoll * eIntRoll); // computing the pitch control input + // this part of the code limits the integrator windeup. an effect that takes place when + if (uRoll > uRollMax){ + uRoll = uRollMax; + } + if (uRoll < -uRollMax){ + uRoll = -uRollMax; + } + else { + eIntRoll += eRoll; // Computing the integral sum + } + elastRoll = eRoll; + //------------------------------------------------------------------------------------------------------------- + // 2- PID pitch + ePitch = desiredPitch - actualPitch; // error + eDerPitch = ePitch - elastPitch ; // derivative of error + uPitch = (KpPitch * ePitch) + (KdPitch * eDerPitch) + (KiPitch * eIntPitch); // computing the pitch control input + // this part of the code limits the integrator windeup. an effect that takes place when + if (uPitch > uPitchMax){ + uPitch = uPitchMax; + } + if (uPitch < -uPitchMax){ + uPitch = -uPitchMax; + } + else { + eIntPitch += ePitch; // Computing the integral sum + } + elastPitch = ePitch; + //------------------------------------------------------------------------------------------------------------- + // 4- PID yaw + /* eYaw= desiredYaw - actualYaw; + eDerYaw = eYaw - elastYaw ; // derivative of error + uYaw = (KpYaw * eYaw) + (KdYaw * eDerYaw) + (KiYaw * eIntYaw); // computing the pitch control input + // this part of the code limits the integrator windeup. an effect that takes place when + if (uYaw > uYawMax){ + uYaw = uYawMax; + } + else { + eIntYaw += eYaw; // Computing the integral sum + } + elastYaw = eYaw; */ + //------------------------------------------------------------------------------------------------------------- + // Now that all pid control inputs are computed. send the PWM control input using the kinematic model to the ESC's + v1= desiredThrottle - uPitch - uRoll - uYaw; //Calculate the pulse for esc 1 (front-right - CCW) + v2= desiredThrottle + uPitch - uRoll + uYaw; //Calculate the pulse for esc 2 (rear-right - CW) + v3= desiredThrottle + uPitch + uRoll - uYaw; //Calculate the pulse for esc 3 (rear-left - CCW) + v4= desiredThrottle - uPitch + uRoll + uYaw; //Calculate the pulse for esc 4 (front-left - CW) + + if ( v1 > 2000 ){ + v1=2000; + } + if ( v1 < 1000){ + v1=1000; + } + if ( v2 > 2000 ){ + v2=2000; + } + if ( v2 < 1000){ + v2=1000; + } + if ( v3 > 2000 ){ + v3=2000; + } + if ( v3 < 1000){ + v3=1000; + } + if ( v4 > 2000 ){ + v4=2000; + } + if ( v4 < 1000){ + v4=1000; + } + } + else{ // In the case where desired throttle is <1050 microseconds, we want to deactivate all motors + v1= desiredThrottle; + v2= desiredThrottle; + v3= desiredThrottle; + v4= desiredThrottle; + } + + // outputing the necessary PWM value to the motors + PWM_motor1.pulsewidth_us(abs(v1)); //Set the Pulsewidth output + PWM_motor2.pulsewidth_us(abs(v2)); //Set the Pulsewidth output + PWM_motor3.pulsewidth_us(abs(v3)); //Set the Pulsewidth output + PWM_motor4.pulsewidth_us(abs(v4)); //Set the Pulsewidth output + + } + +} + + +// ******** Periodic Interrupt Handler 1******** +void PeriodicInterruptISR(void){ + osSignalSet(PiControlId,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. +} +