sakthi priya amirtharaj
/
BAE_hw_test1_5
with i2c
Fork of BAE_b4hw_test by
Diff: ACS.cpp
- Revision:
- 8:6d856d863537
- Parent:
- 6:1cdbda747f99
diff -r 603c2a89effc -r 6d856d863537 ACS.cpp --- a/ACS.cpp Fri Feb 27 19:06:04 2015 +0000 +++ b/ACS.cpp Wed Mar 04 18:14:01 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 pwm1; +float pwm2; +float pwm3; //--------------------------------TORQUE ROD--------------------------------------------------------------------------------------------------------------// void FUNC_ACS_GENPWM(float M[3]) @@ -26,6 +29,10 @@ printf("\n\rEnterd PWMGEN function\n\r\r"); + for(int i = 0 ; i<3;i++) + { + printf(" %f \t ",M[i]); + } float DCx = 0; //Duty cycle of Moment in x, y, z directions float ix = 0; //Current sent in x, y, z TR's float timep = 0.02 ; @@ -35,7 +42,7 @@ ix = Mx * 0.3 ; //Moment and Current always have the linear relationship - + // 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; @@ -62,8 +69,10 @@ PWM1.period(timep); PWM1 = DCx/100 ; } - else if(ix==0) + //printf("\n \r b4 %f ",ix); + else if(ix==0) { + printf("\n \r ix====0"); DCx = 75; PWM1.period(timep); PWM1 = DCx/100 ; @@ -72,8 +81,10 @@ { // printf("!!!!!!!!!!Error!!!!!!!!!"); } - //DCx = 25; - printf("\n\r icx :%f\n\r",ix); + pwm1 = PWM1; + //DCx = 25; + //PWM1 = 0.50; + 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 @@ -119,8 +130,13 @@ { // printf("!!!!!!!!!!Error!!!!!!!!!"); } + + 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 - float iz = 0; //Current sent in x, y, z TR's + float iz = 0; //Current sent in x, y, z TR's float Mz=M[2]; //Time period is set to 0.2s //Moment in x, y, z directions @@ -163,7 +179,9 @@ else { // printf("!!!!!!!!!!Error!!!!!!!!!"); - } + } + pwm3 = PWM3; + printf("\n\r icy :%f pwm : %f \n\r",iz,pwm3); printf("\n\rExited PWMGEN function\n\r"); } @@ -269,12 +287,23 @@ } } +/*------------------------------------------------------------------------------------------------------------------------------------------------------ +-------------------------------------------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; + +} /*------------------------------------------------------------------------------------------------------------------------------------------------------ -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ -float * FUNC_ACS_CNTRLALGO(float b[3],float omega[3]) +void FUNC_ACS_CNTRLALGO(float b[3],float omega[3],float tauc[3]) { float db[3]; /// inputs //initialization @@ -284,10 +313,12 @@ float den = 0; float den2; int i, j; //temporary variables - float Mu[2], z[2], dv[2], v[2], u[2], tauc[3] = {0, 0, 0}; //outputs + float Mu[2], z[2], dv[2], v[2], u[2]; //outputs + // 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("Entered cntrl algo\n\r"); + printf("\n\r Entered cntrl algo\n\r"); //structure parameters void inverse (float mat[3][3], float inv[3][3]); @@ -295,8 +326,8 @@ //functions ////////// Input from Matlab ////////////// - while(1) - { + // while(1) //removed assumin while is used coz of matlab + //{ /*getInput(inputs); //while(1) @@ -311,6 +342,8 @@ omega[2] = inputs[8];*/ /////////// Control Algorithm ////////////////////// // calculate norm b, norm db + 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]); @@ -324,8 +357,8 @@ { printf("\n\rreached here\n\r"); if(den!=0) - //b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required - ; + b[i]=b[i]/den; //there is a problem here. The code gets stuck here. Maf value is required + } // select kz, kmu, gamma @@ -346,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++) { @@ -375,14 +409,20 @@ // calculate inv(N) store in Jm inverse(invJm, Jm); // calculate tauc + printf("\n \r calculatin tauc"); for(i=0;i<3;i++) { + for(j=0;j<3;j++) tauc[i] += Jm[i][j]*bb[j]; + //printf(" %d \t",i); + //tauc1[i] = tauc[i]; + printf(" %f \t",tauc[i]); } - - return(tauc); - } + + //printf(" %f \n ", tauc[2]); + //return tauc; + } /////////// Output to Matlab ////////////////// /* for(i=0;i<3;i++) {