Revision 7:f53b52e23818, committed 2017-02-13

Comitter:

open4416

Date:

Mon Feb 13 07:38:06 2017 +0000

Parent:

6:c2efb0a3a543

Commit message:

works fine on STM

Changed in this revision

diff -r c2efb0a3a543 -r f53b52e23818 main.cpp
--- a/main.cpp	Sun Sep 25 06:04:57 2016 +0000
+++ b/main.cpp	Mon Feb 13 07:38:06 2017 +0000
@@ -25,14 +25,6 @@
 DigitalOut  SPI_CSXM(D6,1);     //low for ACC/MAG enable
 SPI spi(D4, D5, D3);            //MOSI MISO SCLK
 
-//~~~Servo out~~~//
-PwmOut      Drive1(D8);         //control leg
-PwmOut      Drive2(D9);
-PwmOut      Drive3(D10);
-PwmOut      Drive4(D11);
-PwmOut      Drive5(D14);
-PwmOut      Drive6(D15);
-
 //~~~Serial~~~//
 Serial      pc(D1, D0);         //Serial reg(TX RX)
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
@@ -44,58 +36,6 @@
 Ticker  TT;                         //call a timer
 int     count = 0;                  //one second counter for extrenal led blink
 
-//~~~PWMreference~~~//
-const int   pwm_mid = 1450;         //+2080 for minimall lenght
-const int   PWM_base[1][6] = {      //desired six reference command at 0 deg
-    {pwm_mid+70,pwm_mid+10,pwm_mid+40,pwm_mid+60,pwm_mid+10,pwm_mid+30},
-//    {pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid,pwm_mid},      //only for debugging
-};
-int   PWM[1][6] = {                 //desired six reference command by PWM_base + PWM
-    {0,0,0,0,0,0},                  //transfer equaliyu 10us to 1 deg
-};
-
-//~~~RR varible~~~//
-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
-const float beta = 0.1309f;             //couple angle
-const float   A[3][6] = {                   //base cood
-    {cos(0.5f*alpha-beta),cos(0.5f*alpha+beta),cos(1.5f*alpha-beta),cos(1.5f*alpha+beta),cos(2.5f*alpha-beta),cos(2.5f*alpha+beta)},
-    {sin(0.5f*alpha-beta),sin(0.5f*alpha+beta),sin(1.5f*alpha-beta),sin(1.5f*alpha+beta),sin(2.5f*alpha-beta),sin(2.5f*alpha+beta)},
-    {0,0,0,0,0,0}
-};
-const float   B[3][6] = {                   //top cood(static)
-    {cos(beta),cos(alpha-beta),cos(alpha+beta),cos(2*alpha-beta),cos(2*alpha+beta),cos(3*alpha-beta)},
-    {sin(beta),sin(alpha-beta),sin(alpha+beta),sin(2*alpha-beta),sin(2*alpha+beta),sin(3*alpha-beta)},
-    {Z_dis,Z_dis,Z_dis,Z_dis,Z_dis,Z_dis}
-};
-float   C[3][6] = {                         //top cood(active)
-    {0,0,0,0,0,0},
-    {0,0,0,0,0,0},
-    {0,0,0,0,0,0}
-};
-float   VEC[1][6] = {                       //desired six reference command
-    {0,0,2.52,0,0,0},                       //X Y Z VEC[0][3] VEC[0][4] VEC[0][5]
-};
-float   L[1][6] = {                         //desired six reference command
-    {0,0,0,0,0,0},
-};
-float   Rtot[3][3] = {                      //RR'
-    {0,0,0},
-    {0,0,0},
-    {0,0,0}
-};
-
-//~~~stPF_tracle_R varible~~~//
-const float   L90 = 2.422;      //L under 90° (195 mm)
-const float   Larm = 0.4969;    //arm lenth   (40 mm) b
-const float   Llink = 0.8944;   //link lenth  (72 mm)
-float A_R = 0;
-float B_R = 0;
-float C_R = 0;
-
 //~~~IMU_SPI~~~//
 short low_byte = 0x00;              //buffer
 short high_byte = 0x00;
