![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
gotta check ctrl algo - it gives negative value
Fork of BAE_hw_test1_3 by
Revision 11:6b00f3b1be17, committed 2015-03-04
- Comitter:
- sakthipriya
- Date:
- Wed Mar 04 17:15:15 2015 +0000
- Parent:
- 10:8bfa80bf4263
- Commit message:
- control algo - torque converted to moment
Changed in this revision
diff -r 8bfa80bf4263 -r 6b00f3b1be17 ACS.cpp --- a/ACS.cpp Sun Mar 01 08:26:04 2015 +0000 +++ b/ACS.cpp Wed Mar 04 17:15:15 2015 +0000 @@ -1,6 +1,7 @@ #include "ACS.h" #include "MPU3300.h" #include "pin_config.h" +#include "math.h" //PwmOut PWM1(PTD4); //Functions used to generate PWM signal //PWM output comes from pins p6 @@ -18,7 +19,9 @@ uint8_t trflag_mag; uint8_t trFlag; //ticker Flag for gyro uint8_t drFlag; //data-ready interrupt flag for gyro -float pwm; +float pwm1; +float pwm2; +float pwm3; //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------// void FUNC_ACS_GENPWM(float M[3]) @@ -39,7 +42,7 @@ ix = Mx * 0.3 ; //Moment and Current always have the linear relationship - ix = 0.554999; + // ix = 0.554999; if( ix>0&& ix < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% { DCx = 6*1000000*pow(ix,4) - 377291*pow(ix,3) + 4689.6*pow(ix,2) + 149.19*ix - 0.0008; @@ -78,10 +81,10 @@ { // printf("!!!!!!!!!!Error!!!!!!!!!"); } - pwm = PWM1; + pwm1 = PWM1; //DCx = 25; //PWM1 = 0.50; - printf("\n\r icx :%f pwm : %f \n\r",ix,pwm); + printf("\n\r icx :%f pwm : %f \n\r",ix,pwm1); float DCy = 0; //Duty cycle of Moment in x, y, z directions float iy = 0; //Current sent in x, y, z TR's @@ -128,8 +131,8 @@ // printf("!!!!!!!!!!Error!!!!!!!!!"); } - pwm = PWM2; - printf("\n\r icy :%f pwm : %f \n\r",iy,pwm); + pwm2 = PWM2; + printf("\n\r icy :%f pwm : %f \n\r",iy,pwm2); float DCz = 0; //Duty cycle of Moment in x, y, z directions @@ -177,6 +180,8 @@ { // printf("!!!!!!!!!!Error!!!!!!!!!"); } + pwm3 = PWM3; + printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3); printf("\n\rExited PWMGEN function\n\r"); } @@ -282,6 +287,17 @@ } } +/*------------------------------------------------------------------------------------------------------------------------------------------------------ +-------------------------------------------torque to moment conversion------------------------------------------------------------------------------------------*/ +void moment_calc (float tauc[3], float b[3], float moment[3]) +{ + float b1; + b1 = pow(b[0],2) + pow(b[1],2) +pow(b[2],2) ; + moment[0] = ((tauc[1]*b[2])-(tauc[2]*b[1]))/b1; + moment[1] = ((tauc[2]*b[0])-(tauc[0]*b[2]))/b1; + moment[2] = ((tauc[0]*b[1])-(tauc[1]*b[0]))/b1; + +} /*------------------------------------------------------------------------------------------------------------------------------------------------------ @@ -298,8 +314,8 @@ float den2; int i, j; //temporary variables float Mu[2], z[2], dv[2], v[2], u[2]; //outputs - //float tauc[3]; - float *tauc1; + // float tauc[3]; + //float *tauc1; float invJm[3][3]; float kmu2 = 0.07, gamma2 = 1.9e4, kz2 = 0.4e-2, kmu = 0.003, gamma = 5.6e4, kz = 0.1e-4; printf("\n\r Entered cntrl algo\n\r"); @@ -326,9 +342,7 @@ omega[2] = inputs[8];*/ /////////// Control Algorithm ////////////////////// // calculate norm b, norm db - tauc[0]=0; - tauc[1]=0; - tauc[2]=0; + tauc[0] =tauc[1] =tauc[2]=0 ; den = sqrt((b[0]*b[0]) + (b[1]*b[1]) + (b[2]*b[2])); den2 = (b[0]*db[0]) + (b[1]*db[1]) + (b[2]*db[2]); @@ -365,10 +379,11 @@ } inverse(Jm, invJm); // calculate cross(omega,J*omega)for(i=0;i<3;i++) - + for(i=0; i<3; i++) // for loop included after checking original code + { for(j=0;j<3;j++) bb[i] += omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j] - omega[(i+2)%3]*Jm[(i+1)%3][j]); - + } // calculate invJm*cross(omega,J*omega) store in d for(i=0;i<3;i++) {
diff -r 8bfa80bf4263 -r 6b00f3b1be17 ACS.h --- a/ACS.h Sun Mar 01 08:26:04 2015 +0000 +++ b/ACS.h Wed Mar 04 17:15:15 2015 +0000 @@ -5,6 +5,7 @@ float * FUNC_ACS_MAG_EXEC(void); void FUNC_ACS_MAG_INIT(); //void Read_data_acs() +void moment_calc (float* , float* , float* ); void FUNC_ACS_CNTRLALGO(float*,float*,float a[]); float * FUNC_ACS_EXEC_GYR(); void FUNC_ACS_INIT_GYR();
diff -r 8bfa80bf4263 -r 6b00f3b1be17 main.cpp --- a/main.cpp Sun Mar 01 08:26:04 2015 +0000 +++ b/main.cpp Wed Mar 04 17:15:15 2015 +0000 @@ -98,6 +98,7 @@ float *omega; float mag_field1[3]; float omega1[1]; + float tauc1[3]; float moment[3]; float *mnm_data; while(1) @@ -128,12 +129,19 @@ } if(acs_pflag == 1) { - FUNC_ACS_CNTRLALGO(mag_field1,omega1,moment); + FUNC_ACS_CNTRLALGO(mag_field1,omega1,tauc1); printf("\n\r control algo values "); for(int i=0; i<3; i++) { + printf("%f\t",tauc1[i]); + } + moment_calc (tauc1, mag_field1,moment); + printf("\n\r moment values "); + for(int i=0; i<3; i++) + { printf("%f\t",moment[i]); } + FUNC_ACS_GENPWM(moment); }