Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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
--- 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++) {
--- 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();
--- 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); }