Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@11:31cd02611cd0, 2016-12-22 (annotated)

Committer:
benson516
Date:
Thu Dec 22 19:22:59 2016 +0000
Revision:
11:31cd02611cd0
Parent:
10:9f5f4a22346c
Child:
12:085f35babe21
Current control - done

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