File content as of revision 19:79e69017c855:

/*------------------------------------------------------------------------------------------------------------------------------------------------------
-------------------------------------------CONTROL ALGORITHM------------------------------------------------------------------------------------------*/
#include <mbed.h>
#include <math.h>

#include "pni.h" //pni header file
#include "pin_config.h"
#include "ACS.h"
#include "EPS.h"

/*timer for determining the execution time*/
//Timer time;

//********************************flags******************************************//
extern uint32_t BAE_STATUS;
extern uint32_t BAE_ENABLE;
extern uint8_t ACS_INIT_STATUS;
extern uint8_t ACS_DATA_ACQ_STATUS;
extern uint8_t ACS_ATS_STATUS;
extern uint8_t ACS_MAIN_STATUS;
extern uint8_t ACS_STATUS;

extern uint8_t ACS_ATS_ENABLE;
extern uint8_t ACS_DATA_ACQ_ENABLE;
extern uint8_t 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();
   /* as of now no reset
   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
   // */
    //wait_ms(3000);
    cmd[0]=SENTRALSTATUS;
    i2c.write(SLAVE_ADDR,cmd,1);
    i2c.read(SLAVE_ADDR_READ,&store,1);
    wait_ms(20); // initially 100
    //to check whether EEPROM is uploaded
    switch((int)store) { 
        case(3): { //actually this state correct
            break;
        }
        case(11): {
            break;
        }
        default: {
            cmd[0]=RESETREQ;
            cmd[1]=BIT_RESREQ;
            i2c.write(SLAVE_ADDR,cmd,2);
            wait_ms(2000);//see if it can be changed
        }
    }
    pc_acs.printf("\n\n\rwait is 1 \n\r");
    pc_acs.printf("Sentral Status is %x\n \r",(int)store);
    
    /*ways to make the sensors work even if one sensor cups first making data rate 0x00
    1>make data rate 0x00 change the enable events register value to 0x0A or 0x22 as suitable
    2>change the condition for getting the values from the sensors i.e in data_acq function
    3>check the other register values
    
    other method is pass through working in how it works / decoding SENtral algorithms
    */
    
    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);
    //___________________________________________________________________________________________________
    
    /*just leave it then see what happens?? the gyro data without inputing anything*/
    
    //___________________________________________________________________________________________________
    cmd[0]=GYRORATE; //Output data rate of 150Hz is used for gyroscope
    cmd[1]=BIT_GYROODR;
    i2c.write(SLAVE_ADDR,cmd,2);
    wait_ms(1);
    cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
    cmd[1]=0x00;//actually 0x00
    i2c.write(SLAVE_ADDR,cmd,2);
    wait_ms(1);
    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(1);
    //_______________________________________________________________________________//
    
/*start as of now this approach i.e pass through state*/
/*
        cmd[0]=0x35;//event status
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rEvent Status at start is %x\n \r",(int)status);
            
    cmd[0]=ALGO_CTRL; //When 0x00 is written to ALGO CONTROL register we get scaled sensor values
    cmd[1]=0x01;//Places sentral in standby state
    i2c.write(SLAVE_ADDR,cmd,2);
    wait_ms(1);
    
    
    cmd[0]=0xA0; //Pass through control register
    cmd[1]=0x01;//places SENtral in pass through state
    i2c.write(SLAVE_ADDR,cmd,2);
    wait_ms(1); 
    
    
    cmd[0]=0x9E;//Pass through status reg
    i2c.write(SLAVE_ADDR,cmd,1);
    i2c.read(SLAVE_ADDR_READ,&store,1);
    wait_ms(1);
    printf("\n\r the value of the pass through status register is = %x",(int)store);    
    
        cmd[0]=0x35;//event status
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rEvent Status at end initialization is %x\n \r",(int)status);
*/

    //_______________________________________________________________________________//
    
    ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
}

void FCTN_ATS_DATA_ACQ()
{
    //time.start();
    
    ACS_DATA_ACQ_STATUS = 1;        //set ACS_DATA_ACQ_STATUS flag for att sens 2
    if( ACS_ATS_ENABLE == 1)
    {
    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){ //when both work   
/////          if((int)status==8){  //for just mag
/////         if((int)status == 36){   //for just gyro as status 24(in binary) = 36 in decimal 
/////           if((int)status==___yet to be decided___){ //for pass through state see how it tworks
                
            /*don't ask for info if the gyro cupped as interrupt makes it go haywire that is enables the pins for gyro*/
           cmd[0]=GYRO_XOUT_H; //0x22 gyro LSB of x 
          i2c.write(SLAVE_ADDR,cmd,1);
            i2c.read(SLAVE_ADDR_READ,raw_gyro,6);

/*editing the data*/

            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]);
            }
            
/*read the status values to determine the actual condition/registers values for the gyro/mag off case */
//starts here

        cmd[0]=0x35;//event status
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rEvent Status at the end is %x",(int)status);
        
        
        cmd[0]=0x33;//enable events
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rEnable events is %x",(int)status);
        
        
        cmd[0]=0x57;//gyro rate
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rgyro rate is %x",(int)status);
        
         
        cmd[0]=0x55;//mag rate 0x00 indicate value lost 
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rmag rate is %x",(int)status);
        
        
        cmd[0]=0x36;//sensorstatus
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rsensor Status is %x",(int)status);
        
        
        cmd[0]=0x50;//error register
        i2c.write(SLAVE_ADDR,cmd,1);
        i2c.read(SLAVE_ADDR_READ,&status,1);
        wait_ms(1);
        pc_acs.printf("\n\rerror register value is %x\n \r",(int)status);
                   
//end here            
            
       //     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 = 0;        //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
 
}*/