ACS.cpp

00001 /*------------------------------------------------------------------------------------------------------------------------------------------------------
00002 -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
00003 #include <mbed.h>
00004 #include <math.h>
00005 
00006 #include "pni.h" //pni header file
00007 #include "pin_config.h"
00008 #include "ACS.h"
00009 #include "EPS.h"
00010 
00011 //********************************flags******************************************//
00012 extern uint32_t BAE_STATUS;
00013 extern uint32_t BAE_ENABLE;
00014 extern uint8_t ACS_INIT_STATUS;
00015 extern uint8_t ACS_DATA_ACQ_STATUS;
00016 extern uint8_t ACS_ATS_STATUS;
00017 extern uint8_t ACS_MAIN_STATUS;
00018 extern uint8_t ACS_STATUS;
00019 
00020 extern uint8_t ACS_ATS_ENABLE;
00021 extern uint8_t ACS_DATA_ACQ_ENABLE;
00022 extern uint8_t ACS_STATE;
00023 
00024 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
00025 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
00026 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
00027 
00028 extern PwmOut PWM1; //x                         //Functions used to generate PWM signal 
00029 extern PwmOut PWM2; //y
00030 extern PwmOut PWM3; //z                         //PWM output comes from pins p6
00031 
00032 int g_err_flag_TR_x=0;       // setting x-flag to zero
00033 int g_err_flag_TR_y=0;       // setting y-flag to zero
00034 int g_err_flag_TR_z=0;       // setting z-flag to zero
00035 
00036 extern float data[6];
00037 extern BAE_HK_actual actual_data;
00038 
00039 
00040 //DigitalOut gpo1(PTC0); // enable of att sens2 switch
00041 //DigitalOut gpo2(PTC16); // enable of att sens switch
00042 
00043 
00044 Serial pc_acs(USBTX,USBRX); //for usb communication
00045 //CONTROL_ALGO
00046 
00047 float moment[3]; // Unit: Ampere*Meter^2
00048 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5};  // Unit: Tesla
00049 int flag_firsttime=1, controlmode, alarmmode=0;
00050 
00051 
00052 
00053 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode);
00054 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax);
00055 float max_array(float arr[3]);
00056 void inverse(float mat[3][3],float inv[3][3]);
00057 
00058 //CONTROLALGO PARAMETERS
00059 
00060 
00061 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
00062 {
00063     float b1[3];
00064     float omega1[3];
00065     b1[0] = b[0]/1000000.0;
00066     b1[1] = b[1]/1000000.0;
00067     b1[2] = b[2]/1000000.0;
00068     
00069     omega1[0] = omega[0]*3.14159/180;
00070     omega1[1] = omega[1]*3.14159/180;
00071     omega1[2] = omega[2]*3.14159/180;
00072     controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode);
00073     
00074 }
00075 void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode)
00076 {
00077     float db1[3]; // Unit: Tesla/Second
00078     float sampling_time=10; // Unit: Seconds. Digital Control law excuted once in 10 seconds
00079     float MmntMax=1.1; // Unit: Ampere*Meter^2
00080     float OmegaMax=1*3.1415/180.0; // Unit: Radians/Second
00081     float normalising_fact;
00082     float b1_copy[3], omega1_copy[3], db1_copy[3];
00083     int i;
00084     if(flag_firsttime==1)
00085         {
00086             for(i=0;i<3;i++)
00087         {
00088                 db1[i]=0; // Unit: Tesla/Second
00089         }
00090             flag_firsttime=0;
00091         }
00092     else
00093     {
00094         for(i=0;i<3;i++)
00095         {
00096             db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
00097         }
00098     }
00099     
00100         if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
00101     {
00102             alarmmode=0;
00103     }
00104         else if(max_array(omega1)>OmegaMax && alarmmode==0)
00105     {
00106             alarmmode=1;
00107     }
00108 
00109     for (i=0;i<3;i++)
00110     {
00111         b1_copy[i]=b1[i];
00112         db1_copy[i]=db1[i];
00113         omega1_copy[i]=omega1[i];
00114     }
00115 
00116     if(alarmmode==0)
00117         {
00118             controlmode=0;
00119             controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
00120         for (i=0;i<3;i++)
00121         {
00122             b1[i]=b1_copy[i];
00123             db1[i]=db1_copy[i];
00124             omega1[i]=omega1_copy[i];
00125         }
00126             if(max_array(moment)>MmntMax)
00127             {
00128                 controlmode=1;
00129                 controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
00130             for (i=0;i<3;i++)
00131             {
00132                 b1[i]=b1_copy[i];
00133                 db1[i]=db1_copy[i];
00134                 omega1[i]=omega1_copy[i];
00135             }
00136                 if(max_array(moment)>MmntMax)
00137                 {
00138                     normalising_fact=max_array(moment)/MmntMax;
00139                     for(i=0;i<3;i++)
00140                 {
00141                         moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
00142                 }
00143                 }
00144             }
00145         }
00146         else
00147         {   
00148             controlmode=1;
00149             controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
00150         for (i=0;i<3;i++)
00151         {
00152             b1[i]=b1_copy[i];
00153             db1[i]=db1_copy[i];
00154             omega1[i]=omega1_copy[i];
00155         }
00156             if(max_array(moment)>MmntMax)
00157             {
00158                 normalising_fact=max_array(moment)/MmntMax;
00159                 for(i=0;i<3;i++)
00160             {
00161                     moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
00162             }
00163             }
00164         
00165         }
00166     for (i=0;i<3;i++)
00167     {
00168         b_old[i]=b1[i];
00169     }
00170 }
00171 
00172 void inverse(float mat[3][3],float inv[3][3])
00173 {
00174     int i,j;
00175     float det=0;
00176     for(i=0;i<3;i++)
00177     { 
00178         for(j=0;j<3;j++)
00179         {
00180             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]);
00181         }
00182     }
00183     det+=(mat[0][0]*inv[0][0])+(mat[0][1]*inv[1][0])+(mat[0][2]*inv[2][0]);
00184     for(i=0;i<3;i++)
00185     {
00186         for(j=0;j<3;j++)
00187         {
00188             inv[i][j]/=det;
00189         }
00190     }
00191 }
00192 
00193 float max_array(float arr[3])
00194 {
00195     int i;
00196     float temp_max=fabs(arr[0]);
00197     for(i=1;i<3;i++)
00198     {
00199         if(fabs(arr[i])>temp_max)
00200         {
00201             temp_max=fabs(arr[i]);
00202         }
00203     }
00204     return temp_max;
00205 }
00206 
00207 
00208 void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax)
00209 {
00210     float bb[3]={0,0,0};
00211     float d[3]={0,0,0};
00212     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
00213     float den=0,den2;
00214     float bcopy[3];
00215     int i, j;//temporary variables
00216     float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0},Mmnt[3];//outputs
00217     float invJm[3][3];
00218     float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4,kdetumble=2000000;
00219     int infflag;   // Flag variable to check if the moment value is infinity or NaN
00220     
00221     if(controlmode1==0)
00222     {
00223         den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
00224         den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
00225         for(i=0;i<3;i++)
00226         {
00227             db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3)); // Normalized db. Hence the unit is Second^(-1)
00228         }
00229         for(i=0;i<3;i++)
00230         {
00231             b[i]/=den; // Mormalized b. Hence no unit.
00232         }
00233         if(b[2]>0.9 || b[2]<-0.9)
00234         {
00235             kz=kz2;
00236             kmu=kmu2;
00237             gamma=gamma2;
00238         }
00239         for(i=0;i<2;i++)
00240         {
00241             Mu[i]=b[i];
00242             v[i]=-kmu*Mu[i];
00243             dv[i]=-kmu*db[i];
00244             z[i]=db[i]-v[i];
00245             u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
00246         }
00247         inverse(Jm,invJm);
00248         for(i=0;i<3;i++)
00249         {
00250             for(j=0;j<3;j++)
00251             {
00252                 bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
00253             }
00254         }
00255         for(i=0;i<3;i++)
00256         {
00257             for(j=0;j<3;j++)
00258             {
00259                 d[i]+=bb[j]*invJm[i][j];
00260             }
00261         }
00262         bb[1]=u[0]-(d[1]*b[2])+(d[2]*b[1])-(omega[1]*db[2])+(omega[2]*db[1]);
00263         bb[2]=u[1]-(d[2]*b[0])+(d[0]*b[2])-(omega[2]*db[0])+(omega[0]*db[2]);
00264         bb[0]=0;
00265         for(i=0;i<3;i++)
00266         {
00267             d[i]=invJm[2][i];
00268             invJm[1][i]=-b[2]*invJm[1][i]+b[1]*d[i];
00269             invJm[2][i]=b[2]*invJm[0][i]-b[0]*d[i];
00270             invJm[0][i]=b[i];
00271         }
00272         inverse(invJm,Jm);
00273         for(i=0;i<3;i++)
00274         {
00275             for(j=0;j<3;j++)
00276             {
00277                 tauc[i]+=Jm[i][j]*bb[j]; // Unit: Newton*Meter^2
00278             }
00279         }
00280         for(i=0;i<3;i++)
00281         {
00282             bcopy[i]=b[i]*den;
00283         }
00284         for(i=0;i<3;i++)
00285         {
00286             Mmnt[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
00287             Mmnt[i]/=(den*den); // Unit: Ampere*Meter^2
00288         }
00289         infflag=0;
00290         for (i=0; i<3 && infflag==0; i++)
00291         {
00292             if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1)
00293                 infflag=1;
00294         }
00295         if (infflag==1)
00296         {
00297             for (i=0; i<3; i++)
00298                 Mmnt[i]=2*MmntMax;
00299         }
00300         
00301     }
00302     else if(controlmode1==1)
00303     {
00304         for(i=0;i<3;i++)
00305         {
00306             Mmnt[i]=-kdetumble*(b[(i+1)%3]*omega[(i+2)%3]-b[(i+2)%3]*omega[(i+1)%3]); // Unit: Ampere*Meter^2
00307         }
00308     }
00309     for(i=0;i<3;i++)
00310     {
00311         moment[i]=Mmnt[i]; // Unit: Ampere*Meter^2
00312     }
00313 }
00314 
00315 I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl  for the attitude sensors and battery gauge
00316 
00317 void FCTN_ACS_INIT(void); //initialization of registers happens
00318 void FCTN_ATS_DATA_ACQ(); //data is obtained
00319 void T_OUT(); //timeout function to stop infinite loop
00320 Timeout to; //Timeout variable to
00321 int toFlag; 
00322 
00323 int count =0; // Time for which the BAE uC is running (in seconds)
00324 void T_OUT()
00325 {
00326     toFlag=0; //as T_OUT function gets called the while loop gets terminated
00327 }
00328 
00329 
00330 //DEFINING VARIABLES
00331 char cmd[2];
00332 char raw_gyro[6];
00333 char raw_mag[6];
00334 char store,status;
00335 int16_t bit_data;
00336 float gyro_data[3], mag_data[3],combined_values[6];
00337 float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps
00338 float senstivity_mag  =32.768; //senstivity is obtained from 2^15/1000microtesla
00339 float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0};
00340 
00341 void  FCTN_ACS_INIT()
00342 {
00343     ACS_INIT_STATUS = 's';     //set ACS_INIT_STATUS flag
00344     //FLAG();
00345     pc_acs.printf("Attitude sensor init called \n \r");
00346     //FLAG();
00347     cmd[0]=RESETREQ;
00348     cmd[1]=BIT_RESREQ;
00349     i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up
00350     wait_ms(2000); //waiting for loading configuration file stored in EEPROM
00351     cmd[0]=SENTRALSTATUS;
00352     i2c.write(SLAVE_ADDR,cmd,1);
00353     i2c.read(SLAVE_ADDR_READ,&store,1);
00354     wait_ms(100);
00355     //to check whether EEPROM is uploaded
00356     switch((int)store) { 
00357         case(3): {
00358             break;
00359         }
00360         case(11): {
00361             break;
00362         }
00363         default: {
00364             cmd[0]=RESETREQ;
00365             cmd[1]=BIT_RESREQ;
00366             i2c.write(SLAVE_ADDR,cmd,2);
00367             wait_ms(2000);
00368         }
00369     }
00370     pc_acs.printf("Sentral Status is %x\n \r",(int)store);
00371     cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors
00372     cmd[1]=BIT_RUN_ENB;
00373     i2c.write(SLAVE_ADDR,cmd,2);
00374     wait_ms(100);
00375     cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer
00376     cmd[1]=BIT_MAGODR;
00377     i2c.write(SLAVE_ADDR,cmd,2);
00378     wait_ms(100);
00379     cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
00380     cmd[1]=BIT_GYROODR;
00381     i2c.write(SLAVE_ADDR,cmd,2);
00382     wait_ms(100);
00383     cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
00384     cmd[1]=0x00;
00385     i2c.write(SLAVE_ADDR,cmd,2);
00386     wait_ms(100);
00387     cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values
00388     cmd[1]=BIT_EVT_ENB;
00389     i2c.write(SLAVE_ADDR,cmd,2);
00390     wait_ms(100);
00391     ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
00392 }
00393 
00394 void FCTN_ATS_DATA_ACQ()
00395 {
00396     ACS_DATA_ACQ_STATUS = 1;        //set ACS_DATA_ACQ_STATUS flag for att sens 2
00397     if( ACS_ATS_ENABLE == 1)
00398     {
00399     FLAG();
00400     pc_acs.printf("attitude sensor execution called \n \r");
00401     toFlag=1; //toFlag is set to 1 so that it enters while loop
00402     to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated 
00403     while(toFlag) {
00404         cmd[0]=EVT_STATUS;
00405         i2c.write(SLAVE_ADDR,cmd,1);
00406         i2c.read(SLAVE_ADDR_READ,&status,1);
00407         wait_ms(100);
00408         pc_acs.printf("Event Status is %x\n \r",(int)status);
00409         //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take
00410         if(((int)status&40)==40) {
00411             cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x 
00412             i2c.write(SLAVE_ADDR,cmd,1);
00413             i2c.read(SLAVE_ADDR_READ,raw_gyro,6);
00414             cmd[0]=MAG_XOUT_H; //LSB of x
00415             i2c.write(SLAVE_ADDR,cmd,1);
00416             i2c.read(SLAVE_ADDR_READ,raw_mag,6);
00417         //    pc_acs.printf("\nGyro Values:\n");
00418             for(int i=0; i<3; i++) {
00419                 //concatenating gyro LSB and MSB to get 16 bit signed data values
00420                 bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; 
00421                 gyro_data[i]=(float)bit_data;
00422                 gyro_data[i]=gyro_data[i]/senstivity_gyro;
00423                 gyro_data[i]+=gyro_error[i];
00424     //            pc_acs.printf("%f\t",gyro_data[i]);
00425             }
00426        //     pc_acs.printf("\nMag Values:\n");
00427             for(int i=0; i<3; i++) {
00428                 //concatenating mag LSB and MSB to get 16 bit signed data values
00429                 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i];
00430                 mag_data[i]=(float)bit_data;
00431                 mag_data[i]=mag_data[i]/senstivity_mag;
00432                 mag_data[i]+=mag_error[i];
00433       //          pc_acs.printf("%f\t",mag_data[i]);
00434             }
00435             for(int i=0; i<3; i++) {
00436                // data[i]=gyro_data[i];
00437                 actual_data.AngularSpeed_actual[i] = gyro_data[i];
00438                 actual_data.Bvalue_actual[i] = mag_data[i];
00439                 //data[i+3]=mag_data[i];
00440             }
00441           //  return(combined_values); //returning poiter combined values
00442         } 
00443        //checking for the error
00444         else if (((int)status&2)==2) {
00445             FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error
00446         }
00447     }
00448     }
00449     else    //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
00450     {
00451        ACS_DATA_ACQ_STATUS = 'f';   
00452     }
00453     ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
00454 }
00455 
00456 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
00457 {
00458     printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
00459     
00460     float l_duty_cycle_x=0;    //Duty cycle of Moment in x direction
00461     float l_current_x=0;       //Current sent in x TR's
00462     float l_duty_cycle_y=0;    //Duty cycle of Moment in y direction
00463     float l_current_y=0;       //Current sent in y TR's
00464     float l_duty_cycle_z=0;    //Duty cycle of Moment in z direction
00465     float l_current_z=0;       //Current sent in z TR's
00466  
00467     
00468     for(int i = 0 ; i<3;i++)
00469     {
00470      //   printf(" %f \t ",Moment[i]);  // taking the moment values from control algorithm as inputs
00471     }
00472     
00473     //-----------------------------  x-direction TR  --------------------------------------------//
00474     
00475     
00476     float l_moment_x = Moment[0];         //Moment in x direction
00477     
00478     phase_TR_x = 1;  // setting the default current direction
00479     if (l_moment_x <0)
00480     {
00481         phase_TR_x = 0;    // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 
00482         l_moment_x = abs(l_moment_x);
00483     }
00484     
00485     l_current_x = l_moment_x * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00486     printf("current in trx is %f \r \n",l_current_x);
00487     if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00488     {
00489         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 
00490         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00491         PWM1.period(TIME_PERIOD);
00492         PWM1 = l_duty_cycle_x/100 ;
00493     }
00494     else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
00495     {
00496         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
00497         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00498         PWM1.period(TIME_PERIOD);
00499         PWM1 = l_duty_cycle_x/100 ;            
00500     }
00501     else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
00502     {
00503         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
00504         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00505         PWM1.period(TIME_PERIOD);
00506         PWM1 = l_duty_cycle_x/100 ;            
00507     }
00508     else if(l_current_x==0)
00509     {
00510         printf("\n \r l_current_x====0");
00511         l_duty_cycle_x = 0;      // default value of duty cycle
00512         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00513         PWM1.period(TIME_PERIOD);
00514         PWM1 = l_duty_cycle_x/100 ;            
00515     }
00516     else                                           //not necessary
00517     {
00518         g_err_flag_TR_x = 1;
00519     } 
00520          
00521     //------------------------------------- y-direction TR--------------------------------------//
00522     
00523      
00524     float l_moment_y = Moment[1];         //Moment in y direction
00525     
00526     phase_TR_y = 1;  // setting the default current direction
00527     if (l_moment_y <0)
00528     {
00529         phase_TR_y = 0;   //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high  
00530         l_moment_y = abs(l_moment_y);
00531     }
00532     
00533     
00534     l_current_y = l_moment_y * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00535     printf("current in try is %f \r \n",l_current_y);
00536     if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00537     {
00538         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 
00539         printf("DC for try is %f \r \n",l_duty_cycle_y);
00540         PWM2.period(TIME_PERIOD);
00541         PWM2 = l_duty_cycle_y/100 ;
00542     }
00543     else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
00544     {
00545         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
00546         printf("DC for try is %f \r \n",l_duty_cycle_y);
00547         PWM2.period(TIME_PERIOD);
00548         PWM2 = l_duty_cycle_y/100 ;            
00549     }
00550     else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
00551     {
00552         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
00553         printf("DC for try is %f \r \n",l_duty_cycle_y);
00554         PWM2.period(TIME_PERIOD);
00555         PWM2 = l_duty_cycle_y/100 ;            
00556     }        
00557     else if(l_current_y==0)
00558     {
00559         printf("\n \r l_current_y====0");
00560         l_duty_cycle_y = 0; // default value of duty cycle
00561         printf("DC for try is %f \r \n",l_duty_cycle_y);
00562         PWM2.period(TIME_PERIOD);
00563         PWM2 = l_duty_cycle_y/100 ;            
00564     }
00565     else                               // not necessary
00566     {
00567       g_err_flag_TR_y = 1;
00568     } 
00569              
00570     //----------------------------------------------- z-direction TR -------------------------//  
00571     
00572       
00573     float l_moment_z = Moment[2];         //Moment in z direction
00574     
00575     phase_TR_z = 1;   // setting the default current direction
00576     if (l_moment_z <0)
00577     {
00578         phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 
00579         l_moment_z = abs(l_moment_z);
00580     }
00581     
00582     
00583     l_current_z = l_moment_z * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00584      printf("current in trz is %f \r \n",l_current_z);
00585         if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00586     {
00587         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 
00588         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00589         PWM3.period(TIME_PERIOD);
00590         PWM3 = l_duty_cycle_z/100 ;
00591     }
00592     else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
00593     {
00594         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
00595         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00596         PWM3.period(TIME_PERIOD);
00597         PWM3 = l_duty_cycle_z/100 ;            
00598     }
00599     else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
00600     {
00601         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
00602         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00603         PWM3.period(TIME_PERIOD);
00604         PWM3 = l_duty_cycle_z/100 ;            
00605     }
00606     else if(l_current_z==0)
00607     {
00608         printf("\n \r l_current_z====0");
00609         l_duty_cycle_z = 0; // default value of duty cycle
00610         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00611         PWM3.period(TIME_PERIOD);
00612         PWM3 = l_duty_cycle_z/100 ;            
00613     }
00614     else                               // not necessary
00615     {
00616         g_err_flag_TR_z = 1;
00617     }   
00618     
00619     //-----------------------------------------exiting the function-----------------------------------//
00620     
00621     printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
00622  
00623 }
00624 
00625 
00626 /*void FCTN_ACS_GENPWM_MAIN(float Moment[3])
00627 {
00628     printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
00629     
00630     float l_duty_cycle_x=0;    //Duty cycle of Moment in x direction
00631     float l_current_x=0;       //Current sent in x TR's
00632     float l_duty_cycle_y=0;    //Duty cycle of Moment in y direction
00633     float l_current_y=0;       //Current sent in y TR's
00634     float l_duty_cycle_z=0;    //Duty cycle of Moment in z direction
00635     float l_current_z=0;       //Current sent in z TR's
00636  
00637     
00638     for(int i = 0 ; i<3;i++)
00639     {
00640        printf("pwm %f \t ",Moment[i]);  // taking the moment values from control algorithm as inputs
00641     }
00642     
00643     //-----------------------------  x-direction TR  --------------------------------------------//
00644     
00645     
00646     float l_moment_x = Moment[0];         //Moment in x direction
00647     
00648     phase_TR_x = 1;  // setting the default current direction
00649     if (l_moment_x <0)
00650     {
00651         phase_TR_x = 0;    // if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 
00652         l_moment_x = abs(l_moment_x);
00653     }
00654     
00655     l_current_x = l_moment_x * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00656     pc_acs.printf("current in trx is %f \r \n",l_current_x);
00657     if( l_current_x>0 && l_current_x < 0.006 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00658     {
00659         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 
00660         pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
00661         PWM1.period(TIME_PERIOD);
00662         PWM1 = l_duty_cycle_x/100 ;
00663     }
00664     else if( l_current_x >= 0.006 && l_current_x < 0.0116)
00665     { 
00666         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
00667         pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
00668         PWM1.period(TIME_PERIOD);
00669         PWM1 = l_duty_cycle_x/100 ;             
00670     }
00671     else if (l_current_x >= 0.0116 && l_current_x < 0.0624)
00672     {
00673         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
00674         pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
00675         PWM1.period(TIME_PERIOD);
00676         PWM1 = l_duty_cycle_x/100 ;            
00677     }
00678     else if(l_current_x >= 0.0624 && l_current_x < 0.555)
00679     {
00680         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
00681         pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
00682         PWM1.period(TIME_PERIOD);
00683         PWM1 = l_duty_cycle_x/100 ;            
00684     }
00685     else if(l_current_x==0)
00686     {
00687         printf("\n \r l_current_x====0");
00688         l_duty_cycle_x = 0;      // default value of duty cycle
00689         pc_acs.printf("DC for trx is %f \r \n",l_duty_cycle_x);
00690         PWM1.period(TIME_PERIOD);
00691         PWM1 = l_duty_cycle_x/100 ;            
00692     }
00693     else                                           //not necessary
00694     {
00695         g_err_flag_TR_x = 1;
00696     } 
00697          
00698     //------------------------------------- y-direction TR--------------------------------------//
00699     
00700      
00701     float l_moment_y = Moment[1];         //Moment in y direction
00702     
00703     phase_TR_y = 1;  // setting the default current direction
00704     if (l_moment_y <0)
00705     {
00706         phase_TR_y = 0;   //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high  
00707         l_moment_y = abs(l_moment_y);
00708     }
00709     
00710     
00711     l_current_y = l_moment_y * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00712      pc_acs.printf("current in try is %f \r \n",l_current_y);
00713     if( l_current_y>0 && l_current_y < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
00714     {
00715         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
00716         pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
00717         PWM2.period(TIME_PERIOD);
00718         PWM2 = l_duty_cycle_y/100 ;
00719     }
00720     else if( l_current_y >= 0.006 && l_current_y < 0.0116)
00721     { 
00722         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
00723         pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
00724         PWM2.period(TIME_PERIOD);
00725         PWM2 = l_duty_cycle_y/100 ;             
00726     }
00727     else if (l_current_y >= 0.0116&& l_current_y < 0.0624)
00728     {
00729         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
00730         pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
00731         PWM2.period(TIME_PERIOD);
00732         PWM2 = l_duty_cycle_y/100 ;            
00733     }
00734     else if(l_current_y >= 0.0624 && l_current_y < 0.555)
00735     {
00736         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
00737         pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
00738         PWM2.period(TIME_PERIOD);
00739         PWM2 = l_duty_cycle_y/100 ;            
00740     }
00741     else if(l_current_y==0)
00742     {
00743         printf("\n \r l_current_y====0");
00744         l_duty_cycle_y = 0; // default value of duty cycle
00745         pc_acs.printf("DC for try is %f \r \n",l_duty_cycle_y);
00746         PWM2.period(TIME_PERIOD);
00747         PWM2 = l_duty_cycle_y/100 ;            
00748     }
00749     else                               // not necessary
00750     {
00751       g_err_flag_TR_y = 1;
00752     } 
00753              
00754     //----------------------------------------------- z-direction TR -------------------------//  
00755     
00756       
00757     float l_moment_z = Moment[2];         //Moment in z direction
00758     
00759     phase_TR_z = 1;   // setting the default current direction
00760     if (l_moment_z <0)
00761     {
00762         phase_TR_z = 0; //if the moment value is negative, we send the abs value of corresponding current in opposite direction by setting the phase pin high 
00763         l_moment_z = abs(l_moment_z);
00764     }
00765     
00766     
00767     l_current_z = l_moment_z * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00768      pc_acs.printf("current in trz is %f \r \n",l_current_z);
00769     if( l_current_z>0 && l_current_z < 0.006 )//Current and Duty cycle have the linear relationship between 1% and 100%
00770     {
00771         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
00772         pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
00773         PWM3.period(TIME_PERIOD);
00774         PWM3 = l_duty_cycle_z/100 ;
00775     }
00776     else if( l_current_z >= 0.006 && l_current_z < 0.0116)
00777     { 
00778         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
00779         pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
00780         PWM3.period(TIME_PERIOD);
00781         PWM3 = l_duty_cycle_z/100 ;             
00782     }
00783     else if (l_current_z >= 0.0116 && l_current_z < 0.0624)
00784     {
00785         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
00786         pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
00787         PWM3.period(TIME_PERIOD);
00788         PWM3 = l_duty_cycle_z/100 ;            
00789     }
00790     else if(l_current_z >= 0.0624 && l_current_z < 0.555)
00791     {
00792         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
00793         pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
00794         PWM3.period(TIME_PERIOD);
00795         PWM3 = l_duty_cycle_z/100 ;            
00796     }
00797     else if(l_current_z==0)
00798     {
00799         printf("\n \r l_current_z====0");
00800         l_duty_cycle_z = 0; // default value of duty cycle
00801         pc_acs.printf("DC for trz is %f \r \n",l_duty_cycle_z);
00802         PWM3.period(TIME_PERIOD);
00803         PWM3 = l_duty_cycle_z/100 ;            
00804     }
00805     else                               // not necessary
00806     {
00807         g_err_flag_TR_z = 1;
00808     }   
00809     
00810     //-----------------------------------------exiting the function-----------------------------------//
00811     
00812     printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
00813  
00814 }*/
00815 
00816 
00817