Chun Feng Huang / CURRENT_CONTROL

Motor current controller

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp@5:7ccd2fb7ce7e, 2016-12-15 (annotated)

Committer:
benson516
Date:
Thu Dec 15 22:54:54 2016 +0000
Revision:
5:7ccd2fb7ce7e
Parent:
4:1a6ba05e7736
Child:
6:bae35ca64f10
First 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,
adam_z 1:c5973a56d474 25 float Kp, float Ki, float Kd,
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 5:7ccd2fb7ce7e 44 //
benson516 5:7ccd2fb7ce7e 45 currentOffset = 0.0;
benson516 5:7ccd2fb7ce7e 46 delta_output = 0.0;
benson516 5:7ccd2fb7ce7e 47 }
benson516 5:7ccd2fb7ce7e 48 //
benson516 5:7ccd2fb7ce7e 49 void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
benson516 5:7ccd2fb7ce7e 50 {
benson516 5:7ccd2fb7ce7e 51 analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
benson516 5:7ccd2fb7ce7e 52 Ke = angSpeed2BackEmf;
benson516 5:7ccd2fb7ce7e 53 voltage2Duty = voltage2DutyRatio;
adam_z 3:c787d1c5ad6a 54
adam_z 0:955aa05c968a 55 }
benson516 5:7ccd2fb7ce7e 56 //
benson516 5:7ccd2fb7ce7e 57 float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
benson516 5:7ccd2fb7ce7e 58 if( input_value > limit_H ){
benson516 5:7ccd2fb7ce7e 59 delta = limit_H - input_value;
benson516 5:7ccd2fb7ce7e 60 input_value = limit_H;
benson516 5:7ccd2fb7ce7e 61 }else if( input_value < limit_L ){
benson516 5:7ccd2fb7ce7e 62 delta = limit_L - input_value;
benson516 5:7ccd2fb7ce7e 63 input_value = limit_L;
benson516 5:7ccd2fb7ce7e 64 }else{
benson516 5:7ccd2fb7ce7e 65 delta = 0.0;
benson516 5:7ccd2fb7ce7e 66 }
benson516 5:7ccd2fb7ce7e 67 return input_value;
adam_z 4:1a6ba05e7736 68 }
benson516 5:7ccd2fb7ce7e 69 //
adam_z 4:1a6ba05e7736 70 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
adam_z 0:955aa05c968a 71 {
adam_z 2:562bd14dfd3a 72 analogInValue = currentAnalogIn.read();
benson516 5:7ccd2fb7ce7e 73
benson516 5:7ccd2fb7ce7e 74 // Filter
benson516 5:7ccd2fb7ce7e 75 curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur);
benson516 5:7ccd2fb7ce7e 76
benson516 5:7ccd2fb7ce7e 77 // PID
benson516 5:7ccd2fb7ce7e 78 pid.Compute_noWindUP(curRef, curFeedBack);
benson516 5:7ccd2fb7ce7e 79
benson516 5:7ccd2fb7ce7e 80 MotorPlus = 0.5 + saturation((-pid.output + func_back_emf(angularSpeed) )*voltage2Duty, delta_output, 0.5, -0.5) ;
benson516 5:7ccd2fb7ce7e 81
benson516 5:7ccd2fb7ce7e 82 // Anti-windup
benson516 5:7ccd2fb7ce7e 83 pid.Anti_windup(delta_output);
benson516 5:7ccd2fb7ce7e 84
benson516 5:7ccd2fb7ce7e 85 // Setting up complementary PWM
benson516 5:7ccd2fb7ce7e 86 if(pwmIndex_ == PWM1){
benson516 5:7ccd2fb7ce7e 87 TIM1->CCER |= 4;
benson516 5:7ccd2fb7ce7e 88 }else if(pwmIndex_ == PWM2){
benson516 5:7ccd2fb7ce7e 89 TIM1->CCER |= 64;
benson516 5:7ccd2fb7ce7e 90 }
adam_z 1:c5973a56d474 91 }
benson516 5:7ccd2fb7ce7e 92 // Back emf as the function of rotational speed
benson516 5:7ccd2fb7ce7e 93 float CURRENT_CONTROL::func_back_emf(const float &W_in){
benson516 5:7ccd2fb7ce7e 94 return (Ke*W_in);
adam_z 1:c5973a56d474 95 }
adam_z 3:c787d1c5ad6a 96
adam_z 2:562bd14dfd3a 97 //****************for test************************//
adam_z 1:c5973a56d474 98 void CURRENT_CONTROL::SetPWMDuty(float ratio)
adam_z 1:c5973a56d474 99 {
adam_z 1:c5973a56d474 100 MotorPlus = ratio;
adam_z 1:c5973a56d474 101 if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
adam_z 1:c5973a56d474 102 else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
adam_z 2:562bd14dfd3a 103 }
adam_z 2:562bd14dfd3a 104
adam_z 2:562bd14dfd3a 105 float CURRENT_CONTROL::GetAnalogIn(void)
adam_z 2:562bd14dfd3a 106 {
adam_z 2:562bd14dfd3a 107 return analogInValue = currentAnalogIn.read();
adam_z 2:562bd14dfd3a 108 }
adam_z 2:562bd14dfd3a 109
adam_z 2:562bd14dfd3a 110 float CURRENT_CONTROL::GetCurrent(void)
adam_z 2:562bd14dfd3a 111 {
adam_z 3:c787d1c5ad6a 112 return curFeedBack;
adam_z 3:c787d1c5ad6a 113 }
adam_z 3:c787d1c5ad6a 114
adam_z 3:c787d1c5ad6a 115