Diff: ACS.cpp

Revision:

10:f93407b97750

Parent:

9:194afacf7449

Child:

13:fb7facaf308b

diff -r 194afacf7449 -r f93407b97750 ACS.cpp
--- a/ACS.cpp	Fri Apr 01 21:13:16 2016 +0000
+++ b/ACS.cpp	Mon Apr 11 17:26:46 2016 +0000
@@ -11,15 +11,15 @@
 //********************************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 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 uint8_t ACS_ATS_ENABLE;
-extern uint8_t ACS_DATA_ACQ_ENABLE;
-extern uint8_t ACS_STATE;
+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
@@ -37,122 +37,138 @@
 extern BAE_HK_actual actual_data;
 
 
-//DigitalOut ATS1_SW_ENABLE(PTC0); // enable of att sens2 switch
-//DigitalOut ATS2_SW_ENABLE(PTC16); // enable of att sens switch
+//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]);
-  
-int ctrl_count = 0;
-float bcopy[3];
-float moment[3];
- ///////algo working well
+
+//CONTROLALGO PARAMETERS
+
+
 void FCTN_ACS_CNTRLALGO(float b[3],float omega[3])
 {
-    float db[3];
-    float bb[3]={0,0,0};
-    float d[3]={0,0,0};
-    float Jm[3][3]={{0.2730,0,0},{0,0.3018,0},{0,0,0.3031}};
-    float den=0,den2;
-    int i,j;                   //temporary variables
-    float Mu[2],z[2],dv[2],v[2],u[2],tauc[3]={0,0,0};           //outputs
-    float invJm[3][3];
-    float kmu2=0.07,gamma2=1.9e4,kz2=0.4e-2,kmu=0.003,gamma=5.6e4,kz=0.1e-4;
+    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);
     
-    //................. calculating db values...........................
-    if(ctrl_count!=0)
-    {
-        for(i=0;i<3;i++)
-        db[i]= (b[i]-bcopy[i])/10;
-    }
+}
+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++)
-        db[i]= 0;
-    }
-    ctrl_count++;
-    //..................................................................
-    printf("\n\r Entered cntrl algo\n\r");
-    for(int i=0; i<3; i++) 
         {
-        printf("%f\t",omega[i]);
+            db1[i]= (b1[i]-b_old[i])/sampling_time; // Unit: Tesla/Second
         }
-    for(int i=0; i<3; i++) 
-        {
-        printf("%f\t",b[i]);
-        }
-
-    //.........................algo......................................
-    den=sqrt((b[0]*b[0])+(b[1]*b[1])+(b[2]*b[2]));
-    den2=(b[0]*db[0])+(b[1]*db[1])+(b[2]*db[2]);
-    for(i=0;i<3;i++)
+    }
+    
+        if(max_array(omega1)<(0.8*OmegaMax) && alarmmode==1)
     {
-        db[i]=((db[i]*den*den)-(b[i]*(den2)))/(pow(den,3));
-        //db[i]/=den*den*den;
+            alarmmode=0;
     }
-    for(i=0;i<3;i++)
-    {
-        b[i]/=den;
-    }
-    // select kz, kmu, gamma
-    if(b[0]>0.9||b[0]<-0.9)
+        else if(max_array(omega1)>OmegaMax && alarmmode==0)
     {
-        kz=kz2;
-        kmu=kmu2;
-        gamma=gamma2;
+            alarmmode=1;
     }
-    // calculate Mu, v, dv, z, u
-    for(i=0;i<2;i++)
+
+    for (i=0;i<3;i++)
     {
-        Mu[i]=b[i+1];
-        v[i]=-kmu*Mu[i];
-        dv[i]=-kmu*db[i+1];
-        z[i]=db[i+1]-v[i];
-        u[i]=-kz*z[i]+dv[i]-(Mu[i]/gamma);
+        b1_copy[i]=b1[i];
+        db1_copy[i]=db1[i];
+        omega1_copy[i]=omega1[i];
     }