@@ -106,23 +46,6 @@
 float Ax = 0.0;
 float Ay = 0.0;
 float Az = 0.0;
-float gDIR[1][3] = {                //g vector's direction , required unitconstrain
-    {0.0,0.0,0.0},                  //X Y Z
-};
-float Bet = 0.002;                  //confidence of Acc data
-float gUnity = 0.0;
-float Gdx = 0.0;
-float Gdy = 0.0;
-float Gdz = 0.0;
-float Adx = 0.0;
-float Ady = 0.0;
-float Adz = 0.0;
-float Ele_phy = 0.0;                //estimation of top plate attitide
-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↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
@@ -131,14 +54,9 @@
 void    init_TIMER();           //set TT_main() rate
 void    TT_main();              //timebase function rated by TT
 void    init_IO();              //initialize IO state
+
 void    init_IMU();             //initialize IMU
-void    init_GXdrift();          //read GXdrift at start up
 void    read_IMU();             //read IMU data give raw data
-void    state_update();         //estimation of new attitude
-
-void    RR();                   //status generator
-void    stPF_lenth_uni();       //referenve generator
-void    stPF_travle_R();        //lenth to pwm pulse width
 
 float   lpf(float input, float output_old, float frequency);    //lpf discrete
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
@@ -149,12 +67,9 @@
 int main()
 {
     pc.baud(115200);            //set baud rate
-    wait_ms(1000);
 
     init_IO();                  //initialized value
     init_IMU();                 //initialize IMU
-    init_GXdrift();              //read GXdrift at start up
-    wait_ms(1000);
 
     init_TIMER();               //start TT_main
 
@@ -181,29 +96,10 @@
     count = count+1;                //one second counter
 
     read_IMU();                     //read IMU data give raw data
-    state_update();                 //estimation of new attitude
-
-    RR();                           //VEC generated
-    stPF_lenth_uni();               //L generated
-    stPF_travle_R();                //PWM generated
-
-    for(int i=0; i<6; i++) {            //safty constrain
-        PWM[0][i] =  constrain(PWM[0][i],700,2100);
-    }
 
-    Drive1.pulsewidth_us(PWM[0][0]);    //drive command
-    Drive2.pulsewidth_us(PWM[0][1]);
-    Drive3.pulsewidth_us(PWM[0][2]);
-    Drive4.pulsewidth_us(PWM[0][3]);
-    Drive5.pulsewidth_us(PWM[0][4]);
-    Drive6.pulsewidth_us(PWM[0][5]);
+    //for Serial-Oscilloscope
+    pc.printf("%.2f,%.2f,%.2f\r", Ax, Ay, Az);
 
-//for Serial-Oscilloscope
-//    pc.printf("%.3f\r", Bet);
-    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("%.3f,%.3f,%.3f\r", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
 }
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
@@ -254,39 +150,6 @@
 
 
 
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_G/Xdrift funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void init_GXdrift(void)              //initialize
-{
-    for(int i=0; i<1000; i++) {
-        read_IMU();                 //note GXdrift = 0 at this moment
-        gDIR[0][0] = gDIR[0][0] - Wx;
-        gDIR[0][1] = gDIR[0][1] - Wy;
-        gDIR[0][2] = gDIR[0][2] - Wz;
-        wait_ms(2);
-    }
-    Gdx = gDIR[0][0] /1000.0f;
-    Gdy = gDIR[0][1] /1000.0f;
-    Gdz = gDIR[0][2] /1000.0f;
-
-    for(int i=0; i<1000; i++) {
-        read_IMU();                 //note GXdrift = 0 at this moment
-        gDIR[0][0] = gDIR[0][0] - Ax;
-        gDIR[0][1] = gDIR[0][1] - Ay;
-        gDIR[0][2] = gDIR[0][2] - Az;
-        wait_ms(2);
-    }
-    Adx = gDIR[0][0] /1000.0f;
-    Ady = gDIR[0][1] /1000.0f;
-    Adz = gDIR[0][2] /1000.0f - 1.0f;
-//    pc.printf("%.4f,%.4f,%.4f\r", Gdx, Gdy, Gdz);
-    gDIR[0][0] = 0;
-    gDIR[0][1] = 0;
-    gDIR[0][2] = -1;
-}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_GXdrift funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-
-
-
 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
 void read_IMU(void)                 //read IMU data give raw data
 {
@@ -298,7 +161,7 @@
     Buff = high_byte << 8 |low_byte;
     SPI_CSG = 1;                    //end spi talking
 //    Wx = Buff * Gpx + Gdx;
-    Wx = lpf(Buff * Gpx + Gdx, Wx, 48.0f);
+    Wx = lpf(Buff * Gpx, Wx, 48.0f);
     //Wy
     SPI_CSG = 0;                    //start spi talking Wx
     spi.write(0xEA);                //read B11101010 read/multi/address
@@ -307,7 +170,7 @@
     Buff = high_byte << 8 |low_byte;
     SPI_CSG = 1;                    //end spi talking
 //    Wy = Buff * Gpy + Gdy;
-    Wy = lpf(Buff * Gpy + Gdy, Wy, 48.0f);
+    Wy = lpf(Buff * Gpy, Wy, 48.0f);
     //Wz
     SPI_CSG = 0;                    //start spi talking Wx
     spi.write(0xEC);                //read B11101100 read/multi/address
@@ -316,7 +179,7 @@
     Buff = high_byte << 8 |low_byte;
     SPI_CSG = 1;                    //end spi talking
 //    Wz = Buff * Gpz + Gdz;
-    Wz = lpf(Buff * Gpz + Gdz, Wz, 48.0f);
+    Wz = lpf(Buff * Gpz, Wz, 48.0f);
     //Ax
     SPI_CSXM = 0;                   //start spi talking Ax
     spi.write(0xE8);                //read B11101000 read/multi/address
@@ -324,7 +187,7 @@
     high_byte = spi.write(0);
     Buff = high_byte << 8 |low_byte;
     SPI_CSXM = 1;                   //end spi talking
-    Ax = lpf(Buff*Apx + Adx, Ax, 13.0f);
+    Ax = lpf(Buff * Apx, Ax, 13.0f);
     //Ay
     SPI_CSXM = 0;                   //start spi talking Ax
     spi.write(0xEA);                //read B11101010 read/multi/address
@@ -332,7 +195,7 @@
     high_byte = spi.write(0);
     Buff = high_byte << 8 |low_byte;
     SPI_CSXM = 1;                   //end spi talking
-    Ay = lpf(Buff*Apy + Ady, Ay, 13.0f);
+    Ay = lpf(Buff * Apy, Ay, 13.0f);
     //Az
     SPI_CSXM = 0;                   //start spi talking Ax
     spi.write(0xEC);                //read B11101100 read/multi/address
@@ -340,122 +203,12 @@
     high_byte = spi.write(0);
     Buff = high_byte << 8 |low_byte;
     SPI_CSXM = 1;                   //end spi talking
-    Az = lpf(Buff*Apz + Adz, Az, 13.0f);
+    Az = lpf(Buff * Apz, Az, 13.0f);
 }
 //↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
 
 
 
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void state_update(void)         //estimation of new attitude
-{
-    //pridict
-    gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
-    gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
-    gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;
-
-    //update
-    gDIR[0][0] = (1-Bet) * gDIR[0][0] + Bet * Ax;
-    gDIR[0][1] = (1-Bet) * gDIR[0][1] + Bet * Ay;
-    gDIR[0][2] = (1-Bet) * gDIR[0][2] + Bet * Az;
-
-    //nutralizing
-    gUnity = sqrt( gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2] );
-    for(int i=0; i<3; i++) {
-        gDIR[0][i] =  gDIR[0][i] / gUnity;
-    }
-
-    //transfer
-    Ele_phy = asin(gDIR[0][0]);
-    Til_phy = asin(-gDIR[0][1] / cos(Ele_phy));
-//    //test
-//    gDIR[0][0] = gDIR[0][0] + Wx*dt;
-//    gDIR[0][1] = gDIR[0][1] + Wy*dt;
-//    gDIR[0][2] = gDIR[0][2] + Wz*dt;
-}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-
-
-
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓RR funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void RR()                           //status generator
-{
-    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.090f,0.090f);
-    Til_phy_int =  constrain(Til_phy_int,-0.090f,0.090f);
-
-    Ele_control = -Kp*Ele_phy - Kd*Wy - Ki*Ele_phy_int - Kcon*Wx;
-    Til_control = -Kp*Til_phy - Kd*Wx - Ki*Til_phy_int - Kcon*Wy;
-
-    Ele_control =  constrain(Ele_control,-0.35f,0.35f);
-    Til_control =  constrain(Til_control,-0.35f,0.35f);
-
-    VEC[0][4] = lpf(Ele_control, VEC[0][4], 21.0f);
-    VEC[0][5] = lpf(Til_control, VEC[0][5], 21.0f);
-    VEC[0][2] = Z0 -Z_dis;
-}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of RR funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-
-
-
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_lenth_uni funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void stPF_lenth_uni()               //referenve generator
-{
-    Rtot[0][0] =    cos(VEC[0][4])*cos(VEC[0][3]);
-    Rtot[0][1] =  - cos(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][3])*sin(VEC[0][4])*sin(VEC[0][5]);
-    Rtot[0][2] =    sin(VEC[0][5])*sin(VEC[0][3]) + cos(VEC[0][5])*cos(VEC[0][3])*sin(VEC[0][4]);
-    Rtot[1][0] =    cos(VEC[0][4])*sin(VEC[0][3]);
-    Rtot[1][1] =    cos(VEC[0][5])*cos(VEC[0][3]) + sin(VEC[0][4])*sin(VEC[0][5])*sin(VEC[0][3]);
-    Rtot[1][2] =  - cos(VEC[0][3])*sin(VEC[0][5]) + cos(VEC[0][5])*sin(VEC[0][4])*sin(VEC[0][3]);
-    Rtot[2][0] =  - sin(VEC[0][4]);
-    Rtot[2][1] =    cos(VEC[0][4])*sin(VEC[0][5]);
-    Rtot[2][2] =    cos(VEC[0][4])*cos(VEC[0][5]);
-
-    for(int j=0; j<6; j++) {                    //reset C
-        for(int i=0; i<3; i++) {
-            C[i][j] = 0;
-        }
-    }
-
-    for(int i=0; i<6; i++) {                    //(RR.' * B)
-        for(int j=0; j<3; j++) {
-            for(int k=0; k<3; k++) {
-                C[j][i] = Rtot[j][k] * B[k][i] + C[j][i];
-            }
-        }
-    }
-
-    for(int j=0; j<6; j++) {                    //+ [X,Y,Z]
-        for(int i=0; i<3; i++) {
-            C[i][j] = C[i][j] + VEC[0][i];
-        }
-    }
-    for(int i=0; i<6; i++) {
-        float X = C[0][i]-A[0][i];
-        float Y = C[1][i]-A[1][i];
-        float Z = C[2][i]-A[2][i];
-        L[0][i] = sqrt(X*X+Y*Y+Z*Z);
-    }
-}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_lenth_uni funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-
-
-
-//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓stPF_travle_R funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
-void stPF_travle_R()                //lenth to pwm pulse width
-{
-    for(int i=0; i<6; i++) {
-        A_R = ( L[0][i] - L90 + sqrt( Llink*Llink - Larm*Larm ) ) /Larm;
-        B_R = Llink/Larm;
-        C_R = ( A_R*A_R - B_R*B_R + 1 ) /(A_R*2);
-        PWM[0][i] = -asin(C_R)*573 + PWM_base[0][i];
-    }
-}
-//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of stPF_travle_R funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//
-
-
-
 //↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓lpf funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
 float lpf(float input, float output_old, float frequency)
 {