Team Fox
To be debugged
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of
TFR_BAE_vr1_1_Debug153
by 
Bragadeesh S
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