Diff: main.cpp

Revision:

5:2f0633d8fc20

Parent:

4:b9dd320947ff

Child:

6:c2efb0a3a543

--- a/main.cpp	Wed Sep 14 05:57:55 2016 +0000
+++ b/main.cpp	Sat Sep 17 13:24:17 2016 +0000
@@ -5,7 +5,7 @@
 #define d2r 0.01745329f
 
 #define Rms 5000            //TT rate
-#define dt  0.003f
+#define dt  0.005f
 #define NN  200
 
 #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
@@ -50,8 +50,8 @@
 };
 
 //~~~RR varible~~~//
-float       Z0 = 2.45;                  //as mid point hight
-float       Z_dis = -0.087;             //move rotation center
+float       Z0 = 2.40;                  //as mid point hight
+float       Z_dis = -0.1;               //move rotation center
 
 //~~~stPF_lenth_uni varible~~~//
 const float alpha = 2.094f;             //pair angle
@@ -104,7 +104,7 @@
 float gDIR[1][3] = {                //g vector's direction , required unitconstrain
     {0.0,0.0,0.0},                  //X Y Z
 };
-float Bet = 0.005;                  //confidence of Acc data
+float Bet = 0.002;                  //confidence of Acc data
 float gUnity = 0.0;
 float Gdx = 0.0;
 float Gdy = 0.0;
@@ -113,6 +113,8 @@
 float Til_phy = 0.0;
 float Ele_phy_int = 0.0;
 float Til_phy_int = 0.0;
+float Ele_control = 0.0;
+float Til_control = 0.0;
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
@@ -178,7 +180,7 @@
     stPF_travle_R();                //PWM generated
 
     for(int i=0; i<6; i++) {            //safty constrain
-        PWM[0][i] =  constrain(PWM[0][i],725,2025);
+        PWM[0][i] =  constrain(PWM[0][i],700,2100);
     }
 
     Drive1.pulsewidth_us(PWM[0][0]);    //drive command
@@ -190,9 +192,9 @@
 
 //for Serial-Oscilloscope
 //    pc.printf("%.3f\r", Bet);
-    pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy);
+//    pc.printf("%.3f,%.3f\r", Ele_phy, Til_phy);
 //    pc.printf("%.2f,%.2f\r", VEC[0][4], VEC[0][5]);
-//    pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az);
+    pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az);
 //    pc.printf("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
 }
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
@@ -277,7 +279,7 @@
     Buff = high_byte << 8 |low_byte;
     SPI_CSG = 1;                    //end spi talking
 //    Wx = Buff * Gpx + Gdx;
-    Wx = lpf(Buff * Gpx + Gdx, Wx, 18.0f);
+    Wx = lpf(Buff * Gpx + Gdx, Wx, 25.0f);
     //Wy
     SPI_CSG = 0;                    //start spi talking Wx
     spi.write(0xEA);                //read B11101010 read/multi/address
@@ -286,7 +288,7 @@
     Buff = high_byte << 8 |low_byte;
     SPI_CSG = 1;                    //end spi talking
 //    Wy = Buff * Gpy + Gdy;
-    Wy = lpf(Buff * Gpy + Gdy, Wy, 18.0f);
+    Wy = lpf(Buff * Gpy + Gdy, Wy, 25.0f);
     //Wz
     SPI_CSG = 0;                    //start spi talking Wx
     spi.write(0xEC);                //read B11101100 read/multi/address
@@ -295,7 +297,7 @@
     Buff = high_byte << 8 |low_byte;
     SPI_CSG = 1;                    //end spi talking
 //    Wz = Buff * Gpz + Gdz;
-    Wz = lpf(Buff * Gpz + Gdz, Wz, 18.0f);
+    Wz = lpf(Buff * Gpz + Gdz, Wz, 25.0f);
     //Ax
     SPI_CSXM = 0;                   //start spi talking Ax
     spi.write(0xE8);                //read B11101000 read/multi/address
@@ -361,14 +363,18 @@
 {
     Ele_phy_int = Ele_phy_int + Ele_phy*dt;
     Til_phy_int = Til_phy_int + Til_phy*dt;
-    Ele_phy_int =  constrain(Ele_phy_int,-0.01f,0.01f);
-    Til_phy_int =  constrain(Til_phy_int,-0.01f,0.01f);
+    Ele_phy_int =  constrain(Ele_phy_int,-0.006f,0.006f);
+    Til_phy_int =  constrain(Til_phy_int,-0.006f,0.006f);
+
+    Ele_control = -3.5f*Ele_phy - 0.10f*Wy - 20.0f*Ele_phy_int;
+    Til_control = -3.5f*Til_phy - 0.10f*Wx - 20.0f*Til_phy_int;
 
-    VEC[0][4] = -9.5f*Ele_phy - 0.17f*Wy - 60.0f*Ele_phy_int;
-    VEC[0][5] = -9.5f*Til_phy - 0.17f*Wx - 60.0f*Til_phy_int;
+    Ele_control =  constrain(Ele_control,-0.35f,0.35f);
+    Til_control =  constrain(Til_control,-0.35f,0.35f);
 
-    VEC[0][4] =  constrain(VEC[0][4],-0.30f,0.30f);
-    VEC[0][5] =  constrain(VEC[0][5],-0.30f,0.30f);
+    VEC[0][4] = lpf(Ele_control, VEC[0][4], 20.0f);
+    VEC[0][5] = lpf(Til_control, VEC[0][5], 20.0f);
+    VEC[0][2] = Z0 -Z_dis;
 }
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//