Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@15:d9ccd6c92a21, 2016-12-26 (annotated)

Committer:
benson516
Date:
Mon Dec 26 08:53:09 2016 +0000
Revision:
15:d9ccd6c92a21
Parent:
14:67fc256efeb7
Child:
16:6e3bcd373f9d
Add input saturztion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
adam_z 0:955aa05c968a 1 #include "mbed.h"
adam_z 0:955aa05c968a 2 #include "CURRENT_CONTROL.h"
adam_z 0:955aa05c968a 3
benson516 5:7ccd2fb7ce7e 4 //=====================LPF ====================//
benson516 5:7ccd2fb7ce7e 5 LPF::LPF(float samplingTime, float cutOff_freq_Hz_in)
benson516 5:7ccd2fb7ce7e 6 {
benson516 5:7ccd2fb7ce7e 7 Ts = samplingTime;
benson516 5:7ccd2fb7ce7e 8 cutOff_freq_Hz = cutOff_freq_Hz_in;
benson516 5:7ccd2fb7ce7e 9 alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
benson516 9:d8157fbfcd2a 10 One_alpha_Ts = 1.0 - alpha_Ts;
benson516 7:6794cfba3564 11 output = 0.0;
benson516 12:085f35babe21 12
benson516 7:6794cfba3564 13 //
benson516 12:085f35babe21 14 Flag_Init = false;
benson516 5:7ccd2fb7ce7e 15 }
adam_z 0:955aa05c968a 16
benson516 5:7ccd2fb7ce7e 17 float LPF::filter(float input)
benson516 5:7ccd2fb7ce7e 18 {
benson516 7:6794cfba3564 19 // Initialization
benson516 7:6794cfba3564 20 if (!Flag_Init){
benson516 7:6794cfba3564 21 reset(input);
benson516 7:6794cfba3564 22 Flag_Init = true;
benson516 7:6794cfba3564 23 return output;
benson516 7:6794cfba3564 24 }
benson516 12:085f35babe21 25
benson516 9:d8157fbfcd2a 26 // output = One_alpha_Ts*output + alpha_Ts*input;
benson516 5:7ccd2fb7ce7e 27 output += alpha_Ts*(input - output);
benson516 12:085f35babe21 28
benson516 5:7ccd2fb7ce7e 29 return output;
benson516 5:7ccd2fb7ce7e 30 }
benson516 7:6794cfba3564 31 void LPF::reset(float input)
benson516 7:6794cfba3564 32 {
benson516 7:6794cfba3564 33 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
benson516 7:6794cfba3564 34 output = input;
benson516 7:6794cfba3564 35 return;
benson516 7:6794cfba3564 36 }
benson516 5:7ccd2fb7ce7e 37
benson516 5:7ccd2fb7ce7e 38
benson516 5:7ccd2fb7ce7e 39 //================== CURRENT_CONTROL =================//
adam_z 1:c5973a56d474 40 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
adam_z 1:c5973a56d474 41 PinName PwmChannel1,
adam_z 1:c5973a56d474 42 PinName PwmChannel2,
adam_z 1:c5973a56d474 43 PWMIndex pwmIndex,
benson516 8:fd6fb3cb12ec 44 PinName QEI_A,
benson516 8:fd6fb3cb12ec 45 PinName QEI_B,
benson516 8:fd6fb3cb12ec 46 float pulsesPerRev,
benson516 8:fd6fb3cb12ec 47 int arraysize,
benson516 12:085f35babe21 48 float samplingTime) :
benson516 5:7ccd2fb7ce7e 49 currentAnalogIn(curChannel),
adam_z 0:955aa05c968a 50 MotorPlus(PwmChannel1),
adam_z 0:955aa05c968a 51 MotorMinus(PwmChannel2),
benson516 8:fd6fb3cb12ec 52 wheelSpeed(QEI_A, QEI_B, NC, pulsesPerRev, arraysize, samplingTime, QEI::X4_ENCODING), //(pin1, pin2, pin3, pulsesPerRev, arraysize, sampletime, pulses)
benson516 8:fd6fb3cb12ec 53 pid(0.0,0.0,0.0,samplingTime),
benson516 11:31cd02611cd0 54 lpFilter(samplingTime, 70.0) // 1.5915 Hz = 10 rad/s
benson516 8:fd6fb3cb12ec 55
benson516 12:085f35babe21 56
adam_z 0:955aa05c968a 57 {
adam_z 1:c5973a56d474 58 pwmIndex_ = pwmIndex;
adam_z 1:c5973a56d474 59
adam_z 0:955aa05c968a 60 Ts = samplingTime;
benson516 12:085f35babe21 61
adam_z 0:955aa05c968a 62 //setup motor PWM parameters
benson516 12:085f35babe21 63 MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz
benson516 8:fd6fb3cb12ec 64 //
benson516 8:fd6fb3cb12ec 65 SetPWMDuty(0.5);
benson516 12:085f35babe21 66
benson516 15:d9ccd6c92a21 67 //Current-input limit
benson516 15:d9ccd6c92a21 68 current_limit_H = 1.3;
benson516 15:d9ccd6c92a21 69 current_limit_L = -1.3;
benson516 7:6794cfba3564 70 //
benson516 7:6794cfba3564 71 Flag_Init = false;
benson516 7:6794cfba3564 72 Init_count = 0;
benson516 7:6794cfba3564 73 Accumulated_offset = 0.0;
benson516 12:085f35babe21 74
benson516 5:7ccd2fb7ce7e 75 //
benson516 5:7ccd2fb7ce7e 76 currentOffset = 0.0;
benson516 7:6794cfba3564 77 //
benson516 5:7ccd2fb7ce7e 78 delta_output = 0.0;
benson516 8:fd6fb3cb12ec 79 curFeedBack = 0.0;
benson516 8:fd6fb3cb12ec 80 curFeedBack_filter = 0.0;
benson516 14:67fc256efeb7 81 curCommand = 0.0;
benson516 8:fd6fb3cb12ec 82 voltage_out = 0.0;
benson516 12:085f35babe21 83
benson516 12:085f35babe21 84 // Set PID's parameters
benson516 6:bae35ca64f10 85 /////////////////////
benson516 8:fd6fb3cb12ec 86 pid.Kp = 0.0;
benson516 8:fd6fb3cb12ec 87 pid.Ki = 0.0;
benson516 8:fd6fb3cb12ec 88 pid.Kd = 0.0;
benson516 8:fd6fb3cb12ec 89 pid.Ka = 0.0; // Gain for anti-windup // Ka = 0.0017
benson516 12:085f35babe21 90
benson516 8:fd6fb3cb12ec 91 // Speed
benson516 8:fd6fb3cb12ec 92 angularSpeed = 0.0;
benson516 8:fd6fb3cb12ec 93 Flag_SpeedCal_Iterated = false;
benson516 12:085f35babe21 94
benson516 9:d8157fbfcd2a 95 // Reverse flage for each signal
benson516 9:d8157fbfcd2a 96 reverse_current = false;
benson516 9:d8157fbfcd2a 97 reverse_rotationalSpeed = false;
benson516 9:d8157fbfcd2a 98 reverse_voltage = false;
benson516 8:fd6fb3cb12ec 99 }
benson516 8:fd6fb3cb12ec 100 //
benson516 9:d8157fbfcd2a 101 void CURRENT_CONTROL::SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in)
benson516 8:fd6fb3cb12ec 102 {
benson516 9:d8157fbfcd2a 103 analog2Cur = Analog2Cur_in;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
benson516 8:fd6fb3cb12ec 104 Ke = angSpeed2BackEmf;
benson516 14:67fc256efeb7 105 Kt = Ke;
benson516 14:67fc256efeb7 106 Kt_inv = 1/Kt;
benson516 12:085f35babe21 107 voltage2Duty = voltage2Duty_in;
benson516 9:d8157fbfcd2a 108 }
benson516 9:d8157fbfcd2a 109 void CURRENT_CONTROL::SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in)
benson516 9:d8157fbfcd2a 110 {
benson516 9:d8157fbfcd2a 111 reverse_current = reverse_current_in;
benson516 9:d8157fbfcd2a 112 reverse_rotationalSpeed = reverse_rotationalSpeed_in;
benson516 9:d8157fbfcd2a 113 reverse_voltage = reverse_voltage_in;
benson516 8:fd6fb3cb12ec 114 }
benson516 8:fd6fb3cb12ec 115 //
benson516 8:fd6fb3cb12ec 116 void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka)
benson516 8:fd6fb3cb12ec 117 {
benson516 12:085f35babe21 118 // Set PID's parameters
benson516 8:fd6fb3cb12ec 119 /////////////////////
benson516 6:bae35ca64f10 120 pid.Kp = Kp;
benson516 6:bae35ca64f10 121 pid.Ki = Ki;
benson516 6:bae35ca64f10 122 pid.Kd = Kd;
benson516 6:bae35ca64f10 123 pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
benson516 5:7ccd2fb7ce7e 124 }
benson516 8:fd6fb3cb12ec 125 //
benson516 15:d9ccd6c92a21 126 void CURRENT_CONTROL::setInputLimits(float current_limit_H_in, float current_limit_L_in){
benson516 15:d9ccd6c92a21 127 current_limit_H = current_limit_H_in;
benson516 15:d9ccd6c92a21 128 current_limit_L = current_limit_L_in;
benson516 15:d9ccd6c92a21 129 }
benson516 15:d9ccd6c92a21 130 //
benson516 7:6794cfba3564 131 void CURRENT_CONTROL::OffsetInit(void){
benson516 7:6794cfba3564 132 if (Flag_Init) return;
benson516 7:6794cfba3564 133 //
benson516 12:085f35babe21 134
benson516 7:6794cfba3564 135 Init_count++;
benson516 9:d8157fbfcd2a 136 /*
benson516 7:6794cfba3564 137 Accumulated_offset += GetAnalogIn();
benson516 12:085f35babe21 138
benson516 7:6794cfba3564 139 if (Init_count >= 500){ // Fixed time
benson516 8:fd6fb3cb12ec 140 currentOffset = Accumulated_offset / float(Init_count);
benson516 7:6794cfba3564 141 Flag_Init = true;
benson516 7:6794cfba3564 142 }
benson516 9:d8157fbfcd2a 143 */
benson516 12:085f35babe21 144
benson516 9:d8157fbfcd2a 145 //
benson516 9:d8157fbfcd2a 146 /*
benson516 9:d8157fbfcd2a 147 if (Init_count <= 10){
benson516 9:d8157fbfcd2a 148 Accumulated_offset = GetAnalogIn();
benson516 9:d8157fbfcd2a 149 }else{
benson516 9:d8157fbfcd2a 150 // Accumulated_offset += 0.0063*(GetAnalogIn() - Accumulated_offset); // fc = 1 Hz
benson516 9:d8157fbfcd2a 151 Accumulated_offset = 0.9937*Accumulated_offset + 0.0063*GetAnalogIn(); // fc = 1 Hz
benson516 9:d8157fbfcd2a 152 }
benson516 12:085f35babe21 153
benson516 9:d8157fbfcd2a 154 if (Init_count >= 300){ // Fixed time: 500 samples
benson516 9:d8157fbfcd2a 155 currentOffset = Accumulated_offset;
benson516 9:d8157fbfcd2a 156 Flag_Init = true;
benson516 9:d8157fbfcd2a 157 }
benson516 9:d8157fbfcd2a 158 */
benson516 12:085f35babe21 159
benson516 12:085f35babe21 160
benson516 9:d8157fbfcd2a 161 //
benson516 9:d8157fbfcd2a 162 if (Init_count >= 10){ // Fixed time: 10 samples
benson516 9:d8157fbfcd2a 163 currentOffset = GetAnalogIn();
benson516 9:d8157fbfcd2a 164 Flag_Init = true;
benson516 9:d8157fbfcd2a 165 }
benson516 12:085f35babe21 166
benson516 12:085f35babe21 167
benson516 7:6794cfba3564 168 }
benson516 8:fd6fb3cb12ec 169
benson516 15:d9ccd6c92a21 170 // Without delta output
benson516 15:d9ccd6c92a21 171 float CURRENT_CONTROL::saturation(float input_value, const float &limit_H, const float &limit_L){
benson516 15:d9ccd6c92a21 172 if( input_value > limit_H ){
benson516 15:d9ccd6c92a21 173 input_value = limit_H;
benson516 15:d9ccd6c92a21 174 }else if( input_value < limit_L ){
benson516 15:d9ccd6c92a21 175 input_value = limit_L;
benson516 15:d9ccd6c92a21 176 }else{
benson516 15:d9ccd6c92a21 177 // Nothing
benson516 15:d9ccd6c92a21 178 }
benson516 15:d9ccd6c92a21 179 return input_value;
benson516 15:d9ccd6c92a21 180 }
benson516 15:d9ccd6c92a21 181 // With delta output
benson516 5:7ccd2fb7ce7e 182 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
benson516 5:7ccd2fb7ce7e 183 if( input_value > limit_H ){
benson516 5:7ccd2fb7ce7e 184 delta = limit_H - input_value;
benson516 5:7ccd2fb7ce7e 185 input_value = limit_H;
benson516 12:085f35babe21 186 }else if( input_value < limit_L ){
benson516 5:7ccd2fb7ce7e 187 delta = limit_L - input_value;
benson516 12:085f35babe21 188 input_value = limit_L;
benson516 5:7ccd2fb7ce7e 189 }else{
benson516 5:7ccd2fb7ce7e 190 delta = 0.0;
benson516 5:7ccd2fb7ce7e 191 }
benson516 5:7ccd2fb7ce7e 192 return input_value;
adam_z 4:1a6ba05e7736 193 }
benson516 9:d8157fbfcd2a 194 // Speed_IterateOnce
benson516 9:d8157fbfcd2a 195 ////////////////////////////////
benson516 8:fd6fb3cb12ec 196 float CURRENT_CONTROL::Speed_IterateOnce(void){
benson516 8:fd6fb3cb12ec 197 // The flag "Flag_SpeedCal_Iterated" will be resetted at the end of function Control()
benson516 12:085f35babe21 198 if(Flag_SpeedCal_Iterated) return;
benson516 12:085f35babe21 199
benson516 8:fd6fb3cb12ec 200 // Speed
benson516 8:fd6fb3cb12ec 201 wheelSpeed.Calculate();
benson516 12:085f35babe21 202
benson516 9:d8157fbfcd2a 203 //
benson516 9:d8157fbfcd2a 204 if (reverse_rotationalSpeed)
benson516 9:d8157fbfcd2a 205 angularSpeed = -wheelSpeed.getAngularSpeed();
benson516 9:d8157fbfcd2a 206 else
benson516 9:d8157fbfcd2a 207 angularSpeed = wheelSpeed.getAngularSpeed();
benson516 9:d8157fbfcd2a 208 //
benson516 12:085f35babe21 209
benson516 8:fd6fb3cb12ec 210 // Flag
benson516 8:fd6fb3cb12ec 211 Flag_SpeedCal_Iterated = true;
benson516 12:085f35babe21 212
benson516 8:fd6fb3cb12ec 213 return angularSpeed;
benson516 8:fd6fb3cb12ec 214 }
benson516 8:fd6fb3cb12ec 215 float CURRENT_CONTROL::getAngularSpeed(void){
benson516 8:fd6fb3cb12ec 216 // return wheelSpeed.getAngularSpeed();
benson516 8:fd6fb3cb12ec 217 return angularSpeed;
benson516 8:fd6fb3cb12ec 218 }
benson516 8:fd6fb3cb12ec 219 float CURRENT_CONTROL::getAngularSpeed_deg_s(void){
benson516 12:085f35babe21 220
benson516 9:d8157fbfcd2a 221 if (reverse_rotationalSpeed)
benson516 9:d8157fbfcd2a 222 return -wheelSpeed.getAngularSpeed_deg_s();
benson516 9:d8157fbfcd2a 223 else
benson516 9:d8157fbfcd2a 224 return wheelSpeed.getAngularSpeed_deg_s();
benson516 9:d8157fbfcd2a 225 //
benson516 9:d8157fbfcd2a 226 // return wheelSpeed.getAngularSpeed_deg_s();
benson516 8:fd6fb3cb12ec 227 }
benson516 9:d8157fbfcd2a 228 //////////////////////////////// end Speed_IterateOnce
benson516 9:d8157fbfcd2a 229
benson516 10:9f5f4a22346c 230
benson516 9:d8157fbfcd2a 231 // Control
benson516 10:9f5f4a22346c 232 ///////////////////////////////////////////////////////////
benson516 14:67fc256efeb7 233 void CURRENT_CONTROL::TorqueControl(float TorqueRef, bool enable){
benson516 14:67fc256efeb7 234 Control(Kt_inv*TorqueRef, enable);
benson516 14:67fc256efeb7 235 }
benson516 11:31cd02611cd0 236 void CURRENT_CONTROL::Control(float curRef, bool enable)
adam_z 0:955aa05c968a 237 {
benson516 15:d9ccd6c92a21 238 // Input saturation
benson516 15:d9ccd6c92a21 239 curCommand = saturation(curRef,current_limit_H,current_limit_L);
benson516 15:d9ccd6c92a21 240
benson516 7:6794cfba3564 241 // Init check
benson516 7:6794cfba3564 242 OffsetInit();
benson516 12:085f35babe21 243 if (!Flag_Init) return;
benson516 7:6794cfba3564 244 //////////////////////////////////////
benson516 12:085f35babe21 245
benson516 7:6794cfba3564 246 // Get measurement
benson516 8:fd6fb3cb12ec 247 // Speed
benson516 8:fd6fb3cb12ec 248 Speed_IterateOnce();
benson516 8:fd6fb3cb12ec 249 // Motor current
benson516 7:6794cfba3564 250 GetCurrent();
benson516 12:085f35babe21 251
benson516 7:6794cfba3564 252 //--------------------------------//
benson516 12:085f35babe21 253
benson516 12:085f35babe21 254 if (enable){
benson516 12:085f35babe21 255 // PID
benson516 12:085f35babe21 256 pid.Compute_noWindUP(curRef, curFeedBack_filter);
benson516 12:085f35babe21 257
benson516 12:085f35babe21 258 // Voltage output with back-emf compensation
benson516 12:085f35babe21 259 voltage_out = pid.output + func_back_emf(angularSpeed);
benson516 12:085f35babe21 260 // voltage_out = 0.0 + func_back_emf(angularSpeed);
benson516 12:085f35babe21 261
benson516 12:085f35babe21 262 // Controlller output
benson516 11:31cd02611cd0 263 SetVoltage(voltage_out);
benson516 12:085f35babe21 264 }else{
benson516 12:085f35babe21 265 pid.Reset();
benson516 13:b5f85f926f22 266 voltage_out = 0.0;
benson516 11:31cd02611cd0 267 SetVoltage(0.0);
benson516 12:085f35babe21 268 }
benson516 7:6794cfba3564 269 //--------------------------------//
benson516 12:085f35babe21 270
benson516 5:7ccd2fb7ce7e 271 // Anti-windup
benson516 5:7ccd2fb7ce7e 272 pid.Anti_windup(delta_output);
benson516 12:085f35babe21 273
benson516 8:fd6fb3cb12ec 274 ///////////////////////////////////////
benson516 8:fd6fb3cb12ec 275 // Reset flag
benson516 8:fd6fb3cb12ec 276 Flag_SpeedCal_Iterated = false;
adam_z 1:c5973a56d474 277 }
benson516 10:9f5f4a22346c 278 /////////////////////////////////////////////////////////// end Control
benson516 10:9f5f4a22346c 279
benson516 10:9f5f4a22346c 280
benson516 5:7ccd2fb7ce7e 281 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 282 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 283 return (Ke*W_in);
adam_z 1:c5973a56d474 284 }
adam_z 3:c787d1c5ad6a 285
benson516 15:d9ccd6c92a21 286 // Elementary function (building block)
adam_z 1:c5973a56d474 287 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 288 {
adam_z 1:c5973a56d474 289 MotorPlus = ratio;
benson516 7:6794cfba3564 290 if(pwmIndex_ == PWM1){
benson516 7:6794cfba3564 291 TIM1->CCER |= 4;
benson516 7:6794cfba3564 292 }else if(pwmIndex_ == PWM2){
benson516 7:6794cfba3564 293 TIM1->CCER |= 64;
benson516 7:6794cfba3564 294 }
adam_z 2:562bd14dfd3a 295 }
benson516 9:d8157fbfcd2a 296 void CURRENT_CONTROL::SetVoltage(float volt)
benson516 9:d8157fbfcd2a 297 {
benson516 9:d8157fbfcd2a 298 // Output saturation and unit changing
benson516 9:d8157fbfcd2a 299 if (reverse_voltage)
benson516 9:d8157fbfcd2a 300 SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 9:d8157fbfcd2a 301 else
benson516 9:d8157fbfcd2a 302 SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 9:d8157fbfcd2a 303 }
benson516 9:d8157fbfcd2a 304 //
adam_z 2:562bd14dfd3a 305 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 306 {
benson516 7:6794cfba3564 307 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 308 return analogInValue;
adam_z 2:562bd14dfd3a 309 }
adam_z 2:562bd14dfd3a 310
adam_z 2:562bd14dfd3a 311 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 312 {
benson516 7:6794cfba3564 313 // Init check
benson516 12:085f35babe21 314 if (!Flag_Init) return 0.0;
benson516 12:085f35babe21 315
benson516 7:6794cfba3564 316 //-----------------------------------------//
benson516 9:d8157fbfcd2a 317 GetAnalogIn();
benson516 12:085f35babe21 318
benson516 7:6794cfba3564 319 // Unit transformation
benson516 9:d8157fbfcd2a 320 if (reverse_current){
benson516 9:d8157fbfcd2a 321 curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
benson516 9:d8157fbfcd2a 322 }else{
benson516 9:d8157fbfcd2a 323 curFeedBack = (analogInValue - currentOffset)*analog2Cur;
benson516 9:d8157fbfcd2a 324 }
benson516 9:d8157fbfcd2a 325 //
benson516 12:085f35babe21 326
benson516 7:6794cfba3564 327 // Low-pass filter
benson516 7:6794cfba3564 328 curFeedBack_filter = lpFilter.filter(curFeedBack);
benson516 12:085f35babe21 329
benson516 12:085f35babe21 330 return curFeedBack_filter; //curFeedBack;
adam_z 3:c787d1c5ad6a 331 }