Diff: main.cpp

Revision:

5:c3caf8b83e6b

Parent:

4:a5b51a651db7

Child:

6:8cc6df266363

--- a/main.cpp	Fri Dec 19 12:13:43 2014 +0000
+++ b/main.cpp	Fri Dec 19 13:33:21 2014 +0000
@@ -1,55 +1,164 @@
 #include "mbed.h"
 #include "FastPWM.h"
+#include "kalman.h"
+#include "Offsets.h"
+#include "stdio.h"
+#include "IMU.h"
+
 #define PWM_period 2500
+#define M_PI 3.141592
+#define M_PI2 1.570796
+#define dt 0.005
+
+
+//kalman
+// Structs for containing filter data
+kalman_data pitch_data;
+kalman_data roll_data;
+
+kalman_data pitch_data2;
+kalman_data roll_data2;
+
+DigitalOut myled(PTA2);
+DigitalOut myled2(PTA1);
+Serial pc(USBTX, USBRX);
+Serial bluetooth(D1, D0);
+IMU imu(PTE25,PTE24);
+Offsets off;
+
+FastPWM M1(D10);
+FastPWM M2(D11);
+FastPWM M3(D12);
+FastPWM M4(D13);
 
 
-int valPWM1 = 1000;
-int valPWM2 = 1000;
-int valPWM3 = 1000;
-int valPWM4 = 1000;
+Ticker triger1; //przerwanie filtracji
+//Ticker triger2; //przerwanie wysyłania danych
 
-/*
-PwmOut motor1 (D10);
-PwmOut motor2 (D11);
-PwmOut motor3 (D12);
-PwmOut motor4 (D13);
-*/
-FastPWM motor1(D10);
-
-Serial pc(USBTX, USBRX);
-Serial bluetooth(D1, D0);
-
+float d[9];
+double D[9];
+float o[3];
+float O[3];
+char buff[160];
+float r,katx,katy;
+float rbut,katxbut,katybut;
+float pitch, roll;
+float pitch2, roll2;
+double i;
+float offsetGyro[3];
+char odczyt[20];
 char znak;
 char znak2;
