Diff: CURRENT_CONTROL.cpp

Revision:

5:7ccd2fb7ce7e

Parent:

4:1a6ba05e7736

Child:

6:bae35ca64f10

diff -r 1a6ba05e7736 -r 7ccd2fb7ce7e CURRENT_CONTROL.cpp
--- a/CURRENT_CONTROL.cpp	Thu Apr 28 09:07:32 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Thu Dec 15 22:54:54 2016 +0000
@@ -1,59 +1,99 @@
 #include "mbed.h"
 #include "CURRENT_CONTROL.h"
 
+//=====================LPF ====================//
+LPF::LPF(float samplingTime, float cutOff_freq_Hz_in)
+{
+    Ts = samplingTime;
+    cutOff_freq_Hz = cutOff_freq_Hz_in;
+    alpha_Ts = (2*3.1415926)*cutOff_freq_Hz*Ts;
+}
 
+float LPF::filter(float input)
+{
+    // output = (1.0 - alpha_Ts)*output + alpha_Ts*input;
+    output += alpha_Ts*(input - output);
+    return output;
+}
+
+
+//================== CURRENT_CONTROL =================//
 CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
                                  PinName PwmChannel1,
                                  PinName PwmChannel2,
                                  PWMIndex pwmIndex,
                                  float Kp, float Ki, float Kd,
-                                 float samplingTime) : currentAnalogIn(curChannel),
+                                 float samplingTime) : 
+    currentAnalogIn(curChannel),
     MotorPlus(PwmChannel1),
     MotorMinus(PwmChannel2),
     pid(Kp,Ki,Kd,samplingTime),
-    lpFilter(samplingTime)
+    lpFilter(samplingTime, 1.5915) // 10 rad/s
     
 {
     pwmIndex_ = pwmIndex;
 
     Ts = samplingTime;
+    
     //setup motor PWM parameters
     MotorPlus.period_us(15);//default period equals to 25 microseconds
     MotorPlus.write(0.5);   //duty ratio = 0.5 in complementary output mode -> static
     if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
     else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output
     
-    
+    //
+    currentOffset = 0.0;
+    delta_output = 0.0;
+}
+//
+void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
+{
+    analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
+    Ke = angSpeed2BackEmf;
+    voltage2Duty = voltage2DutyRatio;
     
 }
-
-float CURRENT_CONTROL::saturation(float input, float limit_H, float limit_L)
-{
-    float output;
-    if(input < limit_H && input > limit_L)output = input;
-    else output = input >=limit_H?limit_H:limit_L;
-    return output;
+//
+float CURRENT_CONTROL::saturation(float input_value, float &delta, const float &limit_H, const float &limit_L){
+    if( input_value > limit_H ){
+        delta = limit_H - input_value;
+        input_value = limit_H;
+    }else if( input_value < limit_L ){  
+        delta = limit_L - input_value;
+        input_value = limit_L; 
+    }else{
+        delta = 0.0;
+    }
+    return input_value;
 }
-
+//
 void CURRENT_CONTROL::Control(float curRef, float angularSpeed)
 {
     analogInValue = currentAnalogIn.read();
-    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur, 10);
-    pid.Compute(curRef, curFeedBack);
-    MotorPlus = 0.5 + saturation((-pid.output + Kw*angularSpeed)*voltage2Duty, 0.5, -0.5) ;
-    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
-    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
+    
+    // Filter
+    curFeedBack = lpFilter.filter((analogInValue - currentOffset)*analog2Cur);
+    
+    // PID
+    pid.Compute_noWindUP(curRef, curFeedBack);
+    
+    MotorPlus = 0.5 + saturation((-pid.output + func_back_emf(angularSpeed) )*voltage2Duty, delta_output, 0.5, -0.5) ;
+    
+    // Anti-windup
+    pid.Anti_windup(delta_output);
+    
+    // Setting up complementary PWM
+    if(pwmIndex_ == PWM1){
+        TIM1->CCER |= 4;
+    }else if(pwmIndex_ == PWM2){
+        TIM1->CCER |= 64;
+    }
 }
-
-void CURRENT_CONTROL::SetParams(float Analog2Cur, float angSpeed2BackEmf, float voltage2DutyRatio)
-{
-    analog2Cur = Analog2Cur;//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
-    Kw = angSpeed2BackEmf;
-    voltage2Duty = voltage2DutyRatio;
-    
+// Back emf as the function of rotational speed
+float CURRENT_CONTROL::func_back_emf(const float &W_in){
+    return (Ke*W_in);
 }
 
-
 //****************for test************************//
 void CURRENT_CONTROL::SetPWMDuty(float ratio)
 {
@@ -72,15 +112,4 @@
     return  curFeedBack;   
 }
 
-//=====================LPF ====================//
-LPF::LPF(float samplingTime)
-{
-    Ts = samplingTime;
-}
 
-float LPF::filter(float input, float cutOff)
-{
-    output = (1.0 - cutOff*Ts)*outputLast + cutOff*Ts*input;
-    outputLast = output;
-    return output;
-}
\ No newline at end of file