New PWM code by Yeshwanth
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Wed Jul 13 2022 22:01:32 by
1.7.2