-double i;
-char buff[60];
+
+double PWM1zad;
+double PWM2zad;
+double PWM3zad;
+double PWM4zad;
+double valPWM1;
+double valPWM2;
+double valPWM3;
+double valPWM4;
+
+void task1()
+{
+    imu.readData(d);
+    imu.filterData(d, D);
+    off.offsetData(d,offsetGyro,o);
+    off.offsetData2(D,offsetGyro,O);
+
+    r = sqrt(pow(d[3],2) + pow(d[4],2) + pow(d[5],2));
+    katx = acos(d[4]/r)-M_PI2;
+    katy = acos(d[3]/r)-M_PI2;
+    
+    rbut = sqrt(pow(D[3],2) + pow(D[4],2) + pow(D[5],2));
+    katxbut = acos(D[4]/rbut)-M_PI2;
+    katybut = acos(D[3]/rbut)-M_PI2;
+    
+    
+    //Filtr Kalmana
+    kalman_innovate(&pitch_data, katx, o[0]);
+    kalman_innovate(&roll_data, -katy, o[1]);
+    pitch = pitch_data.x1;
+    roll = roll_data.x1;
+    
+    //Filtr Kalmana butterworth 2nd
+    kalman_innovate(&pitch_data2, katxbut, O[0]);
+    kalman_innovate(&roll_data2, -katybut, O[1]);
+    pitch2 = pitch_data2.x1;
+    roll2 = roll_data2.x1;
+    
+    
+    //sprintf(buff, "%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI);
+    //pc.printf(buff);
+    //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy, roll, -katybut, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
+    //pc.printf(buff);    
+    //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", offsetGyro[0]*180/M_PI, offsetGyro[1]*180/M_PI, offsetGyro[2]*180/M_PI, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
+    //pc.printf(buff);
+    //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy, roll, -katybut, roll2, (d[0]*180/M_PI),(D[0]*180/M_PI),(d[1]*180/M_PI),(D[1]*180/M_PI),(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
+    //pc.printf(buff);
+    
+    myled = !myled;
+}
+
+
+void task2()
+{
+    /*
+    sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI, katx*180/M_PI, pitch*180/M_PI, katxbut*180/M_PI, pitch2*180/M_PI,(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
+    pc.printf(buff);
+    myled2 = !myled2;*/
+}
+
+        
+
 
 int main() {
     
     pc.baud(115200);
     bluetooth.baud(19200);
+    imu.init();
+    kalman_init(&pitch_data);
+    kalman_init(&roll_data);
+    kalman_init(&pitch_data2);
+    kalman_init(&roll_data2);
     
     sprintf(buff, "Hello: \n\r");
     pc.printf(buff);
     
-    motor1.period_us(PWM_period);
-    motor1.pulsewidth_us(valPWM1);
+    //off.setOffsets(offsetGyro, pc, imu);
+    
+
+    
+    triger1.attach(&task1, 0.005);
+    //triger2.attach(&task2, 0.005);
+    i=1000;
+    
     
-   /* motor1.period_us(PWM_period);
-    motor2.period_us(PWM_period);
-    motor3.period_us(PWM_period);
-    motor4.period_us(PWM_period);
-    */
-    i=1000;
+    PWM1zad=10000;
+    PWM2zad=10000;
+    PWM3zad=10000;
+    PWM4zad=10000;
+    
+    M1.period_us(PWM_period);
+    M1.pulsewidth_us(PWM1zad);
+    M2.period_us(PWM_period);
+    M2.pulsewidth_us(PWM2zad);
+    M3.period_us(PWM_period);
+    M3.pulsewidth_us(PWM3zad);
+    M4.period_us(PWM_period);
+    M4.pulsewidth_us(PWM4zad);
     
     while(1) {
          
+    //sprintf(buff, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI, -katybut*180/M_PI, roll2*180/M_PI, katx*180/M_PI, pitch*180/M_PI, katxbut*180/M_PI, pitch2*180/M_PI,(o[0]*180/M_PI),(O[0]*180/M_PI),(o[1]*180/M_PI),(O[1]*180/M_PI));
+    //pc.printf(buff);
+    //myled2 = !myled2;
+         
     
     if(pc.readable()){
-        
         znak=pc.getc();
-        
         switch (znak){
+            
             case 'p':
             sprintf(buff, "odczytany znak: %c\n\r",znak);
             pc.printf(buff);
@@ -72,109 +181,112 @@
             
             case 'q':
             for(i=1000;i<1500;i++){
-            motor1.pulsewidth_us(i);
+            M1.pulsewidth_us(i);
             wait(0.005);
             }
             break;
             
             case 'w':
             for(i=1500;i>1000;i--){
-            motor1.pulsewidth_us(i);
+            M1.pulsewidth_us(i);
             wait(0.005);
             }
             break;
             case 'e':
-            for(i=1000;i<1500;i=i+0.1){
-            motor1.pulsewidth_us(i);
-            wait(0.001);
-            }
+                for(i=1000;i<1500;i=i+0.1){
+                M1.pulsewidth_us(i);
+                wait(0.001);
+                }
             break;
             case 'r':
-            for(i=1500;i>1000;i-=0.1){
-            motor1.pulsewidth_us(i);
-            wait(0.001);
-            }
+                for(i=1500;i>1000;i-=0.1){
+                M1.pulsewidth_us(i);
+                wait(0.001);
+                }
             break;
             }
             
-
-        motor1.pulsewidth_us(i);
+        znak=0;
+        M1.pulsewidth_us(i);
         }
-        
-        
-        
-        
+
         
         if(bluetooth.readable()){
         
         znak2=bluetooth.getc();
-        sprintf(buff, "Odczytany znak: %c\n\r",znak2);
-        pc.printf(buff);
         
-        /*switch (znak2){
+        switch (znak2){
             case 'a':
-            sprintf(buff, "odczytany znak: %c\n\r",znak2);
-            pc.printf(buff);
+            PWM1zad-=50;
+            PWM2zad-=50;
+            PWM3zad-=50;
+            PWM4zad-=50;
+            if(PWM1zad<10000){
+                PWM1zad=10000;
+                PWM2zad=10000;
+                PWM3zad=10000;
+                PWM4zad=10000;
+            }
+            //ustawianie
+            M1.pulsewidth_us(PWM1zad);
+            M2.pulsewidth_us(PWM2zad);
+            M3.pulsewidth_us(PWM3zad);
+            M4.pulsewidth_us(PWM4zad);
+            znak2=0;
             break;
             
             case 'b':
-            sprintf(buff, "odczytany znak: %c\n\r",znak2);
+            PWM1zad+=50;
+            PWM2zad+=50;
+            PWM3zad+=50;
+            PWM4zad+=50;
+            if(PWM1zad>=20000){
+                PWM1zad=20000;
+                PWM2zad=20000;
+                PWM3zad=20000;
+                PWM4zad=20000;
+            }
+            //ustawianie
+            M1.pulsewidth_us(PWM1zad);
+            M2.pulsewidth_us(PWM2zad);
+            M3.pulsewidth_us(PWM3zad);
+            M4.pulsewidth_us(PWM4zad);
+            znak2=0;
+            break;
+            
+            case 'x':
+            PWM1zad=10000;
+            PWM2zad=10000;
+            PWM3zad=10000;
+            PWM4zad=10000;
+            M1.pulsewidth_us(PWM1zad);
+            M2.pulsewidth_us(PWM2zad);
+            M3.pulsewidth_us(PWM3zad);
+            M4.pulsewidth_us(PWM4zad);
+            sprintf(buff,"Odlacz silniki\n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"5 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"4 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"3 \n\r");
+            pc.printf(buff);           
+            wait(1.0f);
+            sprintf(buff,"2 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"1 \n\r");
+            pc.printf(buff);
+            wait(1.0f);
+            sprintf(buff,"GO! \n\r");
             pc.printf(buff);
             break;
             
-            case 'u':
-            i+=0.1;
-            break;
-            
-            case 'd':
-            i-=0.1;
-            break;
-            
-            
-            case 'q':
-            for(i=1000;i<1500;i++){
-            motor1.pulsewidth_us(i);
-            wait(0.005);
-            }
-            break;
-            
-            case 'w':
-            for(i=1500;i>1000;i--){
-            motor1.pulsewidth_us(i);
-            wait(0.005);
-            }
-            break;
-            case 'e':
-            for(i=1000;i<1500;i=i+0.1){
-            motor1.pulsewidth_us(i);
-            wait(0.001);
-            }
-            break;
-            case 'r':
-            for(i=1500;i>1000;i-=0.1){
-            motor1.pulsewidth_us(i);
-            wait(0.001);
-            }
-            break;
-            }*/
-            
-
-        //motor1.pulsewidth_us(i);
+        }        
+       
         }
-        
-   /* for (int i = 1000; i < 1500;i++)
-     {
-     
-     valPWM1 = i;
-     valPWM3 = i;
-     valPWM2 = 2500 - i;
-     valPWM4 = 2500 - i;
-     
-    motor1.pulsewidth_ms(valPWM1); 
-    motor2.pulsewidth_ms(valPWM2); 
-    motor3.pulsewidth_ms(valPWM3); 
-    motor4.pulsewidth_ms(valPWM4); 
-       }*/
-       
     }
 }
\ No newline at end of file