main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "rtos.h"
00004 #include "Servo.h"
00005 #include "CID10DOF.h" 
00006 #include <string> 
00007 #include "stdio.h"
00008 #include "MathFuncs.h"
00009 
00010 
00011 #define double_SIZE sizeof(double)
00012 
00013 
00014 Serial pc(USBTX, USBRX);
00015 Serial telem(PC_0, PC_1);
00016 CID10DOF CID10DOF(D14,D15,D2,D4,D5,D7);
00017 AnalogIn Analog_I(A0); 
00018 DigitalOut led1(D6);
00019 DigitalOut led2(D3);
00020 Timer t;
00021 
00022 double bias[3] = {0.0,0.0,0.0}, setpoint[3]= {0.0,0.0,0.0}, speed[4], actSpeed[4];
00023 double ypr[4],M[5],Diff[3];
00024 
00025  
00026 void getData(void const *argument);
00027 void PID(void const *argument);
00028 void motors(void const *argument);
00029 
00030 
00031 int main() {
00032     
00033     pc.baud(115200);
00034     //telem.baud(57600); 
00035         
00036     led2 = !led2;  
00037     
00038     CID10DOF.MPU9250init();
00039     CID10DOF.PID_Config(setpoint, bias);
00040     CID10DOF.Calibrate_Motors();
00041     
00042      for (int i = 0; i < 4; i++){    //initialise speed to be 0.3
00043         speed[i] =  MIN_TRHUST;
00044     } 
00045     
00046     led2 = !led2;  
00047     
00048     Thread thread3(getData, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
00049     Thread thread4(PID, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
00050     Thread thread5(motors, NULL, osPriorityRealtime, DEFAULT_STACK_SIZE);
00051     
00052     t.start();
00053 
00054     while(1) {
00055        
00056        led1 = !led1;
00057        Thread::wait(17);
00058        //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]);
00059        pc.printf("%f, %f, %f, %f, %f, %f\n\r",ypr[1],ypr[2],0.0,t.read(),ypr[1]-setpoint[1],setpoint[1]);
00060        
00061        if(t>10){
00062            setpoint[1] = 5.0;
00063        }
00064        
00065        if(t>15){
00066            setpoint[1] = 0.0;
00067        }
00068        
00069        if(t>20){
00070            setpoint[1] = -5;
00071        }
00072        
00073        if(t>25){
00074            setpoint[1] = 0.0;
00075        }
00076        
00077     }
00078        
00079 }
00080 
00081 
00082 void PID(void const *argument)
00083 {
00084     while(true){
00085         
00086         CID10DOF.PID_Run(ypr, setpoint, Diff);
00087         
00088         actSpeed[0] =  MIN_TRHUST - Diff[2] ;
00089         actSpeed[1] =  MIN_TRHUST + Diff[2] ;
00090              
00091         if(actSpeed[0]<  MIN_TRHUST){actSpeed[0] =  MIN_TRHUST;}
00092         if(actSpeed[0]> 1.0){actSpeed[0] =  MAX_TRHUST;}
00093         if(actSpeed[1]<  MIN_TRHUST){actSpeed[1] = MIN_TRHUST;}
00094         if(actSpeed[1]> 1.0){actSpeed[1] =  MAX_TRHUST;}         
00095         Thread::wait(10);
00096     }
00097 }
00098 
00099 
00100 
00101 void getData(void const *argument)
00102 {    
00103     while(true){
00104         
00105         CID10DOF.MPU9250ReadAxis(ypr);
00106         Thread::wait(5);
00107     }
00108 }
00109 
00110 void motors(void const *argument)
00111 {    
00112     while(true){
00113         
00114         CID10DOF.run (actSpeed);
00115         Thread::wait(20);
00116     }
00117 }
00118 
00119