SHENG-HEN HSIEH
works fine on STM
Fork of
Sample_manerine_SPI_LSM9DS0
by 
SHENG-HEN HSIEH
Diff: main.cpp
- Revision:
- 7:f53b52e23818
- Parent:
- 6:c2efb0a3a543
--- 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)
{