To be debugged
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of TFR_BAE_vr1_1_Debug153 by
Diff: ACS.cpp
- Revision:
- 8:aad4f22221b1
- Parent:
- 6:036d08b62785
- Child:
- 9:f66c57a01e05
--- a/ACS.cpp Sat Jan 23 11:35:20 2016 +0000 +++ b/ACS.cpp Mon Jan 25 17:27:26 2016 +0000 @@ -48,128 +48,380 @@ float bcopy[3]; float moment[3]; ///////algo working well -void FCTN_ACS_CNTRLALGO(float b[3],float omega[3]) +void FCTN_ACS_CNTRLALGO (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode) { - float db[3]; - float bb[3]={0,0,0}; - float d[3]={0,0,0}; - float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}}; - float den=0,den2; - int i,j; //temporary variables - float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs - 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; - - //................. calculating db values........................... - if(ctrl_count!=0) - { - for(i=0;i<3;i++) - db[i]= (b[i]-bcopy[i])/10; - } + float db1[3]; // Unit: Tesla/Second + float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds + float MmntMax=1.1; // Unit: Ampere*Meter^2 + float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second + float normalising_fact; + float b1_copy[3], omega1_copy[3], db1_copy[3]; + int i, j; + if(flag_firsttime==1) + { + for(i=0;i<3;i++) + { + db1[i]=0; // Unit: Tesla/Second + } + flag_firsttime=0; + } else { for(i=0;i<3;i++) - db[i]= 0; - } - ctrl_count++; - //.................................................................. - printf("\n\r Entered cntrl algo\n\r"); - for(int i=0; i<3; i++) { - printf("%f\t",omega[i]); + db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second } - for(int i=0; i<3; i++) - { - printf("%f\t",b[i]); - } - - //.........................algo...................................... - 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]); - for(i=0;i<3;i++) + } + + if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1) { - db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); - //db[i]/=den*den*den; + alarmmode=0; } - for(i=0;i<3;i++) - { - b[i]/=den; - } - // select kz, kmu, gamma - if(b[0]>0.9||b[0]<-0.9) + else if(max_array(omega1)>OmegaMax && alarmmode==0) { - kz=kz2; - kmu=kmu2; - gamma=gamma2; + alarmmode=1; } - // calculate Mu, v, dv, z, u - for(i=0;i<2;i++) + + for (i=0;i<3;i++) { - Mu[i]=b[i+1]; - v[i]=-kmu*Mu[i]; - dv[i]=-kmu*db[i+1]; - z[i]=db[i+1]-v[i]; - u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); + b1_copy[i]=b1[i]; + db1_copy[i]=db1[i]; + omega1_copy[i]=omega1[i]; } - inverse(Jm,invJm); - for(i=0;i<3;i++) - { - 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]); - } - for(i=0;i<3;i++) + + if(alarmmode==0) + { + controlmode=0; + controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); + for (i=0;i<3;i++) + { + b1[i]=b1_copy[i]; + db1[i]=db1_copy[i]; + omega1[i]=omega1_copy[i]; + } + if(max_array(moment)>MmntMax) + { + controlmode=1; + controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); + for (i=0;i<3;i++) + { + b1[i]=b1_copy[i]; + db1[i]=db1_copy[i]; + omega1[i]=omega1_copy[i]; + } + if(max_array(moment)>MmntMax) + { + normalising_fact=max_array(moment)/MmntMax; + for(i=0;i<3;i++) + { + moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 + } + } + } + } + else + { + controlmode=1; + controlmodes(moment,b1,db1,omega1,controlmode,MmntMax); + for (i=0;i<3;i++) + { + b1[i]=b1_copy[i]; + db1[i]=db1_copy[i]; + omega1[i]=omega1_copy[i]; + } + if(max_array(moment)>MmntMax) + { + normalising_fact=max_array(moment)/MmntMax; + for(i=0;i<3;i++) + { + moment[i]/=normalising_fact; // Unit: Ampere*Meter^2 + } + } + + } + for (i=0;i<3;i++) { - for(j=0;j<3;j++) - d[i]+=bb[j]*invJm[i][j]; - } - bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]); - bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]); - bb[0]=0; - for(i=0;i<3;i++) - { - d[i]=invJm[1][i]; - invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i]; - invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i]; - invJm[0][i]=b[i]; + b_old[i]=b1[i]; } - inverse(invJm,Jm); - printf("\n \r calculating tauc"); - for(i=0;i<3;i++) - { - for(j=0;j<3;j++) - tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values - printf(" %f \t",tauc[i]); - } - //..........................tauc to moment conversion.......................... - printf("\n \r calculating moment"); - for(i=0;i<3;i++) - bcopy[i]=b[i]*den; - for(i=0;i<3;i++) - { - moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; - moment[i]/=den; - printf(" %f \t",moment[i]); - } - printf("\n\r exited control algo\n"); } -//..........................function to find inverse.................. -void inverse(float mat[3][3],float inv[3][3]) + +void inverse(float mat[3][3],float inv[3][3],int &singularity_flag) { int i,j; float det=0; for(i=0;i<3;i++) { for(j=0;j<3;j++) + { inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]); + } } det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]); - for(i=0;i<3;i++) - { - for(j=0;j<3;j++) - inv[i][j]/=det; + if (det==0) + { + singularity_flag=1; + } + else + { + singularity_flag=0; + for(i=0;i<3;i++) + { + for(j=0;j<3;j++) + { + inv[i][j]/=det; + } + } } } +float max_array(float arr[3]) +{ + int i; + float temp_max=fabs(arr[0]); + for(i=1;i<3;i++) + { + if(fabs(arr[i])>temp_max) + { + temp_max=fabs(arr[i]); + } + } + return temp_max; +} + + +void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax) +{ + float bb[3]={0,0,0}; + float d[3]={0,0,0}; + float Jm[3][3]={{0.2271,0.0014,-0.0026},{0.0014,0.2167,-0.004},{-0.0026,-0.004,0.2406}}; // Unit: Kilogram*Meter^2. Jm may change depending on the final satellite structure + float den=0,den2; + float bcopy[3]; + int i, j;//temporary variables + float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs + 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,kdetumble=2000000; + int singularity_flag=0; + + if(controlmode1==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]); + if (den==0) + { + singularity_flag=1; + } + if (singularity_flag==0) + { + for(i=0;i<3;i++) + { + db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1) + } + for(i=0;i<3;i++) + { + b[i]/=den; // Mormalized b. Hence no unit. + } + if(b[2]>0.9 || b[2]<-0.9) + { + kz=kz2; + kmu=kmu2; + gamma=gamma2; + } + for(i=0;i<2;i++) + { + Mu[i]=b[i]; + v[i]=-kmu*Mu[i]; + dv[i]=-kmu*db[i]; + z[i]=db[i]-v[i]; + u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); + } + inverse(Jm,invJm,singularity_flag); + for(i=0;i<3;i++) + { + 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]); + } + } + for(i=0;i<3;i++) + { + for(j=0;j<3;j++) + { + d[i]+=bb[j]*invJm[i][j]; + } + } + bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]); + bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]); + bb[0]=0; + for(i=0;i<3;i++) + { + d[i]=invJm[2][i]; + invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i]; + invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i]; + invJm[0][i]=b[i]; + } + inverse(invJm,Jm,singularity_flag); + if (singularity_flag==0) + { + for(i=0;i<3;i++) + { + for(j=0;j<3;j++) + { + tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2 + } + } + for(i=0;i<3;i++) + { + bcopy[i]=b[i]*den; + } + for(i=0;i<3;i++) + { + Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; + Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2 + } + } + } + if (singularity_flag==1) + { + for (i=0;i<3;i++) + { + Mmnt[i]=2*MmntMax; + } + } + } + else if(controlmode1==1) + { + for(i=0;i<3;i++) + { + Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2 + } + } + for(i=0;i<3;i++) + { + moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2 + } +} +//void FCTN_ACS_CNTRLALGO(float b[3],float omega[3]) +//{ +// float db[3]; +// float bb[3]={0,0,0}; +// float d[3]={0,0,0}; +// float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}}; +// float den=0,den2; +// int i,j; //temporary variables +// float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0}; //outputs +// 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; +// +// //................. calculating db values........................... +// if(ctrl_count!=0) +// { +// for(i=0;i<3;i++) +// db[i]= (b[i]-bcopy[i])/10; +// } +// else +// { +// for(i=0;i<3;i++) +// db[i]= 0; +// } +// ctrl_count++; +// //.................................................................. +// printf("\n\r Entered cntrl algo\n\r"); +// for(int i=0; i<3; i++) +// { +// printf("%f\t",omega[i]); +// } +// for(int i=0; i<3; i++) +// { +// printf("%f\t",b[i]); +// } +// +// //.........................algo...................................... +// 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]); +// for(i=0;i<3;i++) +// { +// db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); +// //db[i]/=den*den*den; +// } +// for(i=0;i<3;i++) +// { +// b[i]/=den; +// } +// // select kz, kmu, gamma +// if(b[0]>0.9||b[0]<-0.9) +// { +// kz=kz2; +// kmu=kmu2; +// gamma=gamma2; +// } +// // calculate Mu, v, dv, z, u +// for(i=0;i<2;i++) +// { +// Mu[i]=b[i+1]; +// v[i]=-kmu*Mu[i]; +// dv[i]=-kmu*db[i+1]; +// z[i]=db[i+1]-v[i]; +// u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma); +// } +// inverse(Jm,invJm); +// for(i=0;i<3;i++) +// { +// 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]); +// } +// for(i=0;i<3;i++) +// { +// for(j=0;j<3;j++) +// d[i]+=bb[j]*invJm[i][j]; +// } +// bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]); +// bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]); +// bb[0]=0; +// for(i=0;i<3;i++) +// { +// d[i]=invJm[1][i]; +// invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i]; +// invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i]; +// invJm[0][i]=b[i]; +// } +// inverse(invJm,Jm); +// printf("\n \r calculating tauc"); +// for(i=0;i<3;i++) +// { +// for(j=0;j<3;j++) +// tauc[i]+=Jm[i][j]*bb[j]; // calculating torque values +// printf(" %f \t",tauc[i]); +// } +// //..........................tauc to moment conversion.......................... +// printf("\n \r calculating moment"); +// for(i=0;i<3;i++) +// bcopy[i]=b[i]*den; +// for(i=0;i<3;i++) +// { +// moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3]; +// moment[i]/=den; +// printf(" %f \t",moment[i]); +// } +// printf("\n\r exited control algo\n"); +//} +//..........................function to find inverse.................. +//void inverse(float mat[3][3],float inv[3][3]) +//{ +// int i,j; +// float det=0; +// for(i=0;i<3;i++) +// { +// for(j=0;j<3;j++) +// inv[j][i]=(mat[(i+1)%3][(j+1)%3]*mat[(i+2)%3][(j+2)%3])-(mat[(i+2)%3][(j+1)%3]*mat[(i+1)%3][(j+2)%3]); +// } +// det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]); +// for(i=0;i<3;i++) +// { +// for(j=0;j<3;j++) +// inv[i][j]/=det; +// } +//} + I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge @@ -282,6 +534,9 @@ gyro_data[i]+=gyro_error[i]; // pc_acs.printf("%f\t",gyro_data[i]); } + gyro_data[0] = (gyro_data[0] + 1.52) * (3.1415/180); + gyro_data[1] = (gyro_data[1] - 2.44) * (3.1415/180); + gyro_data[2] = (gyro_data[2] - 1.06) * (3.1415/180); // in radians // pc_acs.printf("\nMag Values:\n"); for(int i=0; i<3; i++) { //concatenating mag LSB and MSB to get 16 bit signed data values @@ -291,6 +546,9 @@ mag_data[i]+=mag_error[i]; // pc_acs.printf("%f\t",mag_data[i]); } + mag_data[0] = ((mag_data[0] - 0.8089)/0.9566)/1000000; + mag_data[1] = ((mag_data[1] + 1.6722)/1.095)/1000000; + mag_data[2] = ((mag_data[2] + 4.7846 )/1.03)/1000000; // in Tesla for(int i=0; i<3; i++) { // data[i]=gyro_data[i]; actual_data.AngularSpeed_actual[i] = gyro_data[i]; @@ -314,7 +572,7 @@ void FCTN_ACS_GENPWM_MAIN(float Moment[3]) { - printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function + printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function float l_duty_cycle_x=0; //Duty cycle of Moment in x direction float l_current_x=0; //Current sent in x TR's @@ -324,7 +582,7 @@ float l_current_z=0; //Current sent in z TR's - for(int i = 0 ; i<3;i++) + //for(int i = 0 ; i<3;i++) { // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs } @@ -343,30 +601,23 @@ l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship pc_acs.printf("current in trx is %f \r \n",l_current_x); - if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100% + if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% { - l_duty_cycle_x = 6*1000000*pow(l_current_x,4) - 377291*pow(l_current_x,3) + 4689.6*pow(l_current_x,2) + 149.19*l_current_x - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation + l_duty_cycle_x = 3*10000000*pow(l_current_x,3)- 90216*pow(l_current_x,2) + 697.78*l_current_x - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; } - else if( l_current_x >= 0.006 && l_current_x < 0.0116) - { - l_duty_cycle_x = 1*100000000*pow(l_current_x,4) - 5*1000000*pow(l_current_x,3) + 62603*pow(l_current_x,2) - 199.29*l_current_x + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation - pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); - PWM1.period(TIME_PERIOD); - PWM1 = l_duty_cycle_x/100 ; - } - else if (l_current_x >= 0.0116 && l_current_x < 0.0624) + else if (l_current_x >= 0.0016 && l_current_x < 0.0171) { - l_duty_cycle_x = 212444*pow(l_current_x,4) - 33244*pow(l_current_x,3) + 1778.4*pow(l_current_x,2) + 120.91*l_current_x + 0.3878; // calculating upto 10% dutycycle by polynomial interpolation + l_duty_cycle_x = - 76880*pow(l_current_x,3) + 1280.8*pow(l_current_x,2) + 583.78*l_current_x + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; } - else if(l_current_x >= 0.0624 && l_current_x < 0.555) + else if(l_current_x >= 0.0171 && l_current_x < 0.1678) { - l_duty_cycle_x = 331.15*pow(l_current_x,4) - 368.09*pow(l_current_x,3) + 140.43*pow(l_current_x,2) + 158.59*l_current_x + 0.0338; // calculating upto 100% dutycycle by polynomial interpolation + l_duty_cycle_x = 275.92*pow(l_current_x,2) + 546.13*l_current_x + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; @@ -383,11 +634,12 @@ { g_err_flag_TR_x = 1; } + //------------------------------------- y-direction TR--------------------------------------// - float l_moment_y = Moment[1]; //Moment in y direction + float l_moment_y = Moment[1]; //Moment in y direction phase_TR_y = 1; // setting the default current direction if (l_moment_y <0) @@ -399,34 +651,27 @@ l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship pc_acs.printf("current in try is %f \r \n",l_current_y); - if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100% + if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% { - l_duty_cycle_y = 6*1000000*pow(l_current_y,4) - 377291*pow(l_current_y,3) + 4689.6*pow(l_current_y,2) + 149.19*l_current_y - 0.0008; // calculating upto 0.1% dutycycle by polynomial interpolation + l_duty_cycle_y = 3*10000000*pow(l_current_y,3)- 90216*pow(l_current_y,2) + 697.78*l_current_y - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } - else if( l_current_y >= 0.006 && l_current_y < 0.0116) - { - l_duty_cycle_y = 1*100000000*pow(l_current_y,4) - 5*1000000*pow(l_current_y,3) + 62603*pow(l_current_y,2) - 199.29*l_current_y + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation - pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); - PWM2.period(TIME_PERIOD); - PWM2 = l_duty_cycle_y/100 ; - } - else if (l_current_y >= 0.0116&& l_current_y < 0.0624) + else if (l_current_y >= 0.0016 && l_current_y < 0.0171) { - l_duty_cycle_y = 212444*pow(l_current_y,4) - 33244*pow(l_current_y,3) + 1778.4*pow(l_current_y,2) + 120.91*l_current_y + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation + l_duty_cycle_y = - 76880*pow(l_current_y,3) + 1280.8*pow(l_current_y,2) + 583.78*l_current_y + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } - else if(l_current_y >= 0.0624 && l_current_y < 0.555) + else if(l_current_y >= 0.0171 && l_current_y < 0.1678) { - l_duty_cycle_y = 331.15*pow(l_current_y,4) - 368.09*pow(l_current_y,3) + 140.43*pow(l_current_y,2) + 158.59*l_current_y + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation + l_duty_cycle_y = 275.92*pow(l_current_y,2) + 546.13*l_current_y + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; - } + } else if(l_current_y==0) { printf("\n \r l_current_y====0"); @@ -439,10 +684,9 @@ { g_err_flag_TR_y = 1; } - + //----------------------------------------------- z-direction TR -------------------------// - float l_moment_z = Moment[2]; //Moment in z direction phase_TR_z = 1; // setting the default current direction @@ -455,30 +699,23 @@ l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship pc_acs.printf("current in trz is %f \r \n",l_current_z); - if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100% + if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% { - l_duty_cycle_z = 6*1000000*pow(l_current_z,4) - 377291*pow(l_current_z,3) + 4689.6*pow(l_current_z,2) + 149.19*l_current_z - 0.0008;// calculating upto 0.1% dutycycle by polynomial interpolation + l_duty_cycle_z = 3*10000000*pow(l_current_z,3)- 90216*pow(l_current_z,2) + 697.78*l_current_z - 0.0048; // calculating upto 0.1% dutycycle by polynomial interpolation pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; } - else if( l_current_z >= 0.006 && l_current_z < 0.0116) - { - l_duty_cycle_z = 1*100000000*pow(l_current_z,4) - 5*1000000*pow(l_current_z,3) + 62603*pow(l_current_z,2) - 199.29*l_current_z + 0.7648;// calculating upto 1% dutycycle by polynomial interpolation - pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); - PWM3.period(TIME_PERIOD); - PWM3 = l_duty_cycle_z/100 ; - } - else if (l_current_z >= 0.0116 && l_current_z < 0.0624) + else if (l_current_z >= 0.0016 && l_current_z < 0.0171) { - l_duty_cycle_z = 212444*pow(l_current_z,4) - 33244*pow(l_current_z,3) + 1778.4*pow(l_current_z,2) + 120.91*l_current_z + 0.3878;// calculating upto 10% dutycycle by polynomial interpolation + l_duty_cycle_z = - 76880*pow(l_current_z,3) + 1280.8*pow(l_current_z,2) + 583.78*l_current_z + 0.0281; // calculating upto 10% dutycycle by polynomial interpolation pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; } - else if(l_current_z >= 0.0624 && l_current_z < 0.555) + else if(l_current_z >= 0.0171 && l_current_z < 0.1678) { - l_duty_cycle_z = 331.15*pow(l_current_z,4) - 368.09*pow(l_current_z,3) + 140.43*pow(l_current_z,2) + 158.59*l_current_z + 0.0338;// calculating upto 100% dutycycle by polynomial interpolation + l_duty_cycle_z = 275.92*pow(l_current_z,2) + 546.13*l_current_z + 0.5316; // calculating upto 100% dutycycle by polynomial interpolation pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; @@ -499,8 +736,8 @@ //-----------------------------------------exiting the function-----------------------------------// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function + } - \ No newline at end of file