Revision 15:d9ccd6c92a21, committed 2016-12-26

Comitter:

benson516

Date:

Mon Dec 26 08:53:09 2016 +0000

Parent:

14:67fc256efeb7

Child:

16:6e3bcd373f9d

Commit message:

Add input saturztion

Changed in this revision

--- a/CURRENT_CONTROL.cpp	Mon Dec 26 07:52:43 2016 +0000
+++ b/CURRENT_CONTROL.cpp	Mon Dec 26 08:53:09 2016 +0000
@@ -64,6 +64,9 @@
     //
     SetPWMDuty(0.5);
 
+    //Current-input limit
+    current_limit_H = 1.3;
+    current_limit_L = -1.3;
     //
     Flag_Init = false;
     Init_count = 0;
@@ -120,6 +123,11 @@
     pid.Ka = Ka; // Gain for anti-windup // Ka = 0.0017
 }
 //
+void CURRENT_CONTROL::setInputLimits(float current_limit_H_in, float current_limit_L_in){
+    current_limit_H = current_limit_H_in;
+    current_limit_L = current_limit_L_in;
+}
+//
 void CURRENT_CONTROL::OffsetInit(void){
     if (Flag_Init) return;
     //
@@ -159,7 +167,18 @@
 
 }
 
-//
+// Without delta output
+float CURRENT_CONTROL::saturation(float input_value, const float &limit_H, const float &limit_L){
+    if( input_value > limit_H ){
+        input_value = limit_H;
+    }else if( input_value < limit_L ){
+        input_value = limit_L;
+    }else{
+        // Nothing
+    }
+    return input_value;
+}
+// With delta 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;
@@ -216,7 +235,9 @@
 }
 void CURRENT_CONTROL::Control(float curRef, bool enable)
 {
-    curCommand = curRef;
+    // Input saturation
+    curCommand = saturation(curRef,current_limit_H,current_limit_L);
+
     // Init check
     OffsetInit();
     if (!Flag_Init) return;
@@ -262,7 +283,7 @@
     return (Ke*W_in);
 }
 
-//****************for test************************//
+// Elementary function (building block)
 void CURRENT_CONTROL::SetPWMDuty(float ratio)
 {
     MotorPlus = ratio;

--- a/CURRENT_CONTROL.h	Mon Dec 26 07:52:43 2016 +0000
+++ b/CURRENT_CONTROL.h	Mon Dec 26 08:53:09 2016 +0000
@@ -41,25 +41,34 @@
         PWM2
     } PWMIndex;
 
+    //Current-input limit
+    float current_limit_H; // +1.3
+    float current_limit_L; // -1.3
+
     //                      AD_pin,             PWM_p,               PWM_n,       pwmIndex  ,                   Ts
     // CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, float samplingTime);
     //                      AD_pin,             PWM_p,               PWM_n,                pwmIndex  ,       QEI_A        , QEI_B      , pulsesPerRev    , arraysize ,  Ts
     CURRENT_CONTROL(PinName curChannel, PinName PwmChannel1, PinName PwmChannel2, PWMIndex pwmIndex, PinName QEI_A, PinName QEI_B, float pulsesPerRev, int arraysize, float samplingTime);
 
-    //
+    // Setting parameters
     void SetParams(float Analog2Cur_in, float angSpeed2BackEmf, float voltage2Duty_in);
     void SetReversal(bool reverse_current_in, bool reverse_rotationalSpeed_in, bool reverse_voltage_in);
     void SetGain(float Kp, float Ki, float Kd, float Ka);
-    //
+    void setInputLimits(float current_limit_H_in, float current_limit_L_in);
+
+    // Initialization
     void OffsetInit(void);
+    // Utilities
+    float saturation(float input_value, const float &limit_H, const float &limit_L);
     float saturation(float input_value, float &delta, const float &limit_H, const float &limit_L);
+    // Control
     void TorqueControl(float TorqueRef, bool enable);
     void Control(float curRef, bool enable);
 
     // Back emf as the function of rotational speed
     float func_back_emf(const float &W_in);
 
-    //functions for test////////
+    // Elementary function (building block)
     void ChangePwmPeriod(float microSeconds);
     void SetPWMDuty(float ratio);
     void SetVoltage(float volt);