Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@6:bae35ca64f10, 2016-12-15 (annotated)

Committer:
benson516
Date:
Thu Dec 15 23:16:09 2016 +0000
Revision:
6:bae35ca64f10
Parent:
5:7ccd2fb7ce7e
Child:
7:6794cfba3564
Another modification

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 5:7ccd2fb7ce7e 10 }
adam_z 0:955aa05c968a 11
benson516 5:7ccd2fb7ce7e 12 float LPF::filter(float input)
benson516 5:7ccd2fb7ce7e 13 {
benson516 5:7ccd2fb7ce7e 14 // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
benson516 5:7ccd2fb7ce7e 15 output += alpha_Ts*(input - output);
benson516 5:7ccd2fb7ce7e 16 return output;
benson516 5:7ccd2fb7ce7e 17 }
benson516 5:7ccd2fb7ce7e 18
benson516 5:7ccd2fb7ce7e 19
benson516 5:7ccd2fb7ce7e 20 //================== CURRENT_CONTROL =================//
adam_z 1:c5973a56d474 21 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
adam_z 1:c5973a56d474 22 PinName PwmChannel1,
adam_z 1:c5973a56d474 23 PinName PwmChannel2,
adam_z 1:c5973a56d474 24 PWMIndex pwmIndex,
benson516 6:bae35ca64f10 25 float Kp, float Ki, float Kd, float Ka,
benson516 5:7ccd2fb7ce7e 26 float samplingTime) :
benson516 5:7ccd2fb7ce7e 27 currentAnalogIn(curChannel),
adam_z 0:955aa05c968a 28 MotorPlus(PwmChannel1),
adam_z 0:955aa05c968a 29 MotorMinus(PwmChannel2),
adam_z 3:c787d1c5ad6a 30 pid(Kp,Ki,Kd,samplingTime),
benson516 5:7ccd2fb7ce7e 31 lpFilter(samplingTime, 1.5915) // 10 rad/s
adam_z 3:c787d1c5ad6a 32
adam_z 0:955aa05c968a 33 {
adam_z 1:c5973a56d474 34 pwmIndex_ = pwmIndex;
adam_z 1:c5973a56d474 35
adam_z 0:955aa05c968a 36 Ts = samplingTime;
benson516 5:7ccd2fb7ce7e 37
adam_z 0:955aa05c968a 38 //setup motor PWM parameters
adam_z 4:1a6ba05e7736 39 MotorPlus.period_us(15);//default period equals to 25 microseconds
adam_z 1:c5973a56d474 40 MotorPlus.write(0.5); //duty ratio = 0.5 in complementary output mode -> static
adam_z 1:c5973a56d474 41 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 42 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
adam_z 3:c787d1c5ad6a 43
benson516 6:bae35ca64f10 44
benson516 5:7ccd2fb7ce7e 45 //
benson516 5:7ccd2fb7ce7e 46 currentOffset = 0.0;
benson516 5:7ccd2fb7ce7e 47 delta_output = 0.0;
benson516 6:bae35ca64f10 48
benson516 6:bae35ca64f10 49 // Set PID's parameters
benson516 6:bae35ca64f10 50 /////////////////////
benson516 6:bae35ca64f10 51 pid.Kp = Kp;
benson516 6:bae35ca64f10 52 pid.Ki = Ki;
benson516 6:bae35ca64f10 53 pid.Kd = Kd;
benson516 6:bae35ca64f10 54 pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
benson516 5:7ccd2fb7ce7e 55 }
benson516 5:7ccd2fb7ce7e 56 //
benson516 5:7ccd2fb7ce7e 57 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
benson516 5:7ccd2fb7ce7e 58 {
benson516 5:7ccd2fb7ce7e 59 analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
benson516 5:7ccd2fb7ce7e 60 Ke = angSpeed2BackEmf;
benson516 5:7ccd2fb7ce7e 61 voltage2Duty = voltage2DutyRatio;
adam_z 3:c787d1c5ad6a 62
adam_z 0:955aa05c968a 63 }
benson516 5:7ccd2fb7ce7e 64 //
benson516 5:7ccd2fb7ce7e 65 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
benson516 5:7ccd2fb7ce7e 66 if( input_value > limit_H ){
benson516 5:7ccd2fb7ce7e 67 delta = limit_H - input_value;
benson516 5:7ccd2fb7ce7e 68 input_value = limit_H;
benson516 5:7ccd2fb7ce7e 69 }else if( input_value < limit_L ){
benson516 5:7ccd2fb7ce7e 70 delta = limit_L - input_value;
benson516 5:7ccd2fb7ce7e 71 input_value = limit_L;
benson516 5:7ccd2fb7ce7e 72 }else{
benson516 5:7ccd2fb7ce7e 73 delta = 0.0;
benson516 5:7ccd2fb7ce7e 74 }
benson516 5:7ccd2fb7ce7e 75 return input_value;
adam_z 4:1a6ba05e7736 76 }
benson516 5:7ccd2fb7ce7e 77 //
adam_z 4:1a6ba05e7736 78 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
adam_z 0:955aa05c968a 79 {
adam_z 2:562bd14dfd3a 80 analogInValue = currentAnalogIn.read();
benson516 5:7ccd2fb7ce7e 81
benson516 5:7ccd2fb7ce7e 82 // Filter
benson516 5:7ccd2fb7ce7e 83 curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur);
benson516 5:7ccd2fb7ce7e 84
benson516 5:7ccd2fb7ce7e 85 // PID
benson516 5:7ccd2fb7ce7e 86 pid.Compute_noWindUP(curRef, curFeedBack);
benson516 5:7ccd2fb7ce7e 87
benson516 6:bae35ca64f10 88 // Output saturation and unit changing
benson516 5:7ccd2fb7ce7e 89 MotorPlus = 0.5 + saturation((-pid.output + func_back_emf(angularSpeed) )*voltage2Duty, delta_output, 0.5, -0.5) ;
benson516 5:7ccd2fb7ce7e 90
benson516 5:7ccd2fb7ce7e 91 // Anti-windup
benson516 5:7ccd2fb7ce7e 92 pid.Anti_windup(delta_output);
benson516 5:7ccd2fb7ce7e 93
benson516 5:7ccd2fb7ce7e 94 // Setting up complementary PWM
benson516 5:7ccd2fb7ce7e 95 if(pwmIndex_ == PWM1){
benson516 5:7ccd2fb7ce7e 96 TIM1->CCER |= 4;
benson516 5:7ccd2fb7ce7e 97 }else if(pwmIndex_ == PWM2){
benson516 5:7ccd2fb7ce7e 98 TIM1->CCER |= 64;
benson516 5:7ccd2fb7ce7e 99 }
adam_z 1:c5973a56d474 100 }
benson516 5:7ccd2fb7ce7e 101 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 102 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 103 return (Ke*W_in);
adam_z 1:c5973a56d474 104 }
adam_z 3:c787d1c5ad6a 105
adam_z 2:562bd14dfd3a 106 //****************for test************************//
adam_z 1:c5973a56d474 107 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 108 {
adam_z 1:c5973a56d474 109 MotorPlus = ratio;
adam_z 1:c5973a56d474 110 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 111 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
adam_z 2:562bd14dfd3a 112 }
adam_z 2:562bd14dfd3a 113
adam_z 2:562bd14dfd3a 114 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 115 {
adam_z 2:562bd14dfd3a 116 return analogInValue = currentAnalogIn.read();
adam_z 2:562bd14dfd3a 117 }
adam_z 2:562bd14dfd3a 118
adam_z 2:562bd14dfd3a 119 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 120 {
adam_z 3:c787d1c5ad6a 121 return curFeedBack;
adam_z 3:c787d1c5ad6a 122 }
adam_z 3:c787d1c5ad6a 123
adam_z 3:c787d1c5ad6a 124