GOPA KUMAR K C
/
NewPWMGen
New PWM code by Yeshwanth
main.cpp@1:c5f613785c7b, 2016-03-19 (annotated)
- Committer:
- gkumar
- Date:
- Sat Mar 19 07:36:08 2016 +0000
- Revision:
- 1:c5f613785c7b
- Parent:
- 0:4e8fb2adea64
New PWM Gen code by Yeshwanth
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
gkumar | 1:c5f613785c7b | 1 | #include "mbed.h" |
gkumar | 1:c5f613785c7b | 2 | #include "math.h" |
gkumar | 1:c5f613785c7b | 3 | #include "stdlib.h" |
gkumar | 0:4e8fb2adea64 | 4 | #include "pin_config.h" |
gkumar | 1:c5f613785c7b | 5 | |
gkumar | 1:c5f613785c7b | 6 | //........................................... |
gkumar | 1:c5f613785c7b | 7 | #define TIME_PERIOD 0.02 |
gkumar | 1:c5f613785c7b | 8 | #define TR_CONSTANT 0.15254 |
gkumar | 1:c5f613785c7b | 9 | |
gkumar | 1:c5f613785c7b | 10 | DigitalOut phase_TR_x(PIN27); // PHASE pin for x-torquerod |
gkumar | 1:c5f613785c7b | 11 | DigitalOut phase_TR_y(PIN28); // PHASE pin for y-torquerod |
gkumar | 1:c5f613785c7b | 12 | DigitalOut phase_TR_z(PIN86); // PHASE pin for z-torquerod |
gkumar | 1:c5f613785c7b | 13 | |
gkumar | 1:c5f613785c7b | 14 | PwmOut PWM1(PIN93); //x //Functions used to generate PWM signal |
gkumar | 1:c5f613785c7b | 15 | PwmOut PWM2(PIN94); //y |
gkumar | 1:c5f613785c7b | 16 | PwmOut PWM3(PIN95); //z //PWM output comes from pins p6 |
gkumar | 1:c5f613785c7b | 17 | |
gkumar | 1:c5f613785c7b | 18 | |
gkumar | 1:c5f613785c7b | 19 | int g_err_flag_TR_x=0; // setting x-flag to zero |
gkumar | 1:c5f613785c7b | 20 | int g_err_flag_TR_y=0; // setting y-flag to zero |
gkumar | 1:c5f613785c7b | 21 | int g_err_flag_TR_z=0; // setting z-flag to zero |
gkumar | 1:c5f613785c7b | 22 | |
gkumar | 0:4e8fb2adea64 | 23 | void FCTN_ACS_GENPWM_MAIN(float Moment[3]) |
gkumar | 0:4e8fb2adea64 | 24 | { |
gkumar | 0:4e8fb2adea64 | 25 | printf("\n\rEntered executable PWMGEN function\n"); // entering the PWMGEN executable function |
gkumar | 0:4e8fb2adea64 | 26 | |
gkumar | 0:4e8fb2adea64 | 27 | float l_duty_cycle_x=0; //Duty cycle of Moment in x direction |
gkumar | 0:4e8fb2adea64 | 28 | float l_current_x=0; //Current sent in x TR's |
gkumar | 0:4e8fb2adea64 | 29 | float l_duty_cycle_y=0; //Duty cycle of Moment in y direction |
gkumar | 0:4e8fb2adea64 | 30 | float l_current_y=0; //Current sent in y TR's |
gkumar | 0:4e8fb2adea64 | 31 | float l_duty_cycle_z=0; //Duty cycle of Moment in z direction |
gkumar | 0:4e8fb2adea64 | 32 | float l_current_z=0; //Current sent in z TR's |
gkumar | 0:4e8fb2adea64 | 33 | |
gkumar | 0:4e8fb2adea64 | 34 | |
gkumar | 0:4e8fb2adea64 | 35 | for(int i = 0 ; i<3;i++) |
gkumar | 0:4e8fb2adea64 | 36 | { |
gkumar | 0:4e8fb2adea64 | 37 | // printf(" %f \t ",Moment[i]); // taking the moment values from control algorithm as inputs |
gkumar | 0:4e8fb2adea64 | 38 | } |
gkumar | 0:4e8fb2adea64 | 39 | |
gkumar | 0:4e8fb2adea64 | 40 | //----------------------------- x-direction TR --------------------------------------------// |
gkumar | 0:4e8fb2adea64 | 41 | |
gkumar | 0:4e8fb2adea64 | 42 | |
gkumar | 0:4e8fb2adea64 | 43 | float l_moment_x = Moment[0]; //Moment in x direction |
gkumar | 0:4e8fb2adea64 | 44 | |
gkumar | 0:4e8fb2adea64 | 45 | phase_TR_x = 1; // setting the default current direction |
gkumar | 0:4e8fb2adea64 | 46 | if (l_moment_x <0) |
gkumar | 0:4e8fb2adea64 | 47 | { |
gkumar | 0:4e8fb2adea64 | 48 | 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 |
gkumar | 0:4e8fb2adea64 | 49 | l_moment_x = abs(l_moment_x); |
gkumar | 0:4e8fb2adea64 | 50 | } |
gkumar | 0:4e8fb2adea64 | 51 | |
gkumar | 0:4e8fb2adea64 | 52 | l_current_x = l_moment_x * TR_CONSTANT ; //Moment and Current always have the linear relationship |
gkumar | 1:c5f613785c7b | 53 | printf("current in trx is %f \r \n",l_current_x); |
gkumar | 0:4e8fb2adea64 | 54 | if( l_current_x>0 && l_current_x < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% |
gkumar | 0:4e8fb2adea64 | 55 | { |
gkumar | 0:4e8fb2adea64 | 56 | 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 |
gkumar | 1:c5f613785c7b | 57 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
gkumar | 0:4e8fb2adea64 | 58 | PWM1.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 59 | PWM1 = l_duty_cycle_x/100 ; |
gkumar | 0:4e8fb2adea64 | 60 | } |
gkumar | 0:4e8fb2adea64 | 61 | else if (l_current_x >= 0.0016 && l_current_x < 0.0171) |
gkumar | 0:4e8fb2adea64 | 62 | { |
gkumar | 0:4e8fb2adea64 | 63 | 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 |
gkumar | 1:c5f613785c7b | 64 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
gkumar | 0:4e8fb2adea64 | 65 | PWM1.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 66 | PWM1 = l_duty_cycle_x/100 ; |
gkumar | 0:4e8fb2adea64 | 67 | } |
gkumar | 0:4e8fb2adea64 | 68 | else if(l_current_x >= 0.0171 && l_current_x < 0.1678) |
gkumar | 0:4e8fb2adea64 | 69 | { |
gkumar | 0:4e8fb2adea64 | 70 | 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 |
gkumar | 1:c5f613785c7b | 71 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
gkumar | 0:4e8fb2adea64 | 72 | PWM1.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 73 | PWM1 = l_duty_cycle_x/100 ; |
gkumar | 0:4e8fb2adea64 | 74 | } |
gkumar | 0:4e8fb2adea64 | 75 | else if(l_current_x==0) |
gkumar | 0:4e8fb2adea64 | 76 | { |
gkumar | 0:4e8fb2adea64 | 77 | printf("\n \r l_current_x====0"); |
gkumar | 0:4e8fb2adea64 | 78 | l_duty_cycle_x = 0; // default value of duty cycle |
gkumar | 1:c5f613785c7b | 79 | printf("DC for trx is %f \r \n",l_duty_cycle_x); |
gkumar | 0:4e8fb2adea64 | 80 | PWM1.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 81 | PWM1 = l_duty_cycle_x/100 ; |
gkumar | 0:4e8fb2adea64 | 82 | } |
gkumar | 0:4e8fb2adea64 | 83 | else //not necessary |
gkumar | 0:4e8fb2adea64 | 84 | { |
gkumar | 0:4e8fb2adea64 | 85 | g_err_flag_TR_x = 1; |
gkumar | 0:4e8fb2adea64 | 86 | } |
gkumar | 0:4e8fb2adea64 | 87 | |
gkumar | 0:4e8fb2adea64 | 88 | //------------------------------------- y-direction TR--------------------------------------// |
gkumar | 0:4e8fb2adea64 | 89 | |
gkumar | 0:4e8fb2adea64 | 90 | |
gkumar | 0:4e8fb2adea64 | 91 | float l_moment_y = Moment[1]; //Moment in y direction |
gkumar | 0:4e8fb2adea64 | 92 | |
gkumar | 0:4e8fb2adea64 | 93 | phase_TR_y = 1; // setting the default current direction |
gkumar | 0:4e8fb2adea64 | 94 | if (l_moment_y <0) |
gkumar | 0:4e8fb2adea64 | 95 | { |
gkumar | 0:4e8fb2adea64 | 96 | 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 |
gkumar | 0:4e8fb2adea64 | 97 | l_moment_y = abs(l_moment_y); |
gkumar | 0:4e8fb2adea64 | 98 | } |
gkumar | 0:4e8fb2adea64 | 99 | |
gkumar | 0:4e8fb2adea64 | 100 | |
gkumar | 0:4e8fb2adea64 | 101 | l_current_y = l_moment_y * TR_CONSTANT ; //Moment and Current always have the linear relationship |
gkumar | 1:c5f613785c7b | 102 | printf("current in try is %f \r \n",l_current_y); |
gkumar | 1:c5f613785c7b | 103 | if( l_current_y>0 && l_current_y < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% |
gkumar | 0:4e8fb2adea64 | 104 | { |
gkumar | 0:4e8fb2adea64 | 105 | 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 |
gkumar | 1:c5f613785c7b | 106 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
gkumar | 0:4e8fb2adea64 | 107 | PWM2.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 108 | PWM2 = l_duty_cycle_y/100 ; |
gkumar | 0:4e8fb2adea64 | 109 | } |
gkumar | 0:4e8fb2adea64 | 110 | else if (l_current_y >= 0.0016 && l_current_y < 0.0171) |
gkumar | 0:4e8fb2adea64 | 111 | { |
gkumar | 0:4e8fb2adea64 | 112 | 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 |
gkumar | 1:c5f613785c7b | 113 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
gkumar | 0:4e8fb2adea64 | 114 | PWM2.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 115 | PWM2 = l_duty_cycle_y/100 ; |
gkumar | 0:4e8fb2adea64 | 116 | } |
gkumar | 0:4e8fb2adea64 | 117 | else if(l_current_y >= 0.0171 && l_current_y < 0.1678) |
gkumar | 0:4e8fb2adea64 | 118 | { |
gkumar | 0:4e8fb2adea64 | 119 | 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 |
gkumar | 1:c5f613785c7b | 120 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
gkumar | 0:4e8fb2adea64 | 121 | PWM2.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 122 | PWM2 = l_duty_cycle_y/100 ; |
gkumar | 0:4e8fb2adea64 | 123 | } |
gkumar | 0:4e8fb2adea64 | 124 | else if(l_current_y==0) |
gkumar | 0:4e8fb2adea64 | 125 | { |
gkumar | 0:4e8fb2adea64 | 126 | printf("\n \r l_current_y====0"); |
gkumar | 0:4e8fb2adea64 | 127 | l_duty_cycle_y = 0; // default value of duty cycle |
gkumar | 1:c5f613785c7b | 128 | printf("DC for try is %f \r \n",l_duty_cycle_y); |
gkumar | 0:4e8fb2adea64 | 129 | PWM2.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 130 | PWM2 = l_duty_cycle_y/100 ; |
gkumar | 0:4e8fb2adea64 | 131 | } |
gkumar | 0:4e8fb2adea64 | 132 | else // not necessary |
gkumar | 0:4e8fb2adea64 | 133 | { |
gkumar | 0:4e8fb2adea64 | 134 | g_err_flag_TR_y = 1; |
gkumar | 0:4e8fb2adea64 | 135 | } |
gkumar | 0:4e8fb2adea64 | 136 | |
gkumar | 0:4e8fb2adea64 | 137 | //----------------------------------------------- z-direction TR -------------------------// |
gkumar | 0:4e8fb2adea64 | 138 | |
gkumar | 0:4e8fb2adea64 | 139 | |
gkumar | 0:4e8fb2adea64 | 140 | float l_moment_z = Moment[2]; //Moment in z direction |
gkumar | 0:4e8fb2adea64 | 141 | |
gkumar | 0:4e8fb2adea64 | 142 | phase_TR_z = 1; // setting the default current direction |
gkumar | 0:4e8fb2adea64 | 143 | if (l_moment_z <0) |
gkumar | 0:4e8fb2adea64 | 144 | { |
gkumar | 0:4e8fb2adea64 | 145 | 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 |
gkumar | 0:4e8fb2adea64 | 146 | l_moment_z = abs(l_moment_z); |
gkumar | 0:4e8fb2adea64 | 147 | } |
gkumar | 0:4e8fb2adea64 | 148 | |
gkumar | 0:4e8fb2adea64 | 149 | |
gkumar | 0:4e8fb2adea64 | 150 | l_current_z = l_moment_z * TR_CONSTANT ; //Moment and Current always have the linear relationship |
gkumar | 1:c5f613785c7b | 151 | printf("current in trz is %f \r \n",l_current_z); |
gkumar | 0:4e8fb2adea64 | 152 | if( l_current_z>0 && l_current_z < 0.0016 ) //Current and Duty cycle have the linear relationship between 1% and 100% |
gkumar | 0:4e8fb2adea64 | 153 | { |
gkumar | 0:4e8fb2adea64 | 154 | 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 |
gkumar | 1:c5f613785c7b | 155 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
gkumar | 0:4e8fb2adea64 | 156 | PWM3.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 157 | PWM3 = l_duty_cycle_z/100 ; |
gkumar | 0:4e8fb2adea64 | 158 | } |
gkumar | 0:4e8fb2adea64 | 159 | else if (l_current_z >= 0.0016 && l_current_z < 0.0171) |
gkumar | 0:4e8fb2adea64 | 160 | { |
gkumar | 0:4e8fb2adea64 | 161 | 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 |
gkumar | 1:c5f613785c7b | 162 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
gkumar | 0:4e8fb2adea64 | 163 | PWM3.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 164 | PWM3 = l_duty_cycle_z/100 ; |
gkumar | 0:4e8fb2adea64 | 165 | } |
gkumar | 0:4e8fb2adea64 | 166 | else if(l_current_z >= 0.0171 && l_current_z < 0.1678) |
gkumar | 0:4e8fb2adea64 | 167 | { |
gkumar | 0:4e8fb2adea64 | 168 | 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 |
gkumar | 1:c5f613785c7b | 169 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
gkumar | 0:4e8fb2adea64 | 170 | PWM3.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 171 | PWM3 = l_duty_cycle_z/100 ; |
gkumar | 0:4e8fb2adea64 | 172 | } |
gkumar | 0:4e8fb2adea64 | 173 | else if(l_current_z==0) |
gkumar | 0:4e8fb2adea64 | 174 | { |
gkumar | 0:4e8fb2adea64 | 175 | printf("\n \r l_current_z====0"); |
gkumar | 0:4e8fb2adea64 | 176 | l_duty_cycle_z = 0; // default value of duty cycle |
gkumar | 1:c5f613785c7b | 177 | printf("DC for trz is %f \r \n",l_duty_cycle_z); |
gkumar | 0:4e8fb2adea64 | 178 | PWM3.period(TIME_PERIOD); |
gkumar | 0:4e8fb2adea64 | 179 | PWM3 = l_duty_cycle_z/100 ; |
gkumar | 0:4e8fb2adea64 | 180 | } |
gkumar | 0:4e8fb2adea64 | 181 | else // not necessary |
gkumar | 0:4e8fb2adea64 | 182 | { |
gkumar | 0:4e8fb2adea64 | 183 | g_err_flag_TR_z = 1; |
gkumar | 0:4e8fb2adea64 | 184 | } |
gkumar | 0:4e8fb2adea64 | 185 | |
gkumar | 0:4e8fb2adea64 | 186 | //-----------------------------------------exiting the function-----------------------------------// |
gkumar | 0:4e8fb2adea64 | 187 | |
gkumar | 0:4e8fb2adea64 | 188 | printf("\n\rExited executable PWMGEN function\n\r"); // stating the successful exit of TR function |
gkumar | 0:4e8fb2adea64 | 189 | |
gkumar | 1:c5f613785c7b | 190 | } |
gkumar | 1:c5f613785c7b | 191 | |
gkumar | 1:c5f613785c7b | 192 | void main(){ |
gkumar | 1:c5f613785c7b | 193 | float mmt[3]={1,0.5,1.1}; // Unit: Ampere*Meter^2 |
gkumar | 1:c5f613785c7b | 194 | FCTN_ACS_GENPWM_MAIN(mmt); |
gkumar | 1:c5f613785c7b | 195 | } |