Koceila Cherfouh
This is the code to implement a quadcopter. PID controller tuning is necessary.
Dependencies: PwmIn mbed MPU6050IMU
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "rtos.h" 00003 #include "MPU6050.h" 00004 #include "PwmIn.h" 00005 00006 //Functions 00007 void PID1(void const *argument); //PID Control 00008 void PeriodicInterruptISR(void); //Periodic Timer Interrupt Serive Routine 00009 //Processes and Threads 00010 osThreadId PiControlId; // Thread ID 00011 osThreadDef(PID1, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority 00012 00013 void receiver_data(); 00014 //------------------------------------------------------------------------------------------------------------- 00015 00016 //System Definitions: 00017 #define PWM_Period 20000 //in micro-seconds 00018 #define ESC_ARM 1000 // this set the PWM signal to 50% duty cycle for a signal period of 2000 micro seconds 00019 #define dt 0.005 // periodic interrupt each 20 ms... 50 Hz refresh rate of quadcopter motor speeds 00020 00021 00022 // Controller Gains, and boundaries 00023 //Roll 00024 #define uRollMax 300 00025 //Pitch 00026 #define uPitchMax 300 00027 // Throttle 00028 #define MaxThrottle 1600 // 1600 micro seconds 00029 #define MinThrottle 1100 // 1100 micro seconds 00030 // Yaw 00031 #define uYawMax 200 00032 00033 //------------------------------------------------------------------------------------------------------------- 00034 00035 //Port Configuration: 00036 // These are PWM In's that define the system's desired points and other control commands 00037 // Note: the ports of the following PWM input pins have to be interrupt enabled (Port A and D are the only 00038 // interrupt enabled ports for KL25z). 00039 PwmIn channel1(PTA5); // Roll 00040 PwmIn channel2(PTA13); // {itch 00041 PwmIn channel3(PTA16); // Throttle 00042 PwmIn channel4(PTA17); // Yaw 00043 00044 //PWM Outputs 00045 // Configuration of motors: 00046 // Motor4 Motor1 00047 // . ^ . 00048 // . | . 00049 // . | . 00050 // . . 00051 // Motor3 Motor2 00052 PwmOut PWM_motor1(PTB3); //PWM Motor1 00053 PwmOut PWM_motor3(PTB1); //PWM Motor3 00054 PwmOut PWM_motor2(PTB2); //PWM Motor2 00055 PwmOut PWM_motor4(PTB0); //PWM Motor4 00056 00057 // MPU6050 gyroscope and accelerometer 00058 // Note the library MPU6050.h must be modified to match the chosen I2C pins 00059 // defined in the hardware design 00060 MPU6050 mpu6050; 00061 00062 //Timer Interrupts 00063 Timer t; 00064 Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt 00065 Serial pc(USBTX, USBRX); // tx, rx 00066 //------------------------------------------------------------------------------------------------------------- 00067 00068 //Gloabal Variables Definitions: 00069 00070 // PID 00071 //Throttle 00072 float desiredThrottle; 00073 //Roll 00074 float eRoll, desiredRoll, desRoll, actualRoll, eDerRoll, elastRoll, eIntRoll, uRoll; 00075 float KpRoll, KdRoll, KiRoll; 00076 //Pitch 00077 float ePitch, desiredPitch, desPitch, actualPitch, eDerPitch, elastPitch, eIntPitch, uPitch; 00078 float KpPitch, KdPitch, KiPitch; 00079 00080 //Yaw 00081 float eYaw, desiredYaw, desYaw, actualYaw, eDerYaw, elastYaw, eIntYaw, uYaw; 00082 float KpYaw, KdYaw, KiYaw; 00083 00084 // control speeds of the motors 00085 float v1, v2, v3, v4; 00086 00087 // Gyro Stuff 00088 float sum = 0; 00089 uint32_t sumCount = 0; 00090 00091 00092 float RollCalibration, PitchCalibration, YawCalibration; 00093 float actRoll, actPitch, actYaw; 00094 00095 //------------------------------------------------------------------------------------------------------------- 00096 00097 int main() { 00098 00099 // System setup 00100 // this sets up the periodic interrupt, the UART baud rate communication with the PC, as well as 00101 // setting up the PWM signal period of each motor. 00102 pc.baud(9600); // Set max uart baud rate 00103 pc.printf("We have good serial communication! :D\n"); 00104 00105 //Initializing Threads for interrupts 00106 PiControlId = osThreadCreate(osThread(PID1), NULL); //Create Thread for PID1 00107 PeriodicInt.attach(&PeriodicInterruptISR, dt); //Periodic timer interrupt every dt seconds as defined above 00108 00109 //PWM Period 00110 PWM_motor1.period_us(PWM_Period); // This sets the PWM period to [PWM_Period] 00111 PWM_motor2.period_us(PWM_Period); // 00112 PWM_motor3.period_us(PWM_Period); // 00113 PWM_motor4.period_us(PWM_Period); // 00114 00115 // this sets all motor PWM signals to 50% to arm the esc's for operation 00116 PWM_motor1.pulsewidth_us(2000); //Set the Pulsewidth output 00117 PWM_motor2.pulsewidth_us(2000); //Set the Pulsewidth output 00118 PWM_motor3.pulsewidth_us(2000); //Set the Pulsewidth output 00119 PWM_motor4.pulsewidth_us(2000); //Set the Pulsewidth output 00120 00121 wait(10); 00122 PWM_motor1.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output 00123 PWM_motor2.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output 00124 PWM_motor3.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output 00125 PWM_motor4.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output 00126 00127 KpRoll = .55; 00128 KdRoll = 0; 00129 KiRoll = 0; 00130 00131 KpPitch = KpRoll; 00132 KdPitch = KdRoll; 00133 KiPitch = KiRoll; 00134 00135 //Set up I2C 00136 i2c.frequency(400000); // use fast (400 kHz) I2C 00137 t.start(); 00138 // Read the WHO_AM_I register, this is a good test of communication 00139 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 00140 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); 00141 00142 if (whoami == 0x68){ // WHO_AM_I should always be 0x68 00143 00144 pc.printf("MPU6050 is online..."); 00145 wait(1); 00146 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values 00147 pc.printf("x-axis self test: acceleration trim within : "); 00148 pc.printf("%f \n\r", SelfTest[0]); 00149 pc.printf("y-axis self test: acceleration trim within : "); 00150 pc.printf("%f \n\r", SelfTest[1]); 00151 pc.printf("z-axis self test: acceleration trim within : "); 00152 pc.printf("%f \n\r", SelfTest[2]); 00153 pc.printf("x-axis self test: gyration trim within : "); 00154 pc.printf("%f \n\r", SelfTest[3]); 00155 pc.printf("y-axis self test: gyration trim within : "); 00156 pc.printf("%f \n\r", SelfTest[4]); 00157 pc.printf("z-axis self test: gyration trim within : "); 00158 pc.printf("%f \n\r", SelfTest[5]); 00159 00160 wait(1); 00161 00162 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){ 00163 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration 00164 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers 00165 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature 00166 00167 wait(2); 00168 } 00169 else { 00170 pc.printf("Device did not the pass self-test!\n\r"); 00171 } 00172 } 00173 00174 else{ 00175 pc.printf("Could not connect to MPU6050: \n\r"); 00176 pc.printf("%#x \n", whoami); 00177 while(1) ; // Loop forever if communication doesn't happen 00178 } 00179 00180 wait(10); // wait 10 seconds to make sure esc's are armed and ready to go 00181 00182 // counter for mpu calibration 00183 int counter = 0; 00184 //Loop Forever 00185 while (1) { 00186 00187 00188 // If data ready bit set, all data registers have new data 00189 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt 00190 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values 00191 mpu6050.getAres(); 00192 00193 // Now we'll calculate the accleration value into actual g's 00194 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set 00195 ay = (float)accelCount[1]*aRes - accelBias[1]; 00196 az = (float)accelCount[2]*aRes - accelBias[2]; 00197 00198 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values 00199 mpu6050.getGres(); 00200 00201 // Calculate the gyro value into actual degrees per second 00202 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set 00203 gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; 00204 gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; 00205 00206 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values 00207 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade 00208 } 00209 00210 Now = t.read_us(); 00211 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update 00212 lastUpdate = Now; 00213 00214 sum += deltat; 00215 sumCount++; 00216 00217 if(lastUpdate - firstUpdate > 10000000.0f) { 00218 beta = 0.04; // decrease filter gain after stabilized 00219 zeta = 0.015; // increasey bias drift gain after stabilized 00220 } 00221 00222 // Pass gyro rate as rad/s 00223 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); 00224 00225 00226 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. 00227 // In this coordinate system, the positive z-axis is down toward Earth. 00228 // 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. 00229 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. 00230 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. 00231 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. 00232 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be 00233 // applied in the correct order which for this configuration is yaw, pitch, and then roll. 00234 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. 00235 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]); 00236 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 00237 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]); 00238 actPitch = pitch * 180.0f / PI; 00239 actYaw = yaw * 180.0f / PI; 00240 actRoll = roll * 180.0f / PI; 00241 00242 if (counter < 800){ 00243 counter++; 00244 RollCalibration = actRoll; 00245 PitchCalibration= actPitch; 00246 YawCalibration = actYaw; 00247 } 00248 else{ 00249 00250 actRoll = actRoll - RollCalibration; 00251 actualRoll = actPitch - PitchCalibration; 00252 actualYaw = actYaw - YawCalibration; 00253 00254 if(actRoll > 180){ 00255 actualPitch=-(actRoll-360); 00256 } 00257 if(actRoll < -180){ 00258 actualPitch=-(actRoll+360); 00259 } 00260 if ((actRoll > -180) && actRoll < 180) { 00261 actualPitch=-actRoll; 00262 } 00263 00264 00265 // Measuring receiver values 00266 desRoll = 1000000 * channel1.pulsewidth(); // desired point for roll 00267 desPitch = 1000000 * channel2.pulsewidth(); // desired point for pitch 00268 desiredThrottle = 1000000 * channel3.pulsewidth(); // desired point for throttle 00269 desYaw = 1000000 * channel4.pulsewidth(); // desired point for yaw 00270 00271 // The desired roll, pitch and yaw has a range between 1000 to 2000 microseconds 00272 // we will map these values to limit the desired input between -45deg to +45deg for roll and pitch 00273 // As for yaw, the desired input will range between -180 to 180 deg 00274 // 00275 // The equaion below maps the PWM value to a desired deg value, then it is shifter by some amount of degree so that 00276 // the 1000 us is equivalent to -45deg, and the 2000 us point is equivalent to 45deg. 00277 // The same logic is used for the Yaw equation 00278 desiredRoll = (90 * (abs(desRoll -1000))/1000) - 45; 00279 desiredPitch= (90 * (abs(desPitch-1000))/1000) - 45; 00280 desiredYaw = (360* (abs(desYaw -1000))/1000) - 180; 00281 } 00282 // pc.printf("%f %f %f\n\r", actualPitch, desiredPitch, ePitch); 00283 // pc.printf("%f %f %f %f\n\r ", uRoll, uPitch, eRoll, ePitch); 00284 00285 pc.printf("%f %f %f %f\n\r", v1, v2, v3, v4); 00286 // pc.printf("%f %f %f %f %f %f \n\r\n\r\n\r", actualRoll, desiredRoll, actualPitch, desiredPitch, actualYaw, eYaw ); 00287 /* 00288 pc.printf("%f \n\r", v1); 00289 pc.printf("%f \n\r", v2); 00290 pc.printf("%f \n\r", v3); 00291 pc.printf("%f \n\r \n\r", v4); 00292 00293 pc.printf("error in roll %f \n\r \n\r", eRoll); 00294 pc.printf("error in pitch %f \n\r \n\r", ePitch); 00295 pc.printf("error in yaw %f \n\r \n\r", eYaw);*/ 00296 /* 00297 pc.printf("Pulse width Roll value %f \n\r",desRoll); 00298 pc.printf("Pulse width Pitch value %f \n\r",desPitch); 00299 pc.printf("Pulse width Throttle value %f \n\r",desiredThrottle); 00300 pc.printf("Pulse width Yaw value %f \n\r",desYaw); 00301 */ 00302 /* 00303 pc.printf("Desired Roll: %f \n\r", desiredRoll); 00304 pc.printf("Desired Pitch: %f \n\r", desiredPitch); 00305 pc.printf("Desired Throttle: %f \n\r", desiredThrottle); 00306 pc.printf("Desired Yaw: %f \n\r", desiredYaw); 00307 */ 00308 myled= !myled; 00309 count = t.read_ms(); 00310 sum = 0; 00311 sumCount = 0; 00312 00313 //wait(.5); 00314 00315 } 00316 00317 00318 00319 } 00320 00321 00322 00323 // Updated on: Sept 19 2019 00324 // Koceila Cherfouh 00325 // Deescription 00326 // periodic time interrupt that controls the speed of the motor based on the gyro and accelerometer readings using a PID controller. 00327 // 00328 // Update and whats left to do: 00329 // make sure all variables are declared. define max values, and gain values for each PID controllers. 00330 // define the motor speed assignment based on the system's kinematic model 00331 // test and tune the controller gain 00332 00333 // ******** Periodic Thread ******** 00334 void PID1(void const *argument){ 00335 00336 while(true){ 00337 osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. 00338 00339 if (desiredThrottle > 1050){ // 1050 micro seconds will serve as the activation throttle 00340 // 1- PID roll 00341 eRoll= desiredRoll - actualRoll; 00342 eDerRoll = eRoll - elastRoll; // derivative of error 00343 uRoll = (KpRoll * eRoll) + (KdRoll * eDerRoll) + (KiRoll * eIntRoll); // computing the pitch control input 00344 // this part of the code limits the integrator windeup. an effect that takes place when 00345 if (uRoll > uRollMax){ 00346 uRoll = uRollMax; 00347 } 00348 if (uRoll < -uRollMax){ 00349 uRoll = -uRollMax; 00350 } 00351 else { 00352 eIntRoll += eRoll; // Computing the integral sum 00353 } 00354 elastRoll = eRoll; 00355 //------------------------------------------------------------------------------------------------------------- 00356 // 2- PID pitch 00357 ePitch = desiredPitch - actualPitch; // error 00358 eDerPitch = ePitch - elastPitch ; // derivative of error 00359 uPitch = (KpPitch * ePitch) + (KdPitch * eDerPitch) + (KiPitch * eIntPitch); // computing the pitch control input 00360 // this part of the code limits the integrator windeup. an effect that takes place when 00361 if (uPitch > uPitchMax){ 00362 uPitch = uPitchMax; 00363 } 00364 if (uPitch < -uPitchMax){ 00365 uPitch = -uPitchMax; 00366 } 00367 else { 00368 eIntPitch += ePitch; // Computing the integral sum 00369 } 00370 elastPitch = ePitch; 00371 //------------------------------------------------------------------------------------------------------------- 00372 // 4- PID yaw 00373 /* eYaw= desiredYaw - actualYaw; 00374 eDerYaw = eYaw - elastYaw ; // derivative of error 00375 uYaw = (KpYaw * eYaw) + (KdYaw * eDerYaw) + (KiYaw * eIntYaw); // computing the pitch control input 00376 // this part of the code limits the integrator windeup. an effect that takes place when 00377 if (uYaw > uYawMax){ 00378 uYaw = uYawMax; 00379 } 00380 else { 00381 eIntYaw += eYaw; // Computing the integral sum 00382 } 00383 elastYaw = eYaw; */ 00384 //------------------------------------------------------------------------------------------------------------- 00385 // Now that all pid control inputs are computed. send the PWM control input using the kinematic model to the ESC's 00386 v1= desiredThrottle - uPitch - uRoll - uYaw; //Calculate the pulse for esc 1 (front-right - CCW) 00387 v2= desiredThrottle + uPitch - uRoll + uYaw; //Calculate the pulse for esc 2 (rear-right - CW) 00388 v3= desiredThrottle + uPitch + uRoll - uYaw; //Calculate the pulse for esc 3 (rear-left - CCW) 00389 v4= desiredThrottle - uPitch + uRoll + uYaw; //Calculate the pulse for esc 4 (front-left - CW) 00390 00391 if ( v1 > 2000 ){ 00392 v1=2000; 00393 } 00394 if ( v1 < 1000){ 00395 v1=1000; 00396 } 00397 if ( v2 > 2000 ){ 00398 v2=2000; 00399 } 00400 if ( v2 < 1000){ 00401 v2=1000; 00402 } 00403 if ( v3 > 2000 ){ 00404 v3=2000; 00405 } 00406 if ( v3 < 1000){ 00407 v3=1000; 00408 } 00409 if ( v4 > 2000 ){ 00410 v4=2000; 00411 } 00412 if ( v4 < 1000){ 00413 v4=1000; 00414 } 00415 } 00416 else{ // In the case where desired throttle is <1050 microseconds, we want to deactivate all motors 00417 v1= desiredThrottle; 00418 v2= desiredThrottle; 00419 v3= desiredThrottle; 00420 v4= desiredThrottle; 00421 } 00422 00423 // outputing the necessary PWM value to the motors 00424 PWM_motor1.pulsewidth_us(abs(v1)); //Set the Pulsewidth output 00425 PWM_motor2.pulsewidth_us(abs(v2)); //Set the Pulsewidth output 00426 PWM_motor3.pulsewidth_us(abs(v3)); //Set the Pulsewidth output 00427 PWM_motor4.pulsewidth_us(abs(v4)); //Set the Pulsewidth output 00428 00429 } 00430 00431 } 00432 00433 00434 // ******** Periodic Interrupt Handler 1******** 00435 void PeriodicInterruptISR(void){ 00436 osSignalSet(PiControlId,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. 00437 } 00438
Generated on Fri Jul 29 2022 03:37:51 by
1.7.2