This is the code to implement a quadcopter. PID controller tuning is necessary.
Dependencies: PwmIn mbed MPU6050IMU
main.cpp@1:2021161b8fd2, 2019-10-08 (annotated)
- Committer:
- kcherfou
- Date:
- Tue Oct 08 13:51:00 2019 +0000
- Revision:
- 1:2021161b8fd2
- Parent:
- 0:6038ca525e44
changed to mbed OS 5
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
kcherfou | 0:6038ca525e44 | 1 | #include "mbed.h" |
kcherfou | 0:6038ca525e44 | 2 | #include "rtos.h" |
kcherfou | 0:6038ca525e44 | 3 | #include "MPU6050.h" |
kcherfou | 0:6038ca525e44 | 4 | #include "PwmIn.h" |
kcherfou | 0:6038ca525e44 | 5 | |
kcherfou | 0:6038ca525e44 | 6 | //Functions |
kcherfou | 0:6038ca525e44 | 7 | void PID1(void const *argument); //PID Control |
kcherfou | 0:6038ca525e44 | 8 | void PeriodicInterruptISR(void); //Periodic Timer Interrupt Serive Routine |
kcherfou | 0:6038ca525e44 | 9 | //Processes and Threads |
kcherfou | 0:6038ca525e44 | 10 | osThreadId PiControlId; // Thread ID |
kcherfou | 0:6038ca525e44 | 11 | osThreadDef(PID1, osPriorityRealtime, 1024); // Declare PiControlThread as a thread, highest priority |
kcherfou | 0:6038ca525e44 | 12 | |
kcherfou | 0:6038ca525e44 | 13 | void receiver_data(); |
kcherfou | 0:6038ca525e44 | 14 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 15 | |
kcherfou | 0:6038ca525e44 | 16 | //System Definitions: |
kcherfou | 0:6038ca525e44 | 17 | #define PWM_Period 20000 //in micro-seconds |
kcherfou | 0:6038ca525e44 | 18 | #define ESC_ARM 1000 // this set the PWM signal to 50% duty cycle for a signal period of 2000 micro seconds |
kcherfou | 0:6038ca525e44 | 19 | #define dt 0.005 // periodic interrupt each 20 ms... 50 Hz refresh rate of quadcopter motor speeds |
kcherfou | 0:6038ca525e44 | 20 | |
kcherfou | 0:6038ca525e44 | 21 | |
kcherfou | 0:6038ca525e44 | 22 | // Controller Gains, and boundaries |
kcherfou | 0:6038ca525e44 | 23 | //Roll |
kcherfou | 0:6038ca525e44 | 24 | #define uRollMax 300 |
kcherfou | 0:6038ca525e44 | 25 | //Pitch |
kcherfou | 0:6038ca525e44 | 26 | #define uPitchMax 300 |
kcherfou | 0:6038ca525e44 | 27 | // Throttle |
kcherfou | 0:6038ca525e44 | 28 | #define MaxThrottle 1600 // 1600 micro seconds |
kcherfou | 0:6038ca525e44 | 29 | #define MinThrottle 1100 // 1100 micro seconds |
kcherfou | 0:6038ca525e44 | 30 | // Yaw |
kcherfou | 0:6038ca525e44 | 31 | #define uYawMax 200 |
kcherfou | 0:6038ca525e44 | 32 | |
kcherfou | 0:6038ca525e44 | 33 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 34 | |
kcherfou | 0:6038ca525e44 | 35 | //Port Configuration: |
kcherfou | 0:6038ca525e44 | 36 | // These are PWM In's that define the system's desired points and other control commands |
kcherfou | 0:6038ca525e44 | 37 | // Note: the ports of the following PWM input pins have to be interrupt enabled (Port A and D are the only |
kcherfou | 0:6038ca525e44 | 38 | // interrupt enabled ports for KL25z). |
kcherfou | 0:6038ca525e44 | 39 | PwmIn channel1(PTA5); // Roll |
kcherfou | 0:6038ca525e44 | 40 | PwmIn channel2(PTA13); // {itch |
kcherfou | 0:6038ca525e44 | 41 | PwmIn channel3(PTA16); // Throttle |
kcherfou | 0:6038ca525e44 | 42 | PwmIn channel4(PTA17); // Yaw |
kcherfou | 0:6038ca525e44 | 43 | |
kcherfou | 0:6038ca525e44 | 44 | //PWM Outputs |
kcherfou | 0:6038ca525e44 | 45 | // Configuration of motors: |
kcherfou | 0:6038ca525e44 | 46 | // Motor4 Motor1 |
kcherfou | 0:6038ca525e44 | 47 | // . ^ . |
kcherfou | 0:6038ca525e44 | 48 | // . | . |
kcherfou | 0:6038ca525e44 | 49 | // . | . |
kcherfou | 0:6038ca525e44 | 50 | // . . |
kcherfou | 0:6038ca525e44 | 51 | // Motor3 Motor2 |
kcherfou | 0:6038ca525e44 | 52 | PwmOut PWM_motor1(PTB3); //PWM Motor1 |
kcherfou | 0:6038ca525e44 | 53 | PwmOut PWM_motor3(PTB1); //PWM Motor3 |
kcherfou | 0:6038ca525e44 | 54 | PwmOut PWM_motor2(PTB2); //PWM Motor2 |
kcherfou | 0:6038ca525e44 | 55 | PwmOut PWM_motor4(PTB0); //PWM Motor4 |
kcherfou | 0:6038ca525e44 | 56 | |
kcherfou | 0:6038ca525e44 | 57 | // MPU6050 gyroscope and accelerometer |
kcherfou | 0:6038ca525e44 | 58 | // Note the library MPU6050.h must be modified to match the chosen I2C pins |
kcherfou | 0:6038ca525e44 | 59 | // defined in the hardware design |
kcherfou | 0:6038ca525e44 | 60 | MPU6050 mpu6050; |
kcherfou | 0:6038ca525e44 | 61 | |
kcherfou | 0:6038ca525e44 | 62 | //Timer Interrupts |
kcherfou | 0:6038ca525e44 | 63 | Timer t; |
kcherfou | 0:6038ca525e44 | 64 | Ticker PeriodicInt; // Declare a timer interrupt: PeriodicInt |
kcherfou | 0:6038ca525e44 | 65 | Serial pc(USBTX, USBRX); // tx, rx |
kcherfou | 0:6038ca525e44 | 66 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 67 | |
kcherfou | 0:6038ca525e44 | 68 | //Gloabal Variables Definitions: |
kcherfou | 0:6038ca525e44 | 69 | |
kcherfou | 0:6038ca525e44 | 70 | // PID |
kcherfou | 0:6038ca525e44 | 71 | //Throttle |
kcherfou | 0:6038ca525e44 | 72 | float desiredThrottle; |
kcherfou | 0:6038ca525e44 | 73 | //Roll |
kcherfou | 0:6038ca525e44 | 74 | float eRoll, desiredRoll, desRoll, actualRoll, eDerRoll, elastRoll, eIntRoll, uRoll; |
kcherfou | 0:6038ca525e44 | 75 | float KpRoll, KdRoll, KiRoll; |
kcherfou | 0:6038ca525e44 | 76 | //Pitch |
kcherfou | 0:6038ca525e44 | 77 | float ePitch, desiredPitch, desPitch, actualPitch, eDerPitch, elastPitch, eIntPitch, uPitch; |
kcherfou | 0:6038ca525e44 | 78 | float KpPitch, KdPitch, KiPitch; |
kcherfou | 0:6038ca525e44 | 79 | |
kcherfou | 0:6038ca525e44 | 80 | //Yaw |
kcherfou | 0:6038ca525e44 | 81 | float eYaw, desiredYaw, desYaw, actualYaw, eDerYaw, elastYaw, eIntYaw, uYaw; |
kcherfou | 0:6038ca525e44 | 82 | float KpYaw, KdYaw, KiYaw; |
kcherfou | 0:6038ca525e44 | 83 | |
kcherfou | 0:6038ca525e44 | 84 | // control speeds of the motors |
kcherfou | 0:6038ca525e44 | 85 | float v1, v2, v3, v4; |
kcherfou | 0:6038ca525e44 | 86 | |
kcherfou | 0:6038ca525e44 | 87 | // Gyro Stuff |
kcherfou | 0:6038ca525e44 | 88 | float sum = 0; |
kcherfou | 0:6038ca525e44 | 89 | uint32_t sumCount = 0; |
kcherfou | 0:6038ca525e44 | 90 | |
kcherfou | 0:6038ca525e44 | 91 | |
kcherfou | 0:6038ca525e44 | 92 | float RollCalibration, PitchCalibration, YawCalibration; |
kcherfou | 0:6038ca525e44 | 93 | float actRoll, actPitch, actYaw; |
kcherfou | 0:6038ca525e44 | 94 | |
kcherfou | 0:6038ca525e44 | 95 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 96 | |
kcherfou | 0:6038ca525e44 | 97 | int main() { |
kcherfou | 0:6038ca525e44 | 98 | |
kcherfou | 0:6038ca525e44 | 99 | // System setup |
kcherfou | 0:6038ca525e44 | 100 | // this sets up the periodic interrupt, the UART baud rate communication with the PC, as well as |
kcherfou | 0:6038ca525e44 | 101 | // setting up the PWM signal period of each motor. |
kcherfou | 0:6038ca525e44 | 102 | pc.baud(9600); // Set max uart baud rate |
kcherfou | 0:6038ca525e44 | 103 | pc.printf("We have good serial communication! :D\n"); |
kcherfou | 0:6038ca525e44 | 104 | |
kcherfou | 0:6038ca525e44 | 105 | //Initializing Threads for interrupts |
kcherfou | 0:6038ca525e44 | 106 | PiControlId = osThreadCreate(osThread(PID1), NULL); //Create Thread for PID1 |
kcherfou | 0:6038ca525e44 | 107 | PeriodicInt.attach(&PeriodicInterruptISR, dt); //Periodic timer interrupt every dt seconds as defined above |
kcherfou | 0:6038ca525e44 | 108 | |
kcherfou | 0:6038ca525e44 | 109 | //PWM Period |
kcherfou | 0:6038ca525e44 | 110 | PWM_motor1.period_us(PWM_Period); // This sets the PWM period to [PWM_Period] |
kcherfou | 0:6038ca525e44 | 111 | PWM_motor2.period_us(PWM_Period); // |
kcherfou | 0:6038ca525e44 | 112 | PWM_motor3.period_us(PWM_Period); // |
kcherfou | 0:6038ca525e44 | 113 | PWM_motor4.period_us(PWM_Period); // |
kcherfou | 0:6038ca525e44 | 114 | |
kcherfou | 0:6038ca525e44 | 115 | // this sets all motor PWM signals to 50% to arm the esc's for operation |
kcherfou | 0:6038ca525e44 | 116 | PWM_motor1.pulsewidth_us(2000); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 117 | PWM_motor2.pulsewidth_us(2000); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 118 | PWM_motor3.pulsewidth_us(2000); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 119 | PWM_motor4.pulsewidth_us(2000); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 120 | |
kcherfou | 0:6038ca525e44 | 121 | wait(10); |
kcherfou | 0:6038ca525e44 | 122 | PWM_motor1.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 123 | PWM_motor2.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 124 | PWM_motor3.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 125 | PWM_motor4.pulsewidth_us(ESC_ARM); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 126 | |
kcherfou | 0:6038ca525e44 | 127 | KpRoll = .55; |
kcherfou | 0:6038ca525e44 | 128 | KdRoll = 0; |
kcherfou | 0:6038ca525e44 | 129 | KiRoll = 0; |
kcherfou | 0:6038ca525e44 | 130 | |
kcherfou | 0:6038ca525e44 | 131 | KpPitch = KpRoll; |
kcherfou | 0:6038ca525e44 | 132 | KdPitch = KdRoll; |
kcherfou | 0:6038ca525e44 | 133 | KiPitch = KiRoll; |
kcherfou | 0:6038ca525e44 | 134 | |
kcherfou | 0:6038ca525e44 | 135 | //Set up I2C |
kcherfou | 0:6038ca525e44 | 136 | i2c.frequency(400000); // use fast (400 kHz) I2C |
kcherfou | 0:6038ca525e44 | 137 | t.start(); |
kcherfou | 0:6038ca525e44 | 138 | // Read the WHO_AM_I register, this is a good test of communication |
kcherfou | 0:6038ca525e44 | 139 | uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 |
kcherfou | 0:6038ca525e44 | 140 | pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); |
kcherfou | 0:6038ca525e44 | 141 | |
kcherfou | 0:6038ca525e44 | 142 | if (whoami == 0x68){ // WHO_AM_I should always be 0x68 |
kcherfou | 0:6038ca525e44 | 143 | |
kcherfou | 0:6038ca525e44 | 144 | pc.printf("MPU6050 is online..."); |
kcherfou | 0:6038ca525e44 | 145 | wait(1); |
kcherfou | 0:6038ca525e44 | 146 | mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values |
kcherfou | 0:6038ca525e44 | 147 | pc.printf("x-axis self test: acceleration trim within : "); |
kcherfou | 0:6038ca525e44 | 148 | pc.printf("%f \n\r", SelfTest[0]); |
kcherfou | 0:6038ca525e44 | 149 | pc.printf("y-axis self test: acceleration trim within : "); |
kcherfou | 0:6038ca525e44 | 150 | pc.printf("%f \n\r", SelfTest[1]); |
kcherfou | 0:6038ca525e44 | 151 | pc.printf("z-axis self test: acceleration trim within : "); |
kcherfou | 0:6038ca525e44 | 152 | pc.printf("%f \n\r", SelfTest[2]); |
kcherfou | 0:6038ca525e44 | 153 | pc.printf("x-axis self test: gyration trim within : "); |
kcherfou | 0:6038ca525e44 | 154 | pc.printf("%f \n\r", SelfTest[3]); |
kcherfou | 0:6038ca525e44 | 155 | pc.printf("y-axis self test: gyration trim within : "); |
kcherfou | 0:6038ca525e44 | 156 | pc.printf("%f \n\r", SelfTest[4]); |
kcherfou | 0:6038ca525e44 | 157 | pc.printf("z-axis self test: gyration trim within : "); |
kcherfou | 0:6038ca525e44 | 158 | pc.printf("%f \n\r", SelfTest[5]); |
kcherfou | 0:6038ca525e44 | 159 | |
kcherfou | 0:6038ca525e44 | 160 | wait(1); |
kcherfou | 0:6038ca525e44 | 161 | |
kcherfou | 0:6038ca525e44 | 162 | 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){ |
kcherfou | 0:6038ca525e44 | 163 | mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration |
kcherfou | 0:6038ca525e44 | 164 | mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers |
kcherfou | 0:6038ca525e44 | 165 | mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature |
kcherfou | 0:6038ca525e44 | 166 | |
kcherfou | 0:6038ca525e44 | 167 | wait(2); |
kcherfou | 0:6038ca525e44 | 168 | } |
kcherfou | 0:6038ca525e44 | 169 | else { |
kcherfou | 0:6038ca525e44 | 170 | pc.printf("Device did not the pass self-test!\n\r"); |
kcherfou | 0:6038ca525e44 | 171 | } |
kcherfou | 0:6038ca525e44 | 172 | } |
kcherfou | 0:6038ca525e44 | 173 | |
kcherfou | 0:6038ca525e44 | 174 | else{ |
kcherfou | 0:6038ca525e44 | 175 | pc.printf("Could not connect to MPU6050: \n\r"); |
kcherfou | 0:6038ca525e44 | 176 | pc.printf("%#x \n", whoami); |
kcherfou | 0:6038ca525e44 | 177 | while(1) ; // Loop forever if communication doesn't happen |
kcherfou | 0:6038ca525e44 | 178 | } |
kcherfou | 0:6038ca525e44 | 179 | |
kcherfou | 0:6038ca525e44 | 180 | wait(10); // wait 10 seconds to make sure esc's are armed and ready to go |
kcherfou | 0:6038ca525e44 | 181 | |
kcherfou | 0:6038ca525e44 | 182 | // counter for mpu calibration |
kcherfou | 0:6038ca525e44 | 183 | int counter = 0; |
kcherfou | 0:6038ca525e44 | 184 | //Loop Forever |
kcherfou | 0:6038ca525e44 | 185 | while (1) { |
kcherfou | 0:6038ca525e44 | 186 | |
kcherfou | 0:6038ca525e44 | 187 | |
kcherfou | 0:6038ca525e44 | 188 | // If data ready bit set, all data registers have new data |
kcherfou | 0:6038ca525e44 | 189 | if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt |
kcherfou | 0:6038ca525e44 | 190 | mpu6050.readAccelData(accelCount); // Read the x/y/z adc values |
kcherfou | 0:6038ca525e44 | 191 | mpu6050.getAres(); |
kcherfou | 0:6038ca525e44 | 192 | |
kcherfou | 0:6038ca525e44 | 193 | // Now we'll calculate the accleration value into actual g's |
kcherfou | 0:6038ca525e44 | 194 | ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set |
kcherfou | 0:6038ca525e44 | 195 | ay = (float)accelCount[1]*aRes - accelBias[1]; |
kcherfou | 0:6038ca525e44 | 196 | az = (float)accelCount[2]*aRes - accelBias[2]; |
kcherfou | 0:6038ca525e44 | 197 | |
kcherfou | 0:6038ca525e44 | 198 | mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values |
kcherfou | 0:6038ca525e44 | 199 | mpu6050.getGres(); |
kcherfou | 0:6038ca525e44 | 200 | |
kcherfou | 0:6038ca525e44 | 201 | // Calculate the gyro value into actual degrees per second |
kcherfou | 0:6038ca525e44 | 202 | gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set |
kcherfou | 0:6038ca525e44 | 203 | gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; |
kcherfou | 0:6038ca525e44 | 204 | gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; |
kcherfou | 0:6038ca525e44 | 205 | |
kcherfou | 0:6038ca525e44 | 206 | tempCount = mpu6050.readTempData(); // Read the x/y/z adc values |
kcherfou | 0:6038ca525e44 | 207 | temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade |
kcherfou | 0:6038ca525e44 | 208 | } |
kcherfou | 0:6038ca525e44 | 209 | |
kcherfou | 0:6038ca525e44 | 210 | Now = t.read_us(); |
kcherfou | 0:6038ca525e44 | 211 | deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update |
kcherfou | 0:6038ca525e44 | 212 | lastUpdate = Now; |
kcherfou | 0:6038ca525e44 | 213 | |
kcherfou | 0:6038ca525e44 | 214 | sum += deltat; |
kcherfou | 0:6038ca525e44 | 215 | sumCount++; |
kcherfou | 0:6038ca525e44 | 216 | |
kcherfou | 0:6038ca525e44 | 217 | if(lastUpdate - firstUpdate > 10000000.0f) { |
kcherfou | 0:6038ca525e44 | 218 | beta = 0.04; // decrease filter gain after stabilized |
kcherfou | 0:6038ca525e44 | 219 | zeta = 0.015; // increasey bias drift gain after stabilized |
kcherfou | 0:6038ca525e44 | 220 | } |
kcherfou | 0:6038ca525e44 | 221 | |
kcherfou | 0:6038ca525e44 | 222 | // Pass gyro rate as rad/s |
kcherfou | 0:6038ca525e44 | 223 | mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); |
kcherfou | 0:6038ca525e44 | 224 | |
kcherfou | 0:6038ca525e44 | 225 | |
kcherfou | 0:6038ca525e44 | 226 | // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation. |
kcherfou | 0:6038ca525e44 | 227 | // In this coordinate system, the positive z-axis is down toward Earth. |
kcherfou | 0:6038ca525e44 | 228 | // 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. |
kcherfou | 0:6038ca525e44 | 229 | // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative. |
kcherfou | 0:6038ca525e44 | 230 | // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll. |
kcherfou | 0:6038ca525e44 | 231 | // These arise from the definition of the homogeneous rotation matrix constructed from quaternions. |
kcherfou | 0:6038ca525e44 | 232 | // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be |
kcherfou | 0:6038ca525e44 | 233 | // applied in the correct order which for this configuration is yaw, pitch, and then roll. |
kcherfou | 0:6038ca525e44 | 234 | // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links. |
kcherfou | 0:6038ca525e44 | 235 | 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]); |
kcherfou | 0:6038ca525e44 | 236 | pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); |
kcherfou | 0:6038ca525e44 | 237 | 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]); |
kcherfou | 0:6038ca525e44 | 238 | actPitch = pitch * 180.0f / PI; |
kcherfou | 0:6038ca525e44 | 239 | actYaw = yaw * 180.0f / PI; |
kcherfou | 0:6038ca525e44 | 240 | actRoll = roll * 180.0f / PI; |
kcherfou | 0:6038ca525e44 | 241 | |
kcherfou | 0:6038ca525e44 | 242 | if (counter < 800){ |
kcherfou | 0:6038ca525e44 | 243 | counter++; |
kcherfou | 0:6038ca525e44 | 244 | RollCalibration = actRoll; |
kcherfou | 0:6038ca525e44 | 245 | PitchCalibration= actPitch; |
kcherfou | 0:6038ca525e44 | 246 | YawCalibration = actYaw; |
kcherfou | 0:6038ca525e44 | 247 | } |
kcherfou | 0:6038ca525e44 | 248 | else{ |
kcherfou | 0:6038ca525e44 | 249 | |
kcherfou | 0:6038ca525e44 | 250 | actRoll = actRoll - RollCalibration; |
kcherfou | 0:6038ca525e44 | 251 | actualRoll = actPitch - PitchCalibration; |
kcherfou | 0:6038ca525e44 | 252 | actualYaw = actYaw - YawCalibration; |
kcherfou | 0:6038ca525e44 | 253 | |
kcherfou | 0:6038ca525e44 | 254 | if(actRoll > 180){ |
kcherfou | 0:6038ca525e44 | 255 | actualPitch=-(actRoll-360); |
kcherfou | 0:6038ca525e44 | 256 | } |
kcherfou | 0:6038ca525e44 | 257 | if(actRoll < -180){ |
kcherfou | 0:6038ca525e44 | 258 | actualPitch=-(actRoll+360); |
kcherfou | 0:6038ca525e44 | 259 | } |
kcherfou | 0:6038ca525e44 | 260 | if ((actRoll > -180) && actRoll < 180) { |
kcherfou | 0:6038ca525e44 | 261 | actualPitch=-actRoll; |
kcherfou | 0:6038ca525e44 | 262 | } |
kcherfou | 0:6038ca525e44 | 263 | |
kcherfou | 0:6038ca525e44 | 264 | |
kcherfou | 0:6038ca525e44 | 265 | // Measuring receiver values |
kcherfou | 0:6038ca525e44 | 266 | desRoll = 1000000 * channel1.pulsewidth(); // desired point for roll |
kcherfou | 0:6038ca525e44 | 267 | desPitch = 1000000 * channel2.pulsewidth(); // desired point for pitch |
kcherfou | 0:6038ca525e44 | 268 | desiredThrottle = 1000000 * channel3.pulsewidth(); // desired point for throttle |
kcherfou | 0:6038ca525e44 | 269 | desYaw = 1000000 * channel4.pulsewidth(); // desired point for yaw |
kcherfou | 0:6038ca525e44 | 270 | |
kcherfou | 0:6038ca525e44 | 271 | // The desired roll, pitch and yaw has a range between 1000 to 2000 microseconds |
kcherfou | 0:6038ca525e44 | 272 | // we will map these values to limit the desired input between -45deg to +45deg for roll and pitch |
kcherfou | 0:6038ca525e44 | 273 | // As for yaw, the desired input will range between -180 to 180 deg |
kcherfou | 0:6038ca525e44 | 274 | // |
kcherfou | 0:6038ca525e44 | 275 | // The equaion below maps the PWM value to a desired deg value, then it is shifter by some amount of degree so that |
kcherfou | 0:6038ca525e44 | 276 | // the 1000 us is equivalent to -45deg, and the 2000 us point is equivalent to 45deg. |
kcherfou | 0:6038ca525e44 | 277 | // The same logic is used for the Yaw equation |
kcherfou | 0:6038ca525e44 | 278 | desiredRoll = (90 * (abs(desRoll -1000))/1000) - 45; |
kcherfou | 0:6038ca525e44 | 279 | desiredPitch= (90 * (abs(desPitch-1000))/1000) - 45; |
kcherfou | 0:6038ca525e44 | 280 | desiredYaw = (360* (abs(desYaw -1000))/1000) - 180; |
kcherfou | 0:6038ca525e44 | 281 | } |
kcherfou | 0:6038ca525e44 | 282 | // pc.printf("%f %f %f\n\r", actualPitch, desiredPitch, ePitch); |
kcherfou | 0:6038ca525e44 | 283 | // pc.printf("%f %f %f %f\n\r ", uRoll, uPitch, eRoll, ePitch); |
kcherfou | 0:6038ca525e44 | 284 | |
kcherfou | 0:6038ca525e44 | 285 | pc.printf("%f %f %f %f\n\r", v1, v2, v3, v4); |
kcherfou | 0:6038ca525e44 | 286 | // pc.printf("%f %f %f %f %f %f \n\r\n\r\n\r", actualRoll, desiredRoll, actualPitch, desiredPitch, actualYaw, eYaw ); |
kcherfou | 0:6038ca525e44 | 287 | /* |
kcherfou | 0:6038ca525e44 | 288 | pc.printf("%f \n\r", v1); |
kcherfou | 0:6038ca525e44 | 289 | pc.printf("%f \n\r", v2); |
kcherfou | 0:6038ca525e44 | 290 | pc.printf("%f \n\r", v3); |
kcherfou | 0:6038ca525e44 | 291 | pc.printf("%f \n\r \n\r", v4); |
kcherfou | 0:6038ca525e44 | 292 | |
kcherfou | 0:6038ca525e44 | 293 | pc.printf("error in roll %f \n\r \n\r", eRoll); |
kcherfou | 0:6038ca525e44 | 294 | pc.printf("error in pitch %f \n\r \n\r", ePitch); |
kcherfou | 0:6038ca525e44 | 295 | pc.printf("error in yaw %f \n\r \n\r", eYaw);*/ |
kcherfou | 0:6038ca525e44 | 296 | /* |
kcherfou | 0:6038ca525e44 | 297 | pc.printf("Pulse width Roll value %f \n\r",desRoll); |
kcherfou | 0:6038ca525e44 | 298 | pc.printf("Pulse width Pitch value %f \n\r",desPitch); |
kcherfou | 0:6038ca525e44 | 299 | pc.printf("Pulse width Throttle value %f \n\r",desiredThrottle); |
kcherfou | 0:6038ca525e44 | 300 | pc.printf("Pulse width Yaw value %f \n\r",desYaw); |
kcherfou | 0:6038ca525e44 | 301 | */ |
kcherfou | 0:6038ca525e44 | 302 | /* |
kcherfou | 0:6038ca525e44 | 303 | pc.printf("Desired Roll: %f \n\r", desiredRoll); |
kcherfou | 0:6038ca525e44 | 304 | pc.printf("Desired Pitch: %f \n\r", desiredPitch); |
kcherfou | 0:6038ca525e44 | 305 | pc.printf("Desired Throttle: %f \n\r", desiredThrottle); |
kcherfou | 0:6038ca525e44 | 306 | pc.printf("Desired Yaw: %f \n\r", desiredYaw); |
kcherfou | 0:6038ca525e44 | 307 | */ |
kcherfou | 0:6038ca525e44 | 308 | myled= !myled; |
kcherfou | 0:6038ca525e44 | 309 | count = t.read_ms(); |
kcherfou | 0:6038ca525e44 | 310 | sum = 0; |
kcherfou | 0:6038ca525e44 | 311 | sumCount = 0; |
kcherfou | 0:6038ca525e44 | 312 | |
kcherfou | 0:6038ca525e44 | 313 | //wait(.5); |
kcherfou | 0:6038ca525e44 | 314 | |
kcherfou | 0:6038ca525e44 | 315 | } |
kcherfou | 0:6038ca525e44 | 316 | |
kcherfou | 0:6038ca525e44 | 317 | |
kcherfou | 0:6038ca525e44 | 318 | |
kcherfou | 0:6038ca525e44 | 319 | } |
kcherfou | 0:6038ca525e44 | 320 | |
kcherfou | 0:6038ca525e44 | 321 | |
kcherfou | 0:6038ca525e44 | 322 | |
kcherfou | 0:6038ca525e44 | 323 | // Updated on: Sept 19 2019 |
kcherfou | 0:6038ca525e44 | 324 | // Koceila Cherfouh |
kcherfou | 0:6038ca525e44 | 325 | // Deescription |
kcherfou | 0:6038ca525e44 | 326 | // periodic time interrupt that controls the speed of the motor based on the gyro and accelerometer readings using a PID controller. |
kcherfou | 0:6038ca525e44 | 327 | // |
kcherfou | 0:6038ca525e44 | 328 | // Update and whats left to do: |
kcherfou | 0:6038ca525e44 | 329 | // make sure all variables are declared. define max values, and gain values for each PID controllers. |
kcherfou | 0:6038ca525e44 | 330 | // define the motor speed assignment based on the system's kinematic model |
kcherfou | 0:6038ca525e44 | 331 | // test and tune the controller gain |
kcherfou | 0:6038ca525e44 | 332 | |
kcherfou | 0:6038ca525e44 | 333 | // ******** Periodic Thread ******** |
kcherfou | 0:6038ca525e44 | 334 | void PID1(void const *argument){ |
kcherfou | 0:6038ca525e44 | 335 | |
kcherfou | 0:6038ca525e44 | 336 | while(true){ |
kcherfou | 0:6038ca525e44 | 337 | osSignalWait(0x1, osWaitForever); // Go to sleep until signal is received. |
kcherfou | 0:6038ca525e44 | 338 | |
kcherfou | 0:6038ca525e44 | 339 | if (desiredThrottle > 1050){ // 1050 micro seconds will serve as the activation throttle |
kcherfou | 0:6038ca525e44 | 340 | // 1- PID roll |
kcherfou | 0:6038ca525e44 | 341 | eRoll= desiredRoll - actualRoll; |
kcherfou | 0:6038ca525e44 | 342 | eDerRoll = eRoll - elastRoll; // derivative of error |
kcherfou | 0:6038ca525e44 | 343 | uRoll = (KpRoll * eRoll) + (KdRoll * eDerRoll) + (KiRoll * eIntRoll); // computing the pitch control input |
kcherfou | 0:6038ca525e44 | 344 | // this part of the code limits the integrator windeup. an effect that takes place when |
kcherfou | 0:6038ca525e44 | 345 | if (uRoll > uRollMax){ |
kcherfou | 0:6038ca525e44 | 346 | uRoll = uRollMax; |
kcherfou | 0:6038ca525e44 | 347 | } |
kcherfou | 0:6038ca525e44 | 348 | if (uRoll < -uRollMax){ |
kcherfou | 0:6038ca525e44 | 349 | uRoll = -uRollMax; |
kcherfou | 0:6038ca525e44 | 350 | } |
kcherfou | 0:6038ca525e44 | 351 | else { |
kcherfou | 0:6038ca525e44 | 352 | eIntRoll += eRoll; // Computing the integral sum |
kcherfou | 0:6038ca525e44 | 353 | } |
kcherfou | 0:6038ca525e44 | 354 | elastRoll = eRoll; |
kcherfou | 0:6038ca525e44 | 355 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 356 | // 2- PID pitch |
kcherfou | 0:6038ca525e44 | 357 | ePitch = desiredPitch - actualPitch; // error |
kcherfou | 0:6038ca525e44 | 358 | eDerPitch = ePitch - elastPitch ; // derivative of error |
kcherfou | 0:6038ca525e44 | 359 | uPitch = (KpPitch * ePitch) + (KdPitch * eDerPitch) + (KiPitch * eIntPitch); // computing the pitch control input |
kcherfou | 0:6038ca525e44 | 360 | // this part of the code limits the integrator windeup. an effect that takes place when |
kcherfou | 0:6038ca525e44 | 361 | if (uPitch > uPitchMax){ |
kcherfou | 0:6038ca525e44 | 362 | uPitch = uPitchMax; |
kcherfou | 0:6038ca525e44 | 363 | } |
kcherfou | 0:6038ca525e44 | 364 | if (uPitch < -uPitchMax){ |
kcherfou | 0:6038ca525e44 | 365 | uPitch = -uPitchMax; |
kcherfou | 0:6038ca525e44 | 366 | } |
kcherfou | 0:6038ca525e44 | 367 | else { |
kcherfou | 0:6038ca525e44 | 368 | eIntPitch += ePitch; // Computing the integral sum |
kcherfou | 0:6038ca525e44 | 369 | } |
kcherfou | 0:6038ca525e44 | 370 | elastPitch = ePitch; |
kcherfou | 0:6038ca525e44 | 371 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 372 | // 4- PID yaw |
kcherfou | 0:6038ca525e44 | 373 | /* eYaw= desiredYaw - actualYaw; |
kcherfou | 0:6038ca525e44 | 374 | eDerYaw = eYaw - elastYaw ; // derivative of error |
kcherfou | 0:6038ca525e44 | 375 | uYaw = (KpYaw * eYaw) + (KdYaw * eDerYaw) + (KiYaw * eIntYaw); // computing the pitch control input |
kcherfou | 0:6038ca525e44 | 376 | // this part of the code limits the integrator windeup. an effect that takes place when |
kcherfou | 0:6038ca525e44 | 377 | if (uYaw > uYawMax){ |
kcherfou | 0:6038ca525e44 | 378 | uYaw = uYawMax; |
kcherfou | 0:6038ca525e44 | 379 | } |
kcherfou | 0:6038ca525e44 | 380 | else { |
kcherfou | 0:6038ca525e44 | 381 | eIntYaw += eYaw; // Computing the integral sum |
kcherfou | 0:6038ca525e44 | 382 | } |
kcherfou | 0:6038ca525e44 | 383 | elastYaw = eYaw; */ |
kcherfou | 0:6038ca525e44 | 384 | //------------------------------------------------------------------------------------------------------------- |
kcherfou | 0:6038ca525e44 | 385 | // Now that all pid control inputs are computed. send the PWM control input using the kinematic model to the ESC's |
kcherfou | 0:6038ca525e44 | 386 | v1= desiredThrottle - uPitch - uRoll - uYaw; //Calculate the pulse for esc 1 (front-right - CCW) |
kcherfou | 0:6038ca525e44 | 387 | v2= desiredThrottle + uPitch - uRoll + uYaw; //Calculate the pulse for esc 2 (rear-right - CW) |
kcherfou | 0:6038ca525e44 | 388 | v3= desiredThrottle + uPitch + uRoll - uYaw; //Calculate the pulse for esc 3 (rear-left - CCW) |
kcherfou | 0:6038ca525e44 | 389 | v4= desiredThrottle - uPitch + uRoll + uYaw; //Calculate the pulse for esc 4 (front-left - CW) |
kcherfou | 0:6038ca525e44 | 390 | |
kcherfou | 0:6038ca525e44 | 391 | if ( v1 > 2000 ){ |
kcherfou | 0:6038ca525e44 | 392 | v1=2000; |
kcherfou | 0:6038ca525e44 | 393 | } |
kcherfou | 0:6038ca525e44 | 394 | if ( v1 < 1000){ |
kcherfou | 0:6038ca525e44 | 395 | v1=1000; |
kcherfou | 0:6038ca525e44 | 396 | } |
kcherfou | 0:6038ca525e44 | 397 | if ( v2 > 2000 ){ |
kcherfou | 0:6038ca525e44 | 398 | v2=2000; |
kcherfou | 0:6038ca525e44 | 399 | } |
kcherfou | 0:6038ca525e44 | 400 | if ( v2 < 1000){ |
kcherfou | 0:6038ca525e44 | 401 | v2=1000; |
kcherfou | 0:6038ca525e44 | 402 | } |
kcherfou | 0:6038ca525e44 | 403 | if ( v3 > 2000 ){ |
kcherfou | 0:6038ca525e44 | 404 | v3=2000; |
kcherfou | 0:6038ca525e44 | 405 | } |
kcherfou | 0:6038ca525e44 | 406 | if ( v3 < 1000){ |
kcherfou | 0:6038ca525e44 | 407 | v3=1000; |
kcherfou | 0:6038ca525e44 | 408 | } |
kcherfou | 0:6038ca525e44 | 409 | if ( v4 > 2000 ){ |
kcherfou | 0:6038ca525e44 | 410 | v4=2000; |
kcherfou | 0:6038ca525e44 | 411 | } |
kcherfou | 0:6038ca525e44 | 412 | if ( v4 < 1000){ |
kcherfou | 0:6038ca525e44 | 413 | v4=1000; |
kcherfou | 0:6038ca525e44 | 414 | } |
kcherfou | 0:6038ca525e44 | 415 | } |
kcherfou | 0:6038ca525e44 | 416 | else{ // In the case where desired throttle is <1050 microseconds, we want to deactivate all motors |
kcherfou | 0:6038ca525e44 | 417 | v1= desiredThrottle; |
kcherfou | 0:6038ca525e44 | 418 | v2= desiredThrottle; |
kcherfou | 0:6038ca525e44 | 419 | v3= desiredThrottle; |
kcherfou | 0:6038ca525e44 | 420 | v4= desiredThrottle; |
kcherfou | 0:6038ca525e44 | 421 | } |
kcherfou | 0:6038ca525e44 | 422 | |
kcherfou | 0:6038ca525e44 | 423 | // outputing the necessary PWM value to the motors |
kcherfou | 0:6038ca525e44 | 424 | PWM_motor1.pulsewidth_us(abs(v1)); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 425 | PWM_motor2.pulsewidth_us(abs(v2)); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 426 | PWM_motor3.pulsewidth_us(abs(v3)); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 427 | PWM_motor4.pulsewidth_us(abs(v4)); //Set the Pulsewidth output |
kcherfou | 0:6038ca525e44 | 428 | |
kcherfou | 0:6038ca525e44 | 429 | } |
kcherfou | 0:6038ca525e44 | 430 | |
kcherfou | 0:6038ca525e44 | 431 | } |
kcherfou | 0:6038ca525e44 | 432 | |
kcherfou | 0:6038ca525e44 | 433 | |
kcherfou | 0:6038ca525e44 | 434 | // ******** Periodic Interrupt Handler 1******** |
kcherfou | 0:6038ca525e44 | 435 | void PeriodicInterruptISR(void){ |
kcherfou | 0:6038ca525e44 | 436 | osSignalSet(PiControlId,0x1); // Activate the signal, PiControl, with each periodic timer interrupt. |
kcherfou | 0:6038ca525e44 | 437 | } |
kcherfou | 0:6038ca525e44 | 438 |