New PWM code by Yeshwanth

Dependencies:   mbed

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "stdlib.h"
00004 #include "pin_config.h"
00005 
00006 //...........................................
00007 #define TIME_PERIOD  0.02
00008 #define TR_CONSTANT  0.15254
00009 
00010 DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod
00011 DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod
00012 DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod
00013 
00014 PwmOut PWM1(PIN93); //x                         //Functions used to generate PWM signal 
00015 PwmOut PWM2(PIN94); //y
00016 PwmOut PWM3(PIN95); //z                         //PWM output comes from pins p6
00017 
00018 
00019 int g_err_flag_TR_x=0;       // setting x-flag to zero
00020 int g_err_flag_TR_y=0;       // setting y-flag to zero
00021 int g_err_flag_TR_z=0;       // setting z-flag to zero
00022 
00023 void FCTN_ACS_GENPWM_MAIN(float Moment[3])
00024 {
00025     printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function
00026     
00027     float l_duty_cycle_x=0;    //Duty cycle of Moment in x direction
00028     float l_current_x=0;       //Current sent in x TR's
00029     float l_duty_cycle_y=0;    //Duty cycle of Moment in y direction
00030     float l_current_y=0;       //Current sent in y TR's
00031     float l_duty_cycle_z=0;    //Duty cycle of Moment in z direction
00032     float l_current_z=0;       //Current sent in z TR's
00033  
00034     
00035     for(int i = 0 ; i<3;i++)
00036     {
00037      //   printf(" %f \t ",Moment[i]);  // taking the moment values from control algorithm as inputs
00038     }
00039     
00040     //-----------------------------  x-direction TR  --------------------------------------------//
00041     
00042     
00043     float l_moment_x = Moment[0];         //Moment in x direction
00044     
00045     phase_TR_x = 1;  // setting the default current direction
00046     if (l_moment_x <0)
00047     {
00048         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 
00049         l_moment_x = abs(l_moment_x);
00050     }
00051     
00052     l_current_x = l_moment_x * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00053     printf("current in trx is %f \r \n",l_current_x);
00054     if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00055     {
00056         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 
00057         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00058         PWM1.period(TIME_PERIOD);
00059         PWM1 = l_duty_cycle_x/100 ;
00060     }
00061     else if (l_current_x >= 0.0016 && l_current_x < 0.0171)
00062     {
00063         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
00064         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00065         PWM1.period(TIME_PERIOD);
00066         PWM1 = l_duty_cycle_x/100 ;            
00067     }
00068     else if(l_current_x >= 0.0171 && l_current_x < 0.1678)
00069     {
00070         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
00071         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00072         PWM1.period(TIME_PERIOD);
00073         PWM1 = l_duty_cycle_x/100 ;            
00074     }
00075     else if(l_current_x==0)
00076     {
00077         printf("\n \r l_current_x====0");
00078         l_duty_cycle_x = 0;      // default value of duty cycle
00079         printf("DC for trx is %f \r \n",l_duty_cycle_x);
00080         PWM1.period(TIME_PERIOD);
00081         PWM1 = l_duty_cycle_x/100 ;            
00082     }
00083     else                                           //not necessary
00084     {
00085         g_err_flag_TR_x = 1;
00086     } 
00087          
00088     //------------------------------------- y-direction TR--------------------------------------//
00089     
00090      
00091     float l_moment_y = Moment[1];         //Moment in y direction
00092     
00093     phase_TR_y = 1;  // setting the default current direction
00094     if (l_moment_y <0)
00095     {
00096         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  
00097         l_moment_y = abs(l_moment_y);
00098     }
00099     
00100     
00101     l_current_y = l_moment_y * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00102     printf("current in try is %f \r \n",l_current_y);
00103     if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00104     {
00105         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 
00106         printf("DC for try is %f \r \n",l_duty_cycle_y);
00107         PWM2.period(TIME_PERIOD);
00108         PWM2 = l_duty_cycle_y/100 ;
00109     }
00110     else if (l_current_y >= 0.0016 && l_current_y < 0.0171)
00111     {
00112         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
00113         printf("DC for try is %f \r \n",l_duty_cycle_y);
00114         PWM2.period(TIME_PERIOD);
00115         PWM2 = l_duty_cycle_y/100 ;            
00116     }
00117     else if(l_current_y >= 0.0171 && l_current_y < 0.1678)
00118     {
00119         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
00120         printf("DC for try is %f \r \n",l_duty_cycle_y);
00121         PWM2.period(TIME_PERIOD);
00122         PWM2 = l_duty_cycle_y/100 ;            
00123     }        
00124     else if(l_current_y==0)
00125     {
00126         printf("\n \r l_current_y====0");
00127         l_duty_cycle_y = 0; // default value of duty cycle
00128         printf("DC for try is %f \r \n",l_duty_cycle_y);
00129         PWM2.period(TIME_PERIOD);
00130         PWM2 = l_duty_cycle_y/100 ;            
00131     }
00132     else                               // not necessary
00133     {
00134       g_err_flag_TR_y = 1;
00135     } 
00136              
00137     //----------------------------------------------- z-direction TR -------------------------//  
00138     
00139       
00140     float l_moment_z = Moment[2];         //Moment in z direction
00141     
00142     phase_TR_z = 1;   // setting the default current direction
00143     if (l_moment_z <0)
00144     {
00145         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 
00146         l_moment_z = abs(l_moment_z);
00147     }
00148     
00149     
00150     l_current_z = l_moment_z * TR_CONSTANT ;        //Moment and Current always have the linear relationship
00151      printf("current in trz is %f \r \n",l_current_z);
00152         if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100%
00153     {
00154         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 
00155         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00156         PWM3.period(TIME_PERIOD);
00157         PWM3 = l_duty_cycle_z/100 ;
00158     }
00159     else if (l_current_z >= 0.0016 && l_current_z < 0.0171)
00160     {
00161         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
00162         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00163         PWM3.period(TIME_PERIOD);
00164         PWM3 = l_duty_cycle_z/100 ;            
00165     }
00166     else if(l_current_z >= 0.0171 && l_current_z < 0.1678)
00167     {
00168         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
00169         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00170         PWM3.period(TIME_PERIOD);
00171         PWM3 = l_duty_cycle_z/100 ;            
00172     }
00173     else if(l_current_z==0)
00174     {
00175         printf("\n \r l_current_z====0");
00176         l_duty_cycle_z = 0; // default value of duty cycle
00177         printf("DC for trz is %f \r \n",l_duty_cycle_z);
00178         PWM3.period(TIME_PERIOD);
00179         PWM3 = l_duty_cycle_z/100 ;            
00180     }
00181     else                               // not necessary
00182     {
00183         g_err_flag_TR_z = 1;
00184     }   
00185     
00186     //-----------------------------------------exiting the function-----------------------------------//
00187     
00188     printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function
00189  
00190 }
00191 
00192 void main(){
00193     float mmt[3]={1,0.5,1.1}; // Unit: Ampere*Meter^2
00194     FCTN_ACS_GENPWM_MAIN(mmt);
00195     }