Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@8:fd6fb3cb12ec, 2016-12-19 (annotated)

Committer:
benson516
Date:
Mon Dec 19 15:27:13 2016 +0000
Revision:
8:fd6fb3cb12ec
Parent:
7:6794cfba3564
Child:
9:d8157fbfcd2a
Combine "QEI" into "CURRENT_CONTROL"

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 7:6794cfba3564 10 output = 0.0;
benson516 7:6794cfba3564 11
benson516 7:6794cfba3564 12 //
benson516 7:6794cfba3564 13 Flag_Init = false;
benson516 5:7ccd2fb7ce7e 14 }
adam_z 0:955aa05c968a 15
benson516 5:7ccd2fb7ce7e 16 float LPF::filter(float input)
benson516 5:7ccd2fb7ce7e 17 {
benson516 7:6794cfba3564 18 // Initialization
benson516 7:6794cfba3564 19 if (!Flag_Init){
benson516 7:6794cfba3564 20 reset(input);
benson516 7:6794cfba3564 21 Flag_Init = true;
benson516 7:6794cfba3564 22 return output;
benson516 7:6794cfba3564 23 }
benson516 7:6794cfba3564 24
benson516 5:7ccd2fb7ce7e 25 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
benson516 5:7ccd2fb7ce7e 26 output += alpha_Ts*(input - output);
benson516 5:7ccd2fb7ce7e 27 return output;
benson516 5:7ccd2fb7ce7e 28 }
benson516 7:6794cfba3564 29 void LPF::reset(float input)
benson516 7:6794cfba3564 30 {
benson516 7:6794cfba3564 31 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
benson516 7:6794cfba3564 32 output = input;
benson516 7:6794cfba3564 33 return;
benson516 7:6794cfba3564 34 }
benson516 5:7ccd2fb7ce7e 35
benson516 5:7ccd2fb7ce7e 36
benson516 5:7ccd2fb7ce7e 37 //================== CURRENT_CONTROL =================//
adam_z 1:c5973a56d474 38 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
adam_z 1:c5973a56d474 39 PinName PwmChannel1,
adam_z 1:c5973a56d474 40 PinName PwmChannel2,
adam_z 1:c5973a56d474 41 PWMIndex pwmIndex,
benson516 8:fd6fb3cb12ec 42 PinName QEI_A,
benson516 8:fd6fb3cb12ec 43 PinName QEI_B,
benson516 8:fd6fb3cb12ec 44 float pulsesPerRev,
benson516 8:fd6fb3cb12ec 45 int arraysize,
benson516 5:7ccd2fb7ce7e 46 float samplingTime) :
benson516 5:7ccd2fb7ce7e 47 currentAnalogIn(curChannel),
adam_z 0:955aa05c968a 48 MotorPlus(PwmChannel1),
adam_z 0:955aa05c968a 49 MotorMinus(PwmChannel2),
benson516 8:fd6fb3cb12ec 50 wheelSpeed(QEI_A, QEI_B, NC, pulsesPerRev, arraysize, samplingTime, QEI::X4_ENCODING), //(pin1, pin2, pin3, pulsesPerRev, arraysize, sampletime, pulses)
benson516 8:fd6fb3cb12ec 51 pid(0.0,0.0,0.0,samplingTime),
benson516 8:fd6fb3cb12ec 52 lpFilter(samplingTime, 100.0) // 1.5915 Hz = 10 rad/s
benson516 8:fd6fb3cb12ec 53
adam_z 3:c787d1c5ad6a 54
adam_z 0:955aa05c968a 55 {
adam_z 1:c5973a56d474 56 pwmIndex_ = pwmIndex;
adam_z 1:c5973a56d474 57
adam_z 0:955aa05c968a 58 Ts = samplingTime;
benson516 5:7ccd2fb7ce7e 59
adam_z 0:955aa05c968a 60 //setup motor PWM parameters
benson516 7:6794cfba3564 61 MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz
benson516 8:fd6fb3cb12ec 62 //
benson516 8:fd6fb3cb12ec 63 SetPWMDuty(0.5);
adam_z 3:c787d1c5ad6a 64
benson516 7:6794cfba3564 65 //
benson516 7:6794cfba3564 66 Flag_Init = false;
benson516 7:6794cfba3564 67 Init_count = 0;
benson516 7:6794cfba3564 68 Accumulated_offset = 0.0;
benson516 6:bae35ca64f10 69
benson516 5:7ccd2fb7ce7e 70 //
benson516 5:7ccd2fb7ce7e 71 currentOffset = 0.0;
benson516 7:6794cfba3564 72 //
benson516 5:7ccd2fb7ce7e 73 delta_output = 0.0;
benson516 8:fd6fb3cb12ec 74 curFeedBack = 0.0;
benson516 8:fd6fb3cb12ec 75 curFeedBack_filter = 0.0;
benson516 8:fd6fb3cb12ec 76 voltage_out = 0.0;
benson516 6:bae35ca64f10 77
benson516 6:bae35ca64f10 78 // Set PID's parameters
benson516 6:bae35ca64f10 79 /////////////////////
benson516 8:fd6fb3cb12ec 80 pid.Kp = 0.0;
benson516 8:fd6fb3cb12ec 81 pid.Ki = 0.0;
benson516 8:fd6fb3cb12ec 82 pid.Kd = 0.0;
benson516 8:fd6fb3cb12ec 83 pid.Ka = 0.0; // Gain for anti-windup // Ka = 0.0017
benson516 8:fd6fb3cb12ec 84
benson516 8:fd6fb3cb12ec 85 // Speed
benson516 8:fd6fb3cb12ec 86 angularSpeed = 0.0;
benson516 8:fd6fb3cb12ec 87 Flag_SpeedCal_Iterated = false;
benson516 8:fd6fb3cb12ec 88 }
benson516 8:fd6fb3cb12ec 89 //
benson516 8:fd6fb3cb12ec 90 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
benson516 8:fd6fb3cb12ec 91 {
benson516 8:fd6fb3cb12ec 92 analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
benson516 8:fd6fb3cb12ec 93 Ke = angSpeed2BackEmf;
benson516 8:fd6fb3cb12ec 94 voltage2Duty = voltage2DutyRatio;
benson516 8:fd6fb3cb12ec 95 }
benson516 8:fd6fb3cb12ec 96 //
benson516 8:fd6fb3cb12ec 97 void CURRENT_CONTROL::SetGain(float Kp, float Ki, float Kd, float Ka)
benson516 8:fd6fb3cb12ec 98 {
benson516 8:fd6fb3cb12ec 99 // Set PID's parameters
benson516 8:fd6fb3cb12ec 100 /////////////////////
benson516 6:bae35ca64f10 101 pid.Kp = Kp;
benson516 6:bae35ca64f10 102 pid.Ki = Ki;
benson516 6:bae35ca64f10 103 pid.Kd = Kd;
benson516 6:bae35ca64f10 104 pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
benson516 5:7ccd2fb7ce7e 105 }
benson516 8:fd6fb3cb12ec 106 //
benson516 7:6794cfba3564 107 void CURRENT_CONTROL::OffsetInit(void){
benson516 7:6794cfba3564 108 if (Flag_Init) return;
benson516 7:6794cfba3564 109 //
benson516 7:6794cfba3564 110 Init_count++;
benson516 7:6794cfba3564 111 Accumulated_offset += GetAnalogIn();
benson516 7:6794cfba3564 112
benson516 7:6794cfba3564 113 if (Init_count >= 500){ // Fixed time
benson516 8:fd6fb3cb12ec 114 currentOffset = Accumulated_offset / float(Init_count);
benson516 7:6794cfba3564 115 Flag_Init = true;
benson516 7:6794cfba3564 116 }
benson516 7:6794cfba3564 117 }
benson516 8:fd6fb3cb12ec 118
benson516 5:7ccd2fb7ce7e 119 //
benson516 5:7ccd2fb7ce7e 120 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
benson516 5:7ccd2fb7ce7e 121 if( input_value > limit_H ){
benson516 5:7ccd2fb7ce7e 122 delta = limit_H - input_value;
benson516 5:7ccd2fb7ce7e 123 input_value = limit_H;
benson516 5:7ccd2fb7ce7e 124 }else if( input_value < limit_L ){
benson516 5:7ccd2fb7ce7e 125 delta = limit_L - input_value;
benson516 5:7ccd2fb7ce7e 126 input_value = limit_L;
benson516 5:7ccd2fb7ce7e 127 }else{
benson516 5:7ccd2fb7ce7e 128 delta = 0.0;
benson516 5:7ccd2fb7ce7e 129 }
benson516 5:7ccd2fb7ce7e 130 return input_value;
adam_z 4:1a6ba05e7736 131 }
benson516 5:7ccd2fb7ce7e 132 //
benson516 8:fd6fb3cb12ec 133
benson516 8:fd6fb3cb12ec 134 float CURRENT_CONTROL::Speed_IterateOnce(void){
benson516 8:fd6fb3cb12ec 135 // The flag "Flag_SpeedCal_Iterated" will be resetted at the end of function Control()
benson516 8:fd6fb3cb12ec 136 if(Flag_SpeedCal_Iterated) return;
benson516 8:fd6fb3cb12ec 137
benson516 8:fd6fb3cb12ec 138 // Speed
benson516 8:fd6fb3cb12ec 139 wheelSpeed.Calculate();
benson516 8:fd6fb3cb12ec 140 angularSpeed = wheelSpeed.getAngularSpeed();
benson516 8:fd6fb3cb12ec 141
benson516 8:fd6fb3cb12ec 142 // Flag
benson516 8:fd6fb3cb12ec 143 Flag_SpeedCal_Iterated = true;
benson516 8:fd6fb3cb12ec 144
benson516 8:fd6fb3cb12ec 145 return angularSpeed;
benson516 8:fd6fb3cb12ec 146 }
benson516 8:fd6fb3cb12ec 147 float CURRENT_CONTROL::getAngularSpeed(void){
benson516 8:fd6fb3cb12ec 148 // return wheelSpeed.getAngularSpeed();
benson516 8:fd6fb3cb12ec 149 return angularSpeed;
benson516 8:fd6fb3cb12ec 150 }
benson516 8:fd6fb3cb12ec 151 float CURRENT_CONTROL::getAngularSpeed_deg_s(void){
benson516 8:fd6fb3cb12ec 152 return wheelSpeed.getAngularSpeed_deg_s();
benson516 8:fd6fb3cb12ec 153 }
benson516 8:fd6fb3cb12ec 154 //
benson516 8:fd6fb3cb12ec 155 void CURRENT_CONTROL::Control(float curRef, float angularSpeed_in)
adam_z 0:955aa05c968a 156 {
benson516 7:6794cfba3564 157 // Init check
benson516 7:6794cfba3564 158 OffsetInit();
benson516 7:6794cfba3564 159 if (!Flag_Init) return;
benson516 7:6794cfba3564 160 //////////////////////////////////////
benson516 5:7ccd2fb7ce7e 161
benson516 7:6794cfba3564 162 // Get measurement
benson516 8:fd6fb3cb12ec 163 // Speed
benson516 8:fd6fb3cb12ec 164 Speed_IterateOnce();
benson516 8:fd6fb3cb12ec 165 // Motor current
benson516 7:6794cfba3564 166 GetCurrent();
benson516 7:6794cfba3564 167
benson516 7:6794cfba3564 168 //--------------------------------//
benson516 5:7ccd2fb7ce7e 169
benson516 5:7ccd2fb7ce7e 170 // PID
benson516 7:6794cfba3564 171 pid.Compute_noWindUP(curRef, curFeedBack_filter);
benson516 7:6794cfba3564 172
benson516 7:6794cfba3564 173 // Voltage output with back-emf compensation
benson516 7:6794cfba3564 174 voltage_out = -pid.output + func_back_emf(angularSpeed);
benson516 5:7ccd2fb7ce7e 175
benson516 6:bae35ca64f10 176 // Output saturation and unit changing
benson516 7:6794cfba3564 177 SetPWMDuty( 0.5 + saturation( voltage_out*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 7:6794cfba3564 178
benson516 7:6794cfba3564 179
benson516 7:6794cfba3564 180 //--------------------------------//
benson516 5:7ccd2fb7ce7e 181
benson516 5:7ccd2fb7ce7e 182 // Anti-windup
benson516 5:7ccd2fb7ce7e 183 pid.Anti_windup(delta_output);
benson516 5:7ccd2fb7ce7e 184
benson516 8:fd6fb3cb12ec 185 ///////////////////////////////////////
benson516 8:fd6fb3cb12ec 186 // Reset flag
benson516 8:fd6fb3cb12ec 187 Flag_SpeedCal_Iterated = false;
adam_z 1:c5973a56d474 188 }
benson516 5:7ccd2fb7ce7e 189 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 190 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 191 return (Ke*W_in);
adam_z 1:c5973a56d474 192 }
adam_z 3:c787d1c5ad6a 193
adam_z 2:562bd14dfd3a 194 //****************for test************************//
adam_z 1:c5973a56d474 195 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 196 {
adam_z 1:c5973a56d474 197 MotorPlus = ratio;
benson516 7:6794cfba3564 198 if(pwmIndex_ == PWM1){
benson516 7:6794cfba3564 199 TIM1->CCER |= 4;
benson516 7:6794cfba3564 200 }else if(pwmIndex_ == PWM2){
benson516 7:6794cfba3564 201 TIM1->CCER |= 64;
benson516 7:6794cfba3564 202 }
adam_z 2:562bd14dfd3a 203 }
adam_z 2:562bd14dfd3a 204
adam_z 2:562bd14dfd3a 205 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 206 {
benson516 7:6794cfba3564 207 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 208 return analogInValue;
adam_z 2:562bd14dfd3a 209 }
adam_z 2:562bd14dfd3a 210
adam_z 2:562bd14dfd3a 211 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 212 {
benson516 7:6794cfba3564 213 // Init check
benson516 7:6794cfba3564 214 if (!Flag_Init) return 0.0;
benson516 7:6794cfba3564 215
benson516 7:6794cfba3564 216 //-----------------------------------------//
benson516 7:6794cfba3564 217 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 218
benson516 7:6794cfba3564 219 // Unit transformation
benson516 7:6794cfba3564 220 curFeedBack = (analogInValue - currentOffset)*analog2Cur;
benson516 7:6794cfba3564 221
benson516 7:6794cfba3564 222 // Low-pass filter
benson516 7:6794cfba3564 223 curFeedBack_filter = lpFilter.filter(curFeedBack);
benson516 7:6794cfba3564 224
benson516 7:6794cfba3564 225 return curFeedBack; //curFeedBack_filter;
adam_z 3:c787d1c5ad6a 226 }
adam_z 3:c787d1c5ad6a 227
adam_z 3:c787d1c5ad6a 228