Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@9:d8157fbfcd2a, 2016-12-21 (annotated)

Committer:
benson516
Date:
Wed Dec 21 17:39:56 2016 +0000
Revision:
9:d8157fbfcd2a
Parent:
8:fd6fb3cb12ec
Child:
10:9f5f4a22346c
ADD reversal control for each signal

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 8:fd6fb3cb12ec 54 lpFilter(samplingTime, 100.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 9:d8157fbfcd2a 208 // Control
benson516 9:d8157fbfcd2a 209 ////////////////////////////////
benson516 9:d8157fbfcd2a 210 void CURRENT_CONTROL::Control(float curRef)
adam_z 0:955aa05c968a 211 {
benson516 7:6794cfba3564 212 // Init check
benson516 7:6794cfba3564 213 OffsetInit();
benson516 7:6794cfba3564 214 if (!Flag_Init) return;
benson516 7:6794cfba3564 215 //////////////////////////////////////
benson516 5:7ccd2fb7ce7e 216
benson516 7:6794cfba3564 217 // Get measurement
benson516 8:fd6fb3cb12ec 218 // Speed
benson516 8:fd6fb3cb12ec 219 Speed_IterateOnce();
benson516 8:fd6fb3cb12ec 220 // Motor current
benson516 7:6794cfba3564 221 GetCurrent();
benson516 7:6794cfba3564 222
benson516 7:6794cfba3564 223 //--------------------------------//
benson516 5:7ccd2fb7ce7e 224
benson516 5:7ccd2fb7ce7e 225 // PID
benson516 7:6794cfba3564 226 pid.Compute_noWindUP(curRef, curFeedBack_filter);
benson516 7:6794cfba3564 227
benson516 7:6794cfba3564 228 // Voltage output with back-emf compensation
benson516 7:6794cfba3564 229 voltage_out = -pid.output + func_back_emf(angularSpeed);
benson516 5:7ccd2fb7ce7e 230
benson516 6:bae35ca64f10 231 // Output saturation and unit changing
benson516 9:d8157fbfcd2a 232 SetVoltage(voltage_out);
benson516 7:6794cfba3564 233
benson516 7:6794cfba3564 234
benson516 7:6794cfba3564 235 //--------------------------------//
benson516 5:7ccd2fb7ce7e 236
benson516 5:7ccd2fb7ce7e 237 // Anti-windup
benson516 5:7ccd2fb7ce7e 238 pid.Anti_windup(delta_output);
benson516 5:7ccd2fb7ce7e 239
benson516 8:fd6fb3cb12ec 240 ///////////////////////////////////////
benson516 8:fd6fb3cb12ec 241 // Reset flag
benson516 8:fd6fb3cb12ec 242 Flag_SpeedCal_Iterated = false;
adam_z 1:c5973a56d474 243 }
benson516 9:d8157fbfcd2a 244 //////////////////////////////// end Control
benson516 5:7ccd2fb7ce7e 245 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 246 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 247 return (Ke*W_in);
adam_z 1:c5973a56d474 248 }
adam_z 3:c787d1c5ad6a 249
adam_z 2:562bd14dfd3a 250 //****************for test************************//
adam_z 1:c5973a56d474 251 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 252 {
adam_z 1:c5973a56d474 253 MotorPlus = ratio;
benson516 7:6794cfba3564 254 if(pwmIndex_ == PWM1){
benson516 7:6794cfba3564 255 TIM1->CCER |= 4;
benson516 7:6794cfba3564 256 }else if(pwmIndex_ == PWM2){
benson516 7:6794cfba3564 257 TIM1->CCER |= 64;
benson516 7:6794cfba3564 258 }
adam_z 2:562bd14dfd3a 259 }
benson516 9:d8157fbfcd2a 260 void CURRENT_CONTROL::SetVoltage(float volt)
benson516 9:d8157fbfcd2a 261 {
benson516 9:d8157fbfcd2a 262 // Output saturation and unit changing
benson516 9:d8157fbfcd2a 263 if (reverse_voltage)
benson516 9:d8157fbfcd2a 264 SetPWMDuty( 0.5 - saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 9:d8157fbfcd2a 265 else
benson516 9:d8157fbfcd2a 266 SetPWMDuty( 0.5 + saturation( volt*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 9:d8157fbfcd2a 267 }
benson516 9:d8157fbfcd2a 268 //
adam_z 2:562bd14dfd3a 269 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 270 {
benson516 7:6794cfba3564 271 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 272 return analogInValue;
adam_z 2:562bd14dfd3a 273 }
adam_z 2:562bd14dfd3a 274
adam_z 2:562bd14dfd3a 275 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 276 {
benson516 7:6794cfba3564 277 // Init check
benson516 7:6794cfba3564 278 if (!Flag_Init) return 0.0;
benson516 7:6794cfba3564 279
benson516 7:6794cfba3564 280 //-----------------------------------------//
benson516 9:d8157fbfcd2a 281 GetAnalogIn();
benson516 7:6794cfba3564 282
benson516 7:6794cfba3564 283 // Unit transformation
benson516 9:d8157fbfcd2a 284 if (reverse_current){
benson516 9:d8157fbfcd2a 285 curFeedBack = -1*(analogInValue - currentOffset)*analog2Cur;
benson516 9:d8157fbfcd2a 286 }else{
benson516 9:d8157fbfcd2a 287 curFeedBack = (analogInValue - currentOffset)*analog2Cur;
benson516 9:d8157fbfcd2a 288 }
benson516 9:d8157fbfcd2a 289 //
benson516 7:6794cfba3564 290
benson516 7:6794cfba3564 291 // Low-pass filter
benson516 7:6794cfba3564 292 curFeedBack_filter = lpFilter.filter(curFeedBack);
benson516 7:6794cfba3564 293
benson516 9:d8157fbfcd2a 294 return curFeedBack_filter; //curFeedBack;
adam_z 3:c787d1c5ad6a 295 }
adam_z 3:c787d1c5ad6a 296
adam_z 3:c787d1c5ad6a 297