File content as of revision 1:2021161b8fd2:

#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.
}