Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@13:b5f85f926f22, 2016-12-24 (annotated)

Committer:
benson516
Date:
Sat Dec 24 09:55:00 2016 +0000
Revision:
13:b5f85f926f22
Parent:
12:085f35babe21
Child:
14:67fc256efeb7
Fix the bug

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 8:fd6fb3cb12ec 78 voltage_out = 0.0;
benson516 12:085f35babe21 79
benson516 12:085f35babe21 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 12:085f35babe21 86
benson516 8:fd6fb3cb12ec 87 // Speed
benson516 8:fd6fb3cb12ec 88 angularSpeed = 0.0;
benson516 8:fd6fb3cb12ec 89 Flag_SpeedCal_Iterated = false;
benson516 12:085f35babe21 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 12:085f35babe21 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 12:085f35babe21 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 12:085f35babe21 123
benson516 7:6794cfba3564 124 Init_count++;
benson516 9:d8157fbfcd2a 125 /*
benson516 7:6794cfba3564 126 Accumulated_offset += GetAnalogIn();
benson516 12:085f35babe21 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 12:085f35babe21 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 12:085f35babe21 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 12:085f35babe21 148
benson516 12:085f35babe21 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 12:085f35babe21 155
benson516 12:085f35babe21 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 12:085f35babe21 164 }else if( input_value < limit_L ){
benson516 5:7ccd2fb7ce7e 165 delta = limit_L - input_value;
benson516 12:085f35babe21 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 12:085f35babe21 176 if(Flag_SpeedCal_Iterated) return;
benson516 12:085f35babe21 177
benson516 8:fd6fb3cb12ec 178 // Speed
benson516 8:fd6fb3cb12ec 179 wheelSpeed.Calculate();
benson516 12:085f35babe21 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 12:085f35babe21 187
benson516 8:fd6fb3cb12ec 188 // Flag
benson516 8:fd6fb3cb12ec 189 Flag_SpeedCal_Iterated = true;
benson516 12:085f35babe21 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 12:085f35babe21 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 12:085f35babe21 215 if (!Flag_Init) return;
benson516 7:6794cfba3564 216 //////////////////////////////////////
benson516 12:085f35babe21 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 12:085f35babe21 223
benson516 7:6794cfba3564 224 //--------------------------------//
benson516 12:085f35babe21 225
benson516 12:085f35babe21 226 if (enable){
benson516 12:085f35babe21 227 // PID
benson516 12:085f35babe21 228 pid.Compute_noWindUP(curRef, curFeedBack_filter);
benson516 12:085f35babe21 229
benson516 12:085f35babe21 230 // Voltage output with back-emf compensation
benson516 12:085f35babe21 231 voltage_out = pid.output + func_back_emf(angularSpeed);
benson516 12:085f35babe21 232 // voltage_out = 0.0 + func_back_emf(angularSpeed);
benson516 12:085f35babe21 233
benson516 12:085f35babe21 234 // Controlller output
benson516 11:31cd02611cd0 235 SetVoltage(voltage_out);
benson516 12:085f35babe21 236 }else{
benson516 12:085f35babe21 237 pid.Reset();
benson516 13:b5f85f926f22 238 voltage_out = 0.0;
benson516 11:31cd02611cd0 239 SetVoltage(0.0);
benson516 12:085f35babe21 240 }
benson516 7:6794cfba3564 241 //--------------------------------//
benson516 12:085f35babe21 242
benson516 5:7ccd2fb7ce7e 243 // Anti-windup
benson516 5:7ccd2fb7ce7e 244 pid.Anti_windup(delta_output);
benson516 12:085f35babe21 245
benson516 8:fd6fb3cb12ec 246 ///////////////////////////////////////
benson516 8:fd6fb3cb12ec 247 // Reset flag
benson516 8:fd6fb3cb12ec 248 Flag_SpeedCal_Iterated = false;
adam_z 1:c5973a56d474 249 }
benson516 10:9f5f4a22346c 250 /////////////////////////////////////////////////////////// end Control
benson516 10:9f5f4a22346c 251
benson516 10:9f5f4a22346c 252
benson516 5:7ccd2fb7ce7e 253 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 254 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 255 return (Ke*W_in);
adam_z 1:c5973a56d474 256 }
adam_z 3:c787d1c5ad6a 257
adam_z 2:562bd14dfd3a 258 //****************for test************************//
adam_z 1:c5973a56d474 259 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 260 {
adam_z 1:c5973a56d474 261 MotorPlus = ratio;
benson516 7:6794cfba3564 262 if(pwmIndex_ == PWM1){
benson516 7:6794cfba3564 263 TIM1->CCER |= 4;
benson516 7:6794cfba3564 264 }else if(pwmIndex_ == PWM2){
benson516 7:6794cfba3564 265 TIM1->CCER |= 64;
benson516 7:6794cfba3564 266 }
adam_z 2:562bd14dfd3a 267 }
benson516 9:d8157fbfcd2a 268 void CURRENT_CONTROL::SetVoltage(float volt)
benson516 9:d8157fbfcd2a 269 {
benson516 9:d8157fbfcd2a 270 // Output saturation and unit changing
benson516 9:d8157fbfcd2a 271 if (reverse_voltage)
benson516 9:d8157fbfcd2a 272 SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 9:d8157fbfcd2a 273 else
benson516 9:d8157fbfcd2a 274 SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 9:d8157fbfcd2a 275 }
benson516 9:d8157fbfcd2a 276 //
adam_z 2:562bd14dfd3a 277 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 278 {
benson516 7:6794cfba3564 279 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 280 return analogInValue;
adam_z 2:562bd14dfd3a 281 }
adam_z 2:562bd14dfd3a 282
adam_z 2:562bd14dfd3a 283 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 284 {
benson516 7:6794cfba3564 285 // Init check
benson516 12:085f35babe21 286 if (!Flag_Init) return 0.0;
benson516 12:085f35babe21 287
benson516 7:6794cfba3564 288 //-----------------------------------------//
benson516 9:d8157fbfcd2a 289 GetAnalogIn();
benson516 12:085f35babe21 290
benson516 7:6794cfba3564 291 // Unit transformation
benson516 9:d8157fbfcd2a 292 if (reverse_current){
benson516 9:d8157fbfcd2a 293 curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
benson516 9:d8157fbfcd2a 294 }else{
benson516 9:d8157fbfcd2a 295 curFeedBack = (analogInValue - currentOffset)*analog2Cur;
benson516 9:d8157fbfcd2a 296 }
benson516 9:d8157fbfcd2a 297 //
benson516 12:085f35babe21 298
benson516 7:6794cfba3564 299 // Low-pass filter
benson516 7:6794cfba3564 300 curFeedBack_filter = lpFilter.filter(curFeedBack);
benson516 12:085f35babe21 301
benson516 12:085f35babe21 302 return curFeedBack_filter; //curFeedBack;
adam_z 3:c787d1c5ad6a 303 }