-    inverse(Jm,invJm);
-    for(i=0;i<3;i++)
-    {
-        for(j=0;j<3;j++)
-            bb[i]+=omega[j]*(omega[(i+1)%3]*Jm[(i+2)%3][j]-omega[(i+2)%3]*Jm[(i+1)%3][j]);
-    }
-    for(i=0;i<3;i++)
+
+    if(alarmmode==0)
+        {
+            controlmode=0;
+            controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+        for (i=0;i<3;i++)
+        {
+            b1[i]=b1_copy[i];
+            db1[i]=db1_copy[i];
+            omega1[i]=omega1_copy[i];
+        }
+            if(max_array(moment)>MmntMax)
+            {
+                controlmode=1;
+                controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+            for (i=0;i<3;i++)
+            {
+                b1[i]=b1_copy[i];
+                db1[i]=db1_copy[i];
+                omega1[i]=omega1_copy[i];
+            }
+                if(max_array(moment)>MmntMax)
+                {
+                    normalising_fact=max_array(moment)/MmntMax;
+                    for(i=0;i<3;i++)
+                {
+                        moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+                }
+                }
+            }
+        }
+        else
+        {   
+            controlmode=1;
+            controlmodes(moment,b1,db1,omega1,controlmode,MmntMax);
+        for (i=0;i<3;i++)
+        {
+            b1[i]=b1_copy[i];
+            db1[i]=db1_copy[i];
+            omega1[i]=omega1_copy[i];
+        }
+            if(max_array(moment)>MmntMax)
+            {
+                normalising_fact=max_array(moment)/MmntMax;
+                for(i=0;i<3;i++)
+            {
+                    moment[i]/=normalising_fact; // Unit: Ampere*Meter^2
+            }
+            }
+        
+        }
+    for (i=0;i<3;i++)
     {
-        for(j=0;j<3;j++)
-            d[i]+=bb[j]*invJm[i][j];
-    }
-    bb[1]=u[0]+(d[0]*b[2])-(d[2]*b[0])-(omega[0]*db[2])+(omega[2]*db[0]);
-    bb[2]=u[1]-(d[0]*b[1])+(d[1]*b[0])+(omega[0]*db[1])-(omega[1]*db[0]);
-    bb[0]=0;
-    for(i=0;i<3;i++)
-    {
-        d[i]=invJm[1][i];
-        invJm[1][i]=b[2]*invJm[0][i]-b[0]*invJm[2][i];
-        invJm[2][i]=-b[1]*invJm[0][i]+b[0]*d[i];
-        invJm[0][i]=b[i];
+        b_old[i]=b1[i];
     }
-    inverse(invJm,Jm);
-    printf("\n \r calculating tauc");
-    for(i=0;i<3;i++)
-    {
-        for(j=0;j<3;j++)
-            tauc[i]+=Jm[i][j]*bb[j];            // calculating torque values
-            printf(" %f \t",tauc[i]);    
-    }
-    //..........................tauc to moment conversion..........................
-    printf("\n \r calculating moment");
-    for(i=0;i<3;i++)
-        bcopy[i]=b[i]*den;
-    for(i=0;i<3;i++)
-    {
-        moment[i]=bcopy[(i+1)%3]*tauc[(i+2)%3]-bcopy[(i+2)%3]*tauc[(i+1)%3];
-        moment[i]/=den;
-        printf(" %f \t",moment[i]); 
-    }
-    printf("\n\r exited control algo\n");
 }
-//..........................function to find inverse..................
+
 void inverse(float mat[3][3],float inv[3][3])
 {
     int i,j;
@@ -160,16 +176,141 @@
     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
 
@@ -199,7 +340,7 @@
 
 void  FCTN_ACS_INIT()
 {
-    ACS_INIT_STATUS = 1;     //set ACS_INIT_STATUS flag
+    ACS_INIT_STATUS = 's';     //set ACS_INIT_STATUS flag
     //FLAG();
     pc_acs.printf("Attitude sensor init called \n \r");
     //FLAG();
@@ -247,13 +388,13 @@
     cmd[1]=BIT_EVT_ENB;
     i2c.write(SLAVE_ADDR,cmd,2);
     wait_ms(100);
-    ACS_INIT_STATUS = 0; //set ACS_INIT_STATUS flag
+    ACS_INIT_STATUS = 'c'; //set ACS_INIT_STATUS flag
 }
 
 void FCTN_ATS_DATA_ACQ()
 {
-    ACS_DATA_ACQ_STATUS = 1;        //set ACS_DATA_ACQ_STATUS flag for att sens 2
-    if( ACS_ATS_ENABLE == 1)
+    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");
@@ -307,9 +448,9 @@
     }
     else    //ACS_DATA_ACQ_STATUS = ACS_DATA_ACQ_FAILURE
     {
-       ACS_DATA_ACQ_STATUS = 1;   
+       ACS_DATA_ACQ_STATUS = 'f';   
     }
-    ACS_DATA_ACQ_STATUS = 0;        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
+    ACS_DATA_ACQ_STATUS = 'c';        //clear ACS_DATA_ACQ_STATUS flag for att sens 2
 }
 
 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
@@ -342,6 +483,176 @@
     }
     
     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%
     {
@@ -349,7 +660,6 @@
         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)
     { 
@@ -501,7 +811,7 @@
     
     printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
  
-}
+}*/
 
 
     
\ No newline at end of file