File content as of revision 0:89cf0851969b:

#include "mbed.h"
#include "math.h"
#include "rtos.h"
#include "Servo.h"
#include "CID10DOF.h" 
#include <string> 
#include "stdio.h"
#include "MathFuncs.h"


#define double_SIZE sizeof(double)


Serial pc(USBTX, USBRX);
Serial telem(PC_0, PC_1);
CID10DOF CID10DOF(D14,D15,D2,D4,D5,D7);
AnalogIn Analog_I(A0); 
DigitalOut led1(D6);
DigitalOut led2(D3);
Timer t;

double bias[3] = {0.0,0.0,0.0}, setpoint[3]= {0.0,0.0,0.0}, speed[4], actSpeed[4];
double ypr[4],M[5],Diff[3];

 
void getData(void const *argument);
void PID(void const *argument);
void motors(void const *argument);


int main() {
    
    pc.baud(115200);
    //telem.baud(57600); 
        
    led2 = !led2;  
    
    CID10DOF.MPU9250init();
    CID10DOF.PID_Config(setpoint, bias);
    CID10DOF.Calibrate_Motors();
    
     for (int i = 0; i < 4; i++){    //initialise speed to be 0.3
        speed[i] =  MIN_TRHUST;
    } 
    
    led2 = !led2;  
    
    Thread thread3(getData, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
    Thread thread4(PID, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
    Thread thread5(motors, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
    
    t.start();

    while(1) {
       
       led1 = !led1;
       Thread::wait(17);
       //pc.printf("Y:%f,P:%f,R:%f,PID_P:%.5f,PID_R:%.5f,M1:%f,M2:%f \n\r",ypr[0],ypr[1],ypr[2],Diff[1],Diff[2],actSpeed[0],actSpeed[1]);
       pc.printf("%f, %f, %f, %f, %f, %f\n\r",ypr[1],ypr[2],0.0,t.read(),ypr[1]-setpoint[1],setpoint[1]);
       
       if(t>10){
           setpoint[1] = 5.0;
       }
       
       if(t>15){
           setpoint[1] = 0.0;
       }
       
       if(t>20){
           setpoint[1] = -5;
       }
       
       if(t>25){
           setpoint[1] = 0.0;
       }
       
    }
       
}


void PID(void const *argument)
{
    while(true){
        
        CID10DOF.PID_Run(ypr, setpoint, Diff);
        
        actSpeed[0] =  MIN_TRHUST - Diff[2] ;
        actSpeed[1] =  MIN_TRHUST + Diff[2] ;
             
        if(actSpeed[0]<  MIN_TRHUST){actSpeed[0] =  MIN_TRHUST;}
        if(actSpeed[0]> 1.0){actSpeed[0] =  MAX_TRHUST;}
        if(actSpeed[1]<  MIN_TRHUST){actSpeed[1] = MIN_TRHUST;}
        if(actSpeed[1]> 1.0){actSpeed[1] =  MAX_TRHUST;}         
        Thread::wait(10);
    }
}



void getData(void const *argument)
{    
    while(true){
        
        CID10DOF.MPU9250ReadAxis(ypr);
        Thread::wait(5);
    }
}

void motors(void const *argument)
{    
    while(true){
        
        CID10DOF.run (actSpeed);
        Thread::wait(20);
    }
}