Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@7:6794cfba3564, 2016-12-19 (annotated)

Committer:
benson516
Date:
Mon Dec 19 13:26:05 2016 +0000
Revision:
7:6794cfba3564
Parent:
6:bae35ca64f10
Child:
8:fd6fb3cb12ec
Add OffsetInit()

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 6:bae35ca64f10 42 float Kp, float Ki, float Kd, float Ka,
benson516 5:7ccd2fb7ce7e 43 float samplingTime) :
benson516 5:7ccd2fb7ce7e 44 currentAnalogIn(curChannel),
adam_z 0:955aa05c968a 45 MotorPlus(PwmChannel1),
adam_z 0:955aa05c968a 46 MotorMinus(PwmChannel2),
adam_z 3:c787d1c5ad6a 47 pid(Kp,Ki,Kd,samplingTime),
benson516 7:6794cfba3564 48 lpFilter(samplingTime, 100.0), // 1.5915 Hz = 10 rad/s
benson516 7:6794cfba3564 49 curFeedBack(0.0),
benson516 7:6794cfba3564 50 curFeedBack_filter(0.0),
benson516 7:6794cfba3564 51 voltage_out(0.0)
adam_z 3:c787d1c5ad6a 52
adam_z 0:955aa05c968a 53 {
adam_z 1:c5973a56d474 54 pwmIndex_ = pwmIndex;
adam_z 1:c5973a56d474 55
adam_z 0:955aa05c968a 56 Ts = samplingTime;
benson516 5:7ccd2fb7ce7e 57
adam_z 0:955aa05c968a 58 //setup motor PWM parameters
benson516 7:6794cfba3564 59 MotorPlus.period_us(50); //set the pwm period = 50 us, where the frequency = 20kHz
adam_z 1:c5973a56d474 60 MotorPlus.write(0.5); //duty ratio = 0.5 in complementary output mode -> static
adam_z 1:c5973a56d474 61 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 62 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
adam_z 3:c787d1c5ad6a 63
benson516 7:6794cfba3564 64 //
benson516 7:6794cfba3564 65 Flag_Init = false;
benson516 7:6794cfba3564 66 Init_count = 0;
benson516 7:6794cfba3564 67 Accumulated_offset = 0.0;
benson516 6:bae35ca64f10 68
benson516 5:7ccd2fb7ce7e 69 //
benson516 5:7ccd2fb7ce7e 70 currentOffset = 0.0;
benson516 7:6794cfba3564 71 //
benson516 5:7ccd2fb7ce7e 72 delta_output = 0.0;
benson516 6:bae35ca64f10 73
benson516 6:bae35ca64f10 74 // Set PID's parameters
benson516 6:bae35ca64f10 75 /////////////////////
benson516 6:bae35ca64f10 76 pid.Kp = Kp;
benson516 6:bae35ca64f10 77 pid.Ki = Ki;
benson516 6:bae35ca64f10 78 pid.Kd = Kd;
benson516 6:bae35ca64f10 79 pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
benson516 5:7ccd2fb7ce7e 80 }
benson516 7:6794cfba3564 81 void CURRENT_CONTROL::OffsetInit(void){
benson516 7:6794cfba3564 82 if (Flag_Init) return;
benson516 7:6794cfba3564 83 //
benson516 7:6794cfba3564 84 Init_count++;
benson516 7:6794cfba3564 85 Accumulated_offset += GetAnalogIn();
benson516 7:6794cfba3564 86
benson516 7:6794cfba3564 87 if (Init_count >= 500){ // Fixed time
benson516 7:6794cfba3564 88 currentOffset = Accumulated_offset/float(Init_count);
benson516 7:6794cfba3564 89 Flag_Init = true;
benson516 7:6794cfba3564 90 }
benson516 7:6794cfba3564 91 }
benson516 5:7ccd2fb7ce7e 92 //
benson516 5:7ccd2fb7ce7e 93 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
benson516 5:7ccd2fb7ce7e 94 {
benson516 5:7ccd2fb7ce7e 95 analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
benson516 5:7ccd2fb7ce7e 96 Ke = angSpeed2BackEmf;
benson516 5:7ccd2fb7ce7e 97 voltage2Duty = voltage2DutyRatio;
adam_z 3:c787d1c5ad6a 98
adam_z 0:955aa05c968a 99 }
benson516 5:7ccd2fb7ce7e 100 //
benson516 5:7ccd2fb7ce7e 101 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
benson516 5:7ccd2fb7ce7e 102 if( input_value > limit_H ){
benson516 5:7ccd2fb7ce7e 103 delta = limit_H - input_value;
benson516 5:7ccd2fb7ce7e 104 input_value = limit_H;
benson516 5:7ccd2fb7ce7e 105 }else if( input_value < limit_L ){
benson516 5:7ccd2fb7ce7e 106 delta = limit_L - input_value;
benson516 5:7ccd2fb7ce7e 107 input_value = limit_L;
benson516 5:7ccd2fb7ce7e 108 }else{
benson516 5:7ccd2fb7ce7e 109 delta = 0.0;
benson516 5:7ccd2fb7ce7e 110 }
benson516 5:7ccd2fb7ce7e 111 return input_value;
adam_z 4:1a6ba05e7736 112 }
benson516 5:7ccd2fb7ce7e 113 //
adam_z 4:1a6ba05e7736 114 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
adam_z 0:955aa05c968a 115 {
benson516 7:6794cfba3564 116 // Init check
benson516 7:6794cfba3564 117 OffsetInit();
benson516 7:6794cfba3564 118 if (!Flag_Init) return;
benson516 7:6794cfba3564 119 //////////////////////////////////////
benson516 5:7ccd2fb7ce7e 120
benson516 7:6794cfba3564 121 // Get measurement
benson516 7:6794cfba3564 122 GetCurrent();
benson516 7:6794cfba3564 123
benson516 7:6794cfba3564 124 //--------------------------------//
benson516 5:7ccd2fb7ce7e 125
benson516 5:7ccd2fb7ce7e 126 // PID
benson516 7:6794cfba3564 127 pid.Compute_noWindUP(curRef, curFeedBack_filter);
benson516 7:6794cfba3564 128
benson516 7:6794cfba3564 129 // Voltage output with back-emf compensation
benson516 7:6794cfba3564 130 voltage_out = -pid.output + func_back_emf(angularSpeed);
benson516 5:7ccd2fb7ce7e 131
benson516 6:bae35ca64f10 132 // Output saturation and unit changing
benson516 7:6794cfba3564 133 SetPWMDuty( 0.5 + saturation( voltage_out*voltage2Duty, delta_output, 0.5, -0.5) );
benson516 7:6794cfba3564 134
benson516 7:6794cfba3564 135
benson516 7:6794cfba3564 136 //--------------------------------//
benson516 5:7ccd2fb7ce7e 137
benson516 5:7ccd2fb7ce7e 138 // Anti-windup
benson516 5:7ccd2fb7ce7e 139 pid.Anti_windup(delta_output);
benson516 5:7ccd2fb7ce7e 140
benson516 7:6794cfba3564 141
adam_z 1:c5973a56d474 142 }
benson516 5:7ccd2fb7ce7e 143 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 144 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 145 return (Ke*W_in);
adam_z 1:c5973a56d474 146 }
adam_z 3:c787d1c5ad6a 147
adam_z 2:562bd14dfd3a 148 //****************for test************************//
adam_z 1:c5973a56d474 149 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 150 {
adam_z 1:c5973a56d474 151 MotorPlus = ratio;
benson516 7:6794cfba3564 152 if(pwmIndex_ == PWM1){
benson516 7:6794cfba3564 153 TIM1->CCER |= 4;
benson516 7:6794cfba3564 154 }else if(pwmIndex_ == PWM2){
benson516 7:6794cfba3564 155 TIM1->CCER |= 64;
benson516 7:6794cfba3564 156 }
adam_z 2:562bd14dfd3a 157 }
adam_z 2:562bd14dfd3a 158
adam_z 2:562bd14dfd3a 159 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 160 {
benson516 7:6794cfba3564 161 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 162 return analogInValue;
adam_z 2:562bd14dfd3a 163 }
adam_z 2:562bd14dfd3a 164
adam_z 2:562bd14dfd3a 165 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 166 {
benson516 7:6794cfba3564 167 // Init check
benson516 7:6794cfba3564 168 if (!Flag_Init) return 0.0;
benson516 7:6794cfba3564 169
benson516 7:6794cfba3564 170 //-----------------------------------------//
benson516 7:6794cfba3564 171 analogInValue = currentAnalogIn.read();
benson516 7:6794cfba3564 172
benson516 7:6794cfba3564 173 // Unit transformation
benson516 7:6794cfba3564 174 curFeedBack = (analogInValue - currentOffset)*analog2Cur;
benson516 7:6794cfba3564 175
benson516 7:6794cfba3564 176 // Low-pass filter
benson516 7:6794cfba3564 177 curFeedBack_filter = lpFilter.filter(curFeedBack);
benson516 7:6794cfba3564 178
benson516 7:6794cfba3564 179 return curFeedBack; //curFeedBack_filter;
adam_z 3:c787d1c5ad6a 180 }
adam_z 3:c787d1c5ad6a 181
adam_z 3:c787d1c5ad6a 182