Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@14:67fc256efeb7, 2016-12-26 (annotated)

Committer:
benson516
Date:
Mon Dec 26 07:52:43 2016 +0000
Revision:
14:67fc256efeb7
Parent:
13:b5f85f926f22
Child:
15:d9ccd6c92a21
Add TorqueControl, which accept the torque command instead of current command

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