File content as of revision 9:588b1618c710:

#include "mbed.h"
#include "Quadcopter.h"
#include "PID.h"
#include "MPU6050.h"
#include "Timer.h"

//defines the number of samples to be taken when calculating the offset for gyro and accelerometer
#define OFFSET_SAMPLES 50

//define how the accelerometer is placed on surface
#define X_AXIS 1
#define Y_AXIS 2
#define Z_AXIS 0

//ESC calibration values
#define MAX_CALIBRATE 1.0
#define MIN_CALIBRATE 0.0

//Just to remember which motor corresponds to which number...
#define FL   0    // Front left    
#define FR   1    // Front right
#define BL   2    // back left
#define BR   3    // back right

//input and output values for pitch
#define PITCH_IN_MIN -90.0
#define PITCH_IN_MAX 90.0
#define PITCH_OUT_MIN -0.1
#define PITCH_OUT_MAX 0.1

//input and output values for roll
#define ROLL_IN_MIN -90.0
#define ROLL_IN_MAX 90.0
#define ROLL_OUT_MIN -0.1
#define ROLL_OUT_MAX 0.1

//PID intervals/constants
#define Kc 1.0
#define Ti 0.00
#define Td 0.00
#define RATE 0.01

//--------------------------------ALL THE FUNCTION HEADERS-----------------------
float map(float x, float in_min, float in_max, float out_min, float out_max);   //might be a useful function. One is used inside Quadcopter library though
//---------------------------------------END-------------------------------------

Quadcopter quad (p21, p22, p23, p24);   //intance of the Quadcopter class
Serial bt(p13, p14);    //initialise a bluetooth module
Serial pc(USBTX, USBRX);                // tx, rx
MPU6050 mpu(p9, p10);                   //Also disables sleep mode
Timer timer;                            //need a timer to tell how much time passed from the last calculation

//put Kc, Ti, Td, interval for both pitch and roll PID models
PID pitchPID (Kc, Ti, Td, RATE);
PID rollPID (Kc, Ti, Td, RATE);

//***************************************STARTING MAIN FUNCTION*********************
int main() {
    bt.baud(9600); 
    pc.baud (115200);   //fast transmition speed...
    
    float val = 0.0;
    char buff;  //buffers when bad value received
    
    float pitchDiff;    //difference in pitch. Explained in PID library...
    float rollDiff;     //diference in roll
    
    float speed[4];     //speed for motors
    float actSpeed[4];  //actual speed of for all motors
    
    float accOffset[3]; //offset values
    float gyroOffset[3];
    float angle[3];     //total calculated angle

    float prevTime;     //previous time values will be given in the function 
    
    if (mpu.testConnection())   //just testing if things are working...
        pc.printf("MPU connection succeeded\n\r");
    else
        pc.printf("MPU connection failed\n\r");
    
    mpu.setAlpha(0.97);     //set Alpha coefficient for low/high pass filters
    
    quad.setLimits(MIN_CALIBRATE, MAX_CALIBRATE);   //set calibration limits for the system
    
    pc.printf ("Send c to calibrate and s to skip calibration.\n");
    while(1){
        if (!bt.readable()){
            if (!bt.scanf("%f", &val)){
                bt.scanf("%c", &buff);
            } else {
                if (val == 's'){
                    pc.printf ("Skipping calibration.\n");
                    break;  
                }               
                if (val == 'c'){
                    pc.printf ("Calibrating motors.\n");
                    quad.calibrate();   //calibrating motors  
                } else {
                    pc.printf ("Are you sure that you sent c/s?\n");     
                }
            }
        }   
    }
    
    pitchPID.setInputLimits (PITCH_IN_MIN, PITCH_IN_MAX);   //seting input and output limits for both pitch and roll
    pitchPID.setOutputLimits (PITCH_OUT_MIN, PITCH_OUT_MAX);

    rollPID.setInputLimits (ROLL_IN_MIN, ROLL_IN_MAX);
    rollPID.setOutputLimits (ROLL_OUT_MIN, ROLL_OUT_MAX);
    
    pitchPID.setBias(0.0);      //need to define middle point!!!! very important!!!
    rollPID.setBias(0.0);
    
    pitchPID.setMode(AUTO_MODE);    //start stabilising by puting AUTO mode
    rollPID.setMode(AUTO_MODE);
    
    //need to vary this one to move quadcopter
    pitchPID.setSetPoint (0.0);     //seting the middle point meaning that quadcopter is balancing in one place
    rollPID.setSetPoint (0.0); 

    mpu.getOffset(accOffset, gyroOffset, OFFSET_SAMPLES); //take some samples at the beginning to get an offset
    wait(0.1);                                            //wait to settle down
    
    timer.start();                  //will need timer to detect when was the last time the values were updated
    prevTime = timer.read();        //set previous timer value
    
    for (int i = 0; i < 4; i++){    //initialise speed to be 0.3
        speed[i] = 0.2;
    }   
    quad.run (speed); 
//-------------------------------------------START INFINITE LOOP-------------------------------------------------
    while(1) {
        if(bt.readable()){
            if (!bt.scanf("%f", &val)){
                bt.scanf("%c", &buff);
            } else {
                    pitchPID.setTunings(val, 0.0, 0.0);
                    rollPID.setTunings(val, 0.0, 0.0);  
                    pc.printf ("______________________SET______________________\n"); 
            }             
         }
               
            mpu.computeAngle (angle, accOffset, gyroOffset, timer.read()-prevTime);  // get angle using all these values
            
            rollPID.setInterval(timer.read()-prevTime);         //need to change the interval because don't know how much time passed
            pitchPID.setInterval(timer.read()-prevTime);
            
            prevTime = timer.read();    //get present time -> will be used later on as previous value
        
            rollPID.setProcessValue (angle[Y_AXIS]);    //take angle values, which correspond to pitch and roll and do processing
            pitchPID.setProcessValue (angle[X_AXIS]);
        
            pitchDiff = pitchPID.compute();     //compute the difference
            rollDiff = rollPID.compute();
            
        //    pc.printf ("pitchDiff=%0.4f, rollDiff=%0.4f\n", pitchDiff, rollDiff);
            quad.stabilise(speed, actSpeed, rollDiff, pitchDiff);   //stabilise the speed by giving out actual Speed value
            
            //print some values to check how thing work out
     //       pc.printf("x=%0.3f y=%0.3f z=%0.3f\n", angle[X_AXIS]+90, angle[Y_AXIS]+90, angle[Z_AXIS]);
     //       pc.printf("Speed_FL=%0.4f, Speed_FR=%0.4f, Speed_BL= %0.4f, Speed_BR=%0.4f\n", speed[FL], speed[FR], speed[BL], speed[BR]);
            pc.printf("ActSpeed_FL=%0.4f, ActSpeed_FR=%0.4f, ActSpeed_BL=%0.4f, ActSpeed_BR=%0.4f\n", actSpeed[FL], actSpeed[FR], actSpeed[BL], actSpeed[BR]);
            quad.run(actSpeed); //run the motors at the spesified speed actSpeed
            wait (0.01);
    }
}
//************************************************END MAIN FUNCTION********************************************************

//-----------------------------Mapping function-----------------------------
float map(float x, float in_min, float in_max, float out_min, float out_max){
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}