Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@19:24605ba48462, 2017-02-07 (annotated)

Committer:
benson516
Date:
Tue Feb 07 09:52:56 2017 +0000
Revision:
19:24605ba48462
Parent:
18:76383af914dc
Child:
20:559db13dc85a
test succeed

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