Diff: main.cpp

Revision:

0:89cf0851969b

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Jun 26 18:24:45 2018 +0000
@@ -0,0 +1,119 @@
+#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);
+    }
+}
+
+