Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@17:7f16cc2e46fc, 2017-01-04 (annotated)

Committer:
benson516
Date:
Wed Jan 04 14:39:07 2017 +0000
Revision:
17:7f16cc2e46fc
Parent:
16:6e3bcd373f9d
Child:
18:76383af914dc
Change the cut-off frequency of the current filter

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