Koceila Cherfouh
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
--- /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.
+}
+