Diff: main.cpp

Revision:

6:8cc6df266363

Parent:

5:c3caf8b83e6b

Child:

7:2ba30a0cdc16

--- a/main.cpp	Fri Dec 19 13:33:21 2014 +0000
+++ b/main.cpp	Wed Jan 07 13:44:34 2015 +0000
@@ -49,6 +49,10 @@
 char odczyt[20];
 char znak;
 char znak2;
+float Kp1,Kd1,Kp2,Kd2,Kp3,Kd3;
+float U1,U2,U3;
+float Om1,Om2,Om3,Om4;
+float wyp1,wyp2,wyp3,wyp4;
 
 double PWM1zad;
 double PWM2zad;
@@ -61,6 +65,7 @@
 
 void task1()
 {
+    myled = !myled;
     imu.readData(d);
     imu.filterData(d, D);
     off.offsetData(d,offsetGyro,o);
@@ -87,6 +92,61 @@
     pitch2 = pitch_data2.x1;
     roll2 = roll_data2.x1;
     
+    U1 = 0.0173*(Kp1*(-pitch2)+Kd1*(0-O[0]));
+    U2 = 0.0169*(Kp2*((-20*M_PI/180)-roll2)+Kd2*(0-O[1]));
+    U3 = 0.0333*(/*Kp3*((105*M_PI/180)-fYaw)+*/Kd3*(0-O[2])*180/M_PI);
+    
+    Om1 = 0.00576066*pow((PWM1zad-10000),2) - U2/0.000024768 + U3/0.000132 ;             //kwadraty
+    Om2 = 0.00576066*pow((PWM2zad-10000),2) + U1/0.000024768 - U3/0.000132 ;
+    Om3 = 0.00576066*pow((PWM3zad-10000),2) + U2/0.000024768 + U3/0.000132 ;
+    Om4 = 0.00576066*pow((PWM4zad-10000),2) - U1/0.000024768 - U3/0.000132 ;
+    
+    wyp1 = sqrt(Om1)*13.17523+10000;
+    wyp2 = sqrt(Om2)*13.17523+10000;
+    wyp3 = sqrt(Om3)*13.17523+10000;
+    wyp4 = sqrt(Om4)*13.17523+10000;
+    
+    /*
+    valPWM1 = (wyp1<10000) ? 10000 : (int)wyp1;
+    valPWM1 = (wyp1>20000) ? 20000 : (int)wyp1;
+    valPWM2 = (wyp2<10000) ? 10000 : (int)wyp2;
+    valPWM2 = (wyp2>20000) ? 20000 : (int)wyp2;
+    valPWM3 = (wyp3<10000) ? 10000 : (int)wyp3;
+    valPWM3 = (wyp3>20000) ? 20000 : (int)wyp3;
+    valPWM4 = (wyp4<10000) ? 10000 : (int)wyp4;
+    valPWM4 = (wyp4>20000) ? 20000 : (int)wyp4;*/
+    
+    
+    /*valPWM1 = (int)wyp1;
+    if (wyp1<10000) valPWM1=10000;
+    if (wyp1>20000) valPWM1=20000;
+    valPWM2 = (int)wyp2;
+    if (wyp2<10000) valPWM2=10000;
+    if (wyp2>20000) valPWM2=20000;
+    valPWM3 = (int)wyp3;
+    if (wyp3<10000) valPWM3=10000;
+    if (wyp3>20000) valPWM3=20000;
+    valPWM4 = (int)wyp4;
+    if (wyp4<10000) valPWM4=10000;
+    if (wyp4>20000) valPWM4=20000;*/
+    
+    
+    if(wyp1<=10001 || wyp1>40001) valPWM1=10000;
+    if(wyp1>=20000 && wyp1<40000) valPWM1=20000;
+    if(wyp1>10001 && wyp1<20000) valPWM1=(int)wyp1;
+
+    if(wyp2<=10001 & wyp2>40001) valPWM2=10000;
+    if(wyp2>=20000 & wyp2<40000) valPWM2=20000;
+    if(wyp2>10000 & wyp2<20000) valPWM2=(int)wyp2;
+
+    if(wyp3<=10001 | wyp3>40001) valPWM3=10000;
+    if(wyp3>=20000 & wyp3<40000) valPWM3=20000;
+    if(wyp3>10100 & wyp3<20000) valPWM3=(int)wyp3;
+
+    if(wyp4<=10001 & wyp4>40001) valPWM4=10000;
+    if(wyp4>=20000 & wyp4<40000) valPWM4=20000;
+    if(wyp4>10000 & wyp4<20000) valPWM4=(int)wyp4;    
+    
     
     //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);
@@ -97,6 +157,12 @@
     //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);
     
+    M1.pulsewidth_us(valPWM1);
+    M2.pulsewidth_us(valPWM2);
+    M3.pulsewidth_us(valPWM3);
+    M4.pulsewidth_us(valPWM4);
+    
+    
     myled = !myled;
 }
 
