![](/media/cache/group/nine_tails_kurama_by_zubair43-d4zyf5s.jpg.50x50_q85.jpg)
PWM and ALGO updated.. PWM getting generated
Dependencies: FreescaleIAP mbed-rtos mbed
Fork of TFR_BAE_vr1_1working_finally_PWM_CTRLALGO_update by
ACS.cpp
- Committer:
- Bragadeesh153
- Date:
- 2016-04-07
- Revision:
- 14:ccdf8790a15e
- Parent:
- 13:75aa0104f5e6
File content as of revision 14:ccdf8790a15e:
/*------------------------------------------------------------------------------------------------------------------------------------------------------ -------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/ #include <mbed.h> #include <math.h> #include "pni.h" //pni header file #include "pin_config.h" #include "ACS.h" #include "EPS.h" //********************************flags******************************************// extern uint32_t BAE_STATUS; extern uint32_t BAE_ENABLE; extern char ACS_INIT_STATUS; extern char ACS_DATA_ACQ_STATUS; extern char ACS_ATS_STATUS; extern char ACS_MAIN_STATUS; extern char ACS_STATUS; extern char ACS_ATS_ENABLE; extern char ACS_DATA_ACQ_ENABLE; extern char ACS_STATE; DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod extern PwmOut PWM1; //x //Functions used to generate PWM signal extern PwmOut PWM2; //y extern PwmOut PWM3; //z //PWM output comes from pins p6 int g_err_flag_TR_x=0; // setting x-flag to zero int g_err_flag_TR_y=0; // setting y-flag to zero int g_err_flag_TR_z=0; // setting z-flag to zero extern float data[6]; extern BAE_HK_actual actual_data; //DigitalOut gpo1(PTC0); // enable of att sens2 switch //DigitalOut gpo2(PTC16); // enable of att sens switch Serial pc_acs(USBTX,USBRX); //for usb communication //CONTROL_ALGO float moment[3]; // Unit: Ampere*Meter^2 float b_old[3]={1.15e-5,-0.245e-5,1.98e-5}; // Unit: Tesla int flag_firsttime=1, controlmode, alarmmode=0; void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode); void controlmodes(float moment[3], float b[3], float db[3], float omega[3], int controlmode1, float MmntMax); float max_array(float arr[3]); void inverse(float mat[3][3],float inv[3][3]); //CONTROLALGO PARAMETERS void FCTN_ACS_CNTRLALGO(float b[3],float omega[3]) { float b1[3]; float omega1[3]; b1[0] = b[0]/1000000.0; b1[1] = b[1]/1000000.0; b1[2] = b[2]/1000000.0; omega1[0] = omega[0]*3.14159/180; omega1[1] = omega[1]*3.14159/180; omega1[2] = omega[2]*3.14159/180; controller (moment, b1, omega1, b_old, alarmmode, flag_firsttime, controlmode); } void controller (float moment[3], float b1[3], float omega1[3], float b_old[3], int &alarmmode, int &flag_firsttime, int &controlmode) { 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; 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++) { db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second } } if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1) { alarmmode=0; } else if(max_array(omega1)>OmegaMax && alarmmode==0) { alarmmode=1; } for (i=0;i<3;i++) { b1_copy[i]=b1[i]; db1_copy[i]=db1[i]; omega1_copy[i]=omega1[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++) { b_old[i]=b1[i]; } } 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; } } } 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 infflag; // Flag variable to check if the moment value is infinity or NaN 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]); 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); 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); 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 } infflag=0; for (i=0; i<3 && infflag==0; i++) { if (isinf(Mmnt[i])==1 || isnan(Mmnt[i])==1) infflag=1; } if (infflag==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 } } I2C i2c (PTC9,PTC8); //PTC9-sda,PTC8-scl for the attitude sensors and battery gauge void FCTN_ACS_INIT(void); //initialization of registers happens void FCTN_ATS_DATA_ACQ(); //data is obtained void T_OUT(); //timeout function to stop infinite loop Timeout to; //Timeout variable to int toFlag; int count =0; // Time for which the BAE uC is running (in seconds) void T_OUT() { toFlag=0; //as T_OUT function gets called the while loop gets terminated } //DEFINING VARIABLES char cmd[2]; char raw_gyro[6]; char raw_mag[6]; char store,status; int16_t bit_data; float gyro_data[3], mag_data[3],combined_values[6]; float senstivity_gyro =6.5536; //senstivity is obtained from 2^15/5000dps float senstivity_mag =32.768; //senstivity is obtained from 2^15/1000microtesla float gyro_error[3]= {0,0,0}, mag_error[3]= {0,0,0}; void FCTN_ACS_INIT() { ACS_INIT_STATUS = 's'; //set ACS_INIT_STATUS flag //FLAG(); pc_acs.printf("Attitude sensor init called \n \r"); //FLAG(); cmd[0]=RESETREQ; cmd[1]=BIT_RESREQ; i2c.write(SLAVE_ADDR,cmd,2); //When 0x01 is written in reset request register Emulates a hard power down/power up wait_ms(2000); //waiting for loading configuration file stored in EEPROM cmd[0]=SENTRALSTATUS; i2c.write(SLAVE_ADDR,cmd,1); i2c.read(SLAVE_ADDR_READ,&store,1); wait_ms(100); //to check whether EEPROM is uploaded switch((int)store) { case(3): { break; } case(11): { break; } default: { cmd[0]=RESETREQ; cmd[1]=BIT_RESREQ; i2c.write(SLAVE_ADDR,cmd,2); wait_ms(2000); } } pc_acs.printf("Sentral Status is %x\n \r",(int)store); cmd[0]=HOST_CTRL; //0x01 is written in HOST CONTROL register to enable the sensors cmd[1]=BIT_RUN_ENB; i2c.write(SLAVE_ADDR,cmd,2); wait_ms(100); cmd[0]=MAGRATE; //Output data rate of 100Hz is used for magnetometer cmd[1]=BIT_MAGODR; i2c.write(SLAVE_ADDR,cmd,2); wait_ms(100); cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope cmd[1]=BIT_GYROODR; i2c.write(SLAVE_ADDR,cmd,2); wait_ms(100); cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values cmd[1]=0x00; i2c.write(SLAVE_ADDR,cmd,2); wait_ms(100); cmd[0]=ENB_EVT; //enabling the error,gyro values and magnetometer values cmd[1]=BIT_EVT_ENB; i2c.write(SLAVE_ADDR,cmd,2); wait_ms(100); ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag } void FCTN_ATS_DATA_ACQ() { ACS_DATA_ACQ_STATUS = 's'; //set ACS_DATA_ACQ_STATUS flag for att sens 2 if( ACS_ATS_ENABLE == 'e') { FLAG(); pc_acs.printf("attitude sensor execution called \n \r"); toFlag=1; //toFlag is set to 1 so that it enters while loop to.attach(&T_OUT,2); //after 2 seconds the while loop gets terminated while(toFlag) { cmd[0]=EVT_STATUS; i2c.write(SLAVE_ADDR,cmd,1); i2c.read(SLAVE_ADDR_READ,&status,1); wait_ms(100); pc_acs.printf("Event Status is %x\n \r",(int)status); //if the 6th and 4th bit are 1 then it implies that gyro and magnetometer values are ready to take if(((int)status&40)==40) { cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x i2c.write(SLAVE_ADDR,cmd,1); i2c.read(SLAVE_ADDR_READ,raw_gyro,6); cmd[0]=MAG_XOUT_H; //LSB of x i2c.write(SLAVE_ADDR,cmd,1); i2c.read(SLAVE_ADDR_READ,raw_mag,6); // pc_acs.printf("\nGyro Values:\n"); for(int i=0; i<3; i++) { //concatenating gyro LSB and MSB to get 16 bit signed data values bit_data= ((int16_t)raw_gyro[2*i+1]<<8)|(int16_t)raw_gyro[2*i]; gyro_data[i]=(float)bit_data; gyro_data[i]=gyro_data[i]/senstivity_gyro; gyro_data[i]+=gyro_error[i]; // pc_acs.printf("%f\t",gyro_data[i]); } // 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 bit_data= ((int16_t)raw_mag[2*i+1]<<8)|(int16_t)raw_mag[2*i]; mag_data[i]=(float)bit_data; mag_data[i]=mag_data[i]/senstivity_mag; mag_data[i]+=mag_error[i]; // pc_acs.printf("%f\t",mag_data[i]); } for(int i=0; i<3; i++) { // data[i]=gyro_data[i]; actual_data.AngularSpeed_actual[i] = gyro_data[i]; actual_data.Bvalue_actual[i] = mag_data[i]; //data[i+3]=mag_data[i]; } // return(combined_values); //returning poiter combined values } //checking for the error else if (((int)status&2)==2) { FCTN_ACS_INIT(); //when there is any error then Again inilization is done to remove error } } } else //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE { ACS_DATA_ACQ_STATUS = 'f'; } ACS_DATA_ACQ_STATUS = 'c'; //clear ACS_DATA_ACQ_STATUS flag for att sens 2 } void FCTN_ACS_GENPWM_MAIN(float Moment[3]) { 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 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction float l_current_y=0; //Current sent in y TR's float l_duty_cycle_z=0; //Duty cycle of Moment in z direction float l_current_z=0; //Current sent in z TR's for(int i = 0 ; i<3;i++) { // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs } //----------------------------- x-direction TR --------------------------------------------// float l_moment_x = Moment[0]; //Moment in x direction phase_TR_x = 1; // setting the default current direction if (l_moment_x <0) { 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 l_moment_x = abs(l_moment_x); } l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship printf("current in trx is %f \r \n",l_current_x); 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 = 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 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.0016 && l_current_x < 0.0171) { 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 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.0171 && l_current_x < 0.1678) { 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 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) { printf("\n \r l_current_x====0"); l_duty_cycle_x = 0; // default value of duty cycle printf("DC for trx is %f \r \n",l_duty_cycle_x); PWM1.period(TIME_PERIOD); PWM1 = l_duty_cycle_x/100 ; } else //not necessary { g_err_flag_TR_x = 1; } //------------------------------------- y-direction TR--------------------------------------// float l_moment_y = Moment[1]; //Moment in y direction phase_TR_y = 1; // setting the default current direction if (l_moment_y <0) { 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 l_moment_y = abs(l_moment_y); } l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship printf("current in try is %f \r \n",l_current_y); 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 = 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 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.0016 && l_current_y < 0.0171) { 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 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.0171 && l_current_y < 0.1678) { 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 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"); l_duty_cycle_y = 0; // default value of duty cycle printf("DC for try is %f \r \n",l_duty_cycle_y); PWM2.period(TIME_PERIOD); PWM2 = l_duty_cycle_y/100 ; } else // not necessary { 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 if (l_moment_z <0) { 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 l_moment_z = abs(l_moment_z); } l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship printf("current in trz is %f \r \n",l_current_z); 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 = 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 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.0016 && l_current_z < 0.0171) { 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 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.0171 && l_current_z < 0.1678) { 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 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) { printf("\n \r l_current_z====0"); l_duty_cycle_z = 0; // default value of duty cycle printf("DC for trz is %f \r \n",l_duty_cycle_z); PWM3.period(TIME_PERIOD); PWM3 = l_duty_cycle_z/100 ; } else // not necessary { g_err_flag_TR_z = 1; } //-----------------------------------------exiting the function-----------------------------------// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function } /*void FCTN_ACS_GENPWM_MAIN(float Moment[3]) { 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 float l_duty_cycle_y=0; //Duty cycle of Moment in y direction float l_current_y=0; //Current sent in y TR's float l_duty_cycle_z=0; //Duty cycle of Moment in z direction float l_current_z=0; //Current sent in z TR's for(int i = 0 ; i<3;i++) { printf("pwm %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs } //----------------------------- x-direction TR --------------------------------------------// float l_moment_x = Moment[0]; //Moment in x direction phase_TR_x = 1; // setting the default current direction if (l_moment_x <0) { 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 l_moment_x = abs(l_moment_x); } 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% { 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 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) { 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 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) { 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 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) { printf("\n \r l_current_x====0"); l_duty_cycle_x = 0; // default value of duty cycle 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 //not necessary { g_err_flag_TR_x = 1; } //------------------------------------- y-direction TR--------------------------------------// float l_moment_y = Moment[1]; //Moment in y direction phase_TR_y = 1; // setting the default current direction if (l_moment_y <0) { 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 l_moment_y = abs(l_moment_y); } 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% { 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 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) { 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 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) { 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 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"); l_duty_cycle_y = 0; // default value of duty cycle 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 // not necessary { 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 if (l_moment_z <0) { 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 l_moment_z = abs(l_moment_z); } 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% { 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 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) { 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 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) { 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 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) { printf("\n \r l_current_z====0"); l_duty_cycle_z = 0; // default value of duty cycle 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 // not necessary { g_err_flag_TR_z = 1; } //-----------------------------------------exiting the function-----------------------------------// printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function }*/