GOPA KUMAR K C / Mbed 2 deprecated NewPWMGen

New PWM code by Yeshwanth

Dependencies:   mbed

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?

UserRevisionLine numberNew 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 }