Motor current controller
Fork of CURRENT_CONTROL by
Embed:
(wiki syntax)
Show/hide line numbers
CURRENT_CONTROL.cpp
00001 #include "mbed.h" 00002 #include "CURRENT_CONTROL.h" 00003 00004 /* 00005 //=====================LPF ====================// 00006 LPF::LPF(float samplingTime, float cutOff_freq_Hz_in) 00007 { 00008 Ts = samplingTime; 00009 cutOff_freq_Hz = cutOff_freq_Hz_in; 00010 alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts; 00011 One_alpha_Ts = 1.0 - alpha_Ts; 00012 output = 0.0; 00013 00014 // 00015 Flag_Init = false; 00016 } 00017 00018 float LPF::filter(float input) 00019 { 00020 // Initialization 00021 if (!Flag_Init){ 00022 reset(input); 00023 Flag_Init = true; 00024 return output; 00025 } 00026 00027 // output = One_alpha_Ts*output + alpha_Ts*input; 00028 output += alpha_Ts*(input - output); 00029 00030 return output; 00031 } 00032 void LPF::reset(float input) 00033 { 00034 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input; 00035 output = input; 00036 return; 00037 } 00038 */ 00039 00040 //================== CURRENT_CONTROL =================// 00041 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel, 00042 PinName PwmChannel1, 00043 PinName PwmChannel2, 00044 PWMIndex pwmIndex, 00045 PinName QEI_A, 00046 PinName QEI_B, 00047 float pulsesPerRev, 00048 int arraysize, 00049 float samplingTime) : 00050 currentAnalogIn(curChannel), 00051 MotorPlus(PwmChannel1), 00052 MotorMinus(PwmChannel2), 00053 wheelSpeed(QEI_A, QEI_B, NC, pulsesPerRev, arraysize, samplingTime, QEI::X4_ENCODING), //(pin1, pin2, pin3, pulsesPerRev, arraysize, sampletime, pulses) 00054 pid(0.0,0.0,0.0,samplingTime), 00055 lpf_current(samplingTime, 400.0), // 1.5915 Hz = 10 rad/s 00056 lpf_wheelSpeed(samplingTime, 300.0, 2) // 2nd-order LPF, fc = 200.0 Hz 00057 00058 00059 { 00060 pwmIndex_ = pwmIndex; 00061 00062 Ts = samplingTime; 00063 00064 //setup motor PWM parameters 00065 MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz 00066 // 00067 SetPWMDuty(0.5); 00068 00069 //Current-input limit 00070 current_limit_H = 1.3; 00071 current_limit_L = -1.3; 00072 // 00073 Flag_Init = false; 00074 Init_count = 0; 00075 Accumulated_offset = 0.0; 00076 00077 // 00078 currentOffset = 0.0; 00079 // 00080 delta_output = 0.0; 00081 curFeedBack = 0.0; 00082 curFeedBack_filter = 0.0; 00083 curCommand = 0.0; 00084 voltage_out = 0.0; 00085 00086 // Set PID's parameters 00087 ///////////////////// 00088 pid.Kp = 0.0; 00089 pid.Ki = 0.0; 00090 pid.Kd = 0.0; 00091 pid.Ka = 0.0; // Gain for anti-windup // Ka = 0.0017 00092 00093 // Speed 00094 angularSpeed = 0.0; 00095 Flag_SpeedCal_Iterated = false; 00096 00097 // Reverse flage for each signal 00098 reverse_current = false; 00099 reverse_rotationalSpeed = false; 00100 reverse_voltage = false; 00101 } 00102 // 00103 void CURRENT_CONTROL::SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in) 00104 { 00105 analog2Cur = Analog2Cur_in;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6 00106 Ke = angSpeed2BackEmf; 00107 Kt = Ke; 00108 Kt_inv = 1/Kt; 00109 voltage2Duty = voltage2Duty_in; 00110 } 00111 void CURRENT_CONTROL::SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in) 00112 { 00113 reverse_current = reverse_current_in; 00114 reverse_rotationalSpeed = reverse_rotationalSpeed_in; 00115 reverse_voltage = reverse_voltage_in; 00116 } 00117 // 00118 void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka) 00119 { 00120 // Set PID's parameters 00121 ///////////////////// 00122 pid.Kp = Kp; 00123 pid.Ki = Ki; 00124 pid.Kd = Kd; 00125 pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017 00126 } 00127 // 00128 void CURRENT_CONTROL::setInputLimits(float current_limit_H_in, float current_limit_L_in){ 00129 current_limit_H = current_limit_H_in; 00130 current_limit_L = current_limit_L_in; 00131 } 00132 // 00133 void CURRENT_CONTROL::OffsetInit(void){ 00134 if (Flag_Init) return; 00135 // 00136 00137 Init_count++; 00138 /* 00139 Accumulated_offset += GetAnalogIn(); 00140 00141 if (Init_count >= 500){ // Fixed time 00142 currentOffset = Accumulated_offset / float(Init_count); 00143 Flag_Init = true; 00144 } 00145 */ 00146 00147 // 00148 /* 00149 if (Init_count <= 10){ 00150 Accumulated_offset = GetAnalogIn(); 00151 }else{ 00152 // Accumulated_offset += 0.0063*(GetAnalogIn() - Accumulated_offset); // fc = 1 Hz 00153 Accumulated_offset = 0.9937*Accumulated_offset + 0.0063*GetAnalogIn(); // fc = 1 Hz 00154 } 00155 00156 if (Init_count >= 300){ // Fixed time: 500 samples 00157 currentOffset = Accumulated_offset; 00158 Flag_Init = true; 00159 } 00160 */ 00161 00162 00163 // 00164 if (Init_count >= 10){ // Fixed time: 10 samples 00165 currentOffset = GetAnalogIn(); 00166 Flag_Init = true; 00167 } 00168 00169 00170 } 00171 00172 // Without delta output 00173 float CURRENT_CONTROL::saturation(float input_value, const float &limit_H, const float &limit_L){ 00174 if( input_value > limit_H ){ 00175 input_value = limit_H; 00176 }else if( input_value < limit_L ){ 00177 input_value = limit_L; 00178 }else{ 00179 // Nothing 00180 } 00181 return input_value; 00182 } 00183 // With delta output 00184 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){ 00185 if( input_value > limit_H ){ 00186 delta = limit_H - input_value; 00187 input_value = limit_H; 00188 }else if( input_value < limit_L ){ 00189 delta = limit_L - input_value; 00190 input_value = limit_L; 00191 }else{ 00192 delta = 0.0; 00193 } 00194 return input_value; 00195 } 00196 // Speed_IterateOnce 00197 //////////////////////////////// 00198 float CURRENT_CONTROL::Speed_IterateOnce(void){ 00199 // The flag "Flag_SpeedCal_Iterated" will be resetted at the end of function Control() 00200 if(Flag_SpeedCal_Iterated) return; 00201 00202 // Speed 00203 wheelSpeed.Calculate(); 00204 00205 /* 00206 // 00207 if (reverse_rotationalSpeed) 00208 angularSpeed = -wheelSpeed.getAngularSpeed(); 00209 else 00210 angularSpeed = wheelSpeed.getAngularSpeed(); 00211 // 00212 */ 00213 00214 // 00215 if (reverse_rotationalSpeed) 00216 angularSpeed = lpf_wheelSpeed.filter( -wheelSpeed.getAngularSpeed() ); 00217 else 00218 angularSpeed = lpf_wheelSpeed.filter( wheelSpeed.getAngularSpeed() ); 00219 // 00220 00221 // Flag 00222 Flag_SpeedCal_Iterated = true; 00223 00224 return angularSpeed; 00225 } 00226 float CURRENT_CONTROL::getAngularSpeed(void){ 00227 // return wheelSpeed.getAngularSpeed(); 00228 return angularSpeed; 00229 } 00230 float CURRENT_CONTROL::getAngularSpeed_deg_s(void){ 00231 00232 if (reverse_rotationalSpeed) 00233 return -wheelSpeed.getAngularSpeed_deg_s(); 00234 else 00235 return wheelSpeed.getAngularSpeed_deg_s(); 00236 // 00237 // return wheelSpeed.getAngularSpeed_deg_s(); 00238 } 00239 //////////////////////////////// end Speed_IterateOnce 00240 00241 00242 // Control 00243 /////////////////////////////////////////////////////////// 00244 void CURRENT_CONTROL::TorqueControl(float TorqueRef, bool enable){ 00245 Control(Kt_inv*TorqueRef, enable); 00246 } 00247 void CURRENT_CONTROL::Control(float curRef, bool enable) 00248 { 00249 // Input saturation 00250 curCommand = saturation(curRef,current_limit_H,current_limit_L); 00251 00252 // Init check 00253 OffsetInit(); 00254 if (!Flag_Init) return; 00255 ////////////////////////////////////// 00256 00257 // Get measurement 00258 // Speed 00259 Speed_IterateOnce(); 00260 // Motor current 00261 GetCurrent(); 00262 00263 //--------------------------------// 00264 00265 if (enable){ 00266 // PID 00267 pid.Compute_noWindUP(curRef, curFeedBack_filter); 00268 00269 // Voltage output with back-emf compensation 00270 voltage_out = pid.output + func_back_emf(angularSpeed); 00271 // voltage_out = 0.0 + func_back_emf(angularSpeed); 00272 00273 // Controlller output 00274 SetVoltage(voltage_out); 00275 }else{ 00276 pid.Reset(); 00277 voltage_out = 0.0; 00278 SetVoltage(0.0); 00279 } 00280 //--------------------------------// 00281 00282 // Anti-windup 00283 pid.Anti_windup(delta_output); 00284 00285 /////////////////////////////////////// 00286 // Reset flag 00287 Flag_SpeedCal_Iterated = false; 00288 } 00289 /////////////////////////////////////////////////////////// end Control 00290 00291 00292 // Back emf as the function of rotational speed 00293 float CURRENT_CONTROL::func_back_emf(const float &W_in){ 00294 return (Ke*W_in); 00295 } 00296 00297 // Elementary function (building block) 00298 void CURRENT_CONTROL::SetPWMDuty(float ratio) 00299 { 00300 MotorPlus = ratio; 00301 if(pwmIndex_ == PWM1){ 00302 TIM1->CCER |= 4; 00303 }else if(pwmIndex_ == PWM2){ 00304 TIM1->CCER |= 64; 00305 } 00306 } 00307 void CURRENT_CONTROL::SetVoltage(float volt) 00308 { 00309 // Output saturation and unit changing 00310 if (reverse_voltage) 00311 SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) ); 00312 else 00313 SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) ); 00314 } 00315 // 00316 float CURRENT_CONTROL::GetAnalogIn(void) 00317 { 00318 analogInValue = currentAnalogIn.read(); 00319 return analogInValue; 00320 } 00321 00322 float CURRENT_CONTROL::GetCurrent(void) 00323 { 00324 // Init check 00325 if (!Flag_Init) return 0.0; 00326 00327 //-----------------------------------------// 00328 GetAnalogIn(); 00329 00330 // Unit transformation 00331 if (reverse_current){ 00332 curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur; 00333 }else{ 00334 curFeedBack = (analogInValue - currentOffset)*analog2Cur; 00335 } 00336 // 00337 00338 // Low-pass filter 00339 curFeedBack_filter = lpf_current.filter(curFeedBack); 00340 00341 return curFeedBack_filter; //curFeedBack; 00342 }
Generated on Wed Jul 13 2022 05:32:48 by
1.7.2
