Koceila Cherfouh / Mbed OS QuadcopterDesign

This is the code to implement a quadcopter. PID controller tuning is necessary.

Dependencies:   PwmIn mbed MPU6050IMU

main.cpp@0:6038ca525e44, 2019-10-08 (annotated)

Committer:
kcherfou
Date:
Tue Oct 08 00:06:09 2019 +0000
Revision:
0:6038ca525e44
Child:
1:2021161b8fd2
This program is used to control a quadcopter.

Who changed what in which revision?

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