![](/media/cache/profiles/b61abc775fa6b65019e9f6b02d891041.jpg.50x50_q85.jpg)
Only GYRO/ACC's configuration done
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
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
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) {