SHENG-HEN HSIEH
/
LSM9DS0_STM32compatible
works fine on STM
Fork of Sample_manerine_SPI_LSM9DS0 by
Diff: main.cpp
- Revision:
- 5:2f0633d8fc20
- Parent:
- 4:b9dd320947ff
- Child:
- 6:c2efb0a3a543
diff -r b9dd320947ff -r 2f0633d8fc20 main.cpp --- 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↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//