@@ -125,7 +191,7 @@
     sprintf(buff, "Hello: \n\r");
     pc.printf(buff);
     
-    //off.setOffsets(offsetGyro, pc, imu);
+    off.setOffsets(offsetGyro, pc, imu);
     
 
     
@@ -148,11 +214,28 @@
     M4.period_us(PWM_period);
     M4.pulsewidth_us(PWM4zad);
     
+    Kp1=0;
+    Kd1=0;
+    Kp2=0;
+    Kd2=0;
+    Kp3=0;
+    Kd3=0;
+    
+    
     while(1) {
-         
+    
+    myled2 = !myled2;
     //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;
+    sprintf(buff, "%f,%f,%f,%f,%f,%f,%f\n\r", -katybut*180/M_PI, roll2*180/M_PI, (O[1]*180/M_PI),valPWM1, valPWM2, valPWM3, valPWM4);
+    pc.printf(buff);
+    //sprintf(buff, "%f,%f\n\r", -katy*180/M_PI, roll*180/M_PI);
+    //pc.printf(buff);
+    //sprintf(buff, "%f,%f,%f,%f\n\r", valPWM1, valPWM2, valPWM3, valPWM4);
+    //pc.printf(buff);
+    
+    myled2 = !myled2;
+    
          
     
     if(pc.readable()){
@@ -162,52 +245,11 @@
             case 'p':
             sprintf(buff, "odczytany znak: %c\n\r",znak);
             pc.printf(buff);
-
-            i+=10;
-            break;
-            
-            case 'm':
-            i-=10;
-            break;
-            
-            case 'u':
-            i+=0.1;
-            break;
-            
-            case 'd':
-            i-=0.1;
             break;
             
-            
-            case 'q':
-            for(i=1000;i<1500;i++){
-            M1.pulsewidth_us(i);
-            wait(0.005);
-            }
-            break;
-            
-            case 'w':
-            for(i=1500;i>1000;i--){
-            M1.pulsewidth_us(i);
-            wait(0.005);
-            }
-            break;
-            case 'e':
-                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){
-                M1.pulsewidth_us(i);
-                wait(0.001);
-                }
-            break;
             }
             
         znak=0;
-        M1.pulsewidth_us(i);
         }
 
         
@@ -255,6 +297,13 @@
             break;
             
             case 'x':
+            sprintf(buff,"Nastawy: %f,%f\n\r",Kp2,Kd2);
+            pc.printf(buff);
+            wait(1.0f);
+            Kp1=0;
+            Kd1=0;
+            Kp2=0;
+            Kd2=0;
             PWM1zad=10000;
             PWM2zad=10000;
             PWM3zad=10000;
@@ -285,8 +334,24 @@
             pc.printf(buff);
             break;
             
+            case 'c':
+            Kd1-=0.5;
+            Kd2-=0.5;
+            break;
+            case 'd':
+            Kd1+=0.5;
+            Kd2+=0.5;
+            break;    
+            case 'e':
+            Kp1-=0.5;
+            Kp2-=0.5;
+            break;
+            case 'f':
+            Kp1+=0.5;
+            Kp2+=0.5;
+            break;                     
         }        
-       
         }
+    //myled2 = !myled2;
     }
 }
\ No newline at end of file