Revision 4:e3c9cb64be44, committed 2016-10-26

Comitter:

benson516

Date:

Wed Oct 26 17:56:01 2016 +0000

Parent:

3:d8646d8c994f

Child:

5:016c99bb877f

Child:

7:52f66036f628

Commit message:

Differential drive succeed (Ver. 1.0)

Changed in this revision

--- a/PID.cpp	Tue Oct 25 14:19:13 2016 +0000
+++ b/PID.cpp	Wed Oct 26 17:56:01 2016 +0000
@@ -16,7 +16,8 @@
     Kp = Kp_in;
     Ki = Ki_in;
     Kd = Kd_in;
-    Ka = 0.1;
+    // Ka = 0.1; // Volt.-sec./deg.
+    Ka = 0.0017;// Volt.-sec./rad
     
     // Sampling time
     Ts = Sampletime_in; 
@@ -95,4 +96,64 @@
     }
             
 }
+void PID::Compute_noWindUP(float reference_in, float feedbackvalue_in){
 
+    if(Inputlimit_bool == true){
+        if( reference_in > inputLimits_H){
+            reference = inputLimits_H;
+        }else if( reference_in < inputLimits_L){
+            reference = inputLimits_L;
+        }else{
+            reference = reference_in;
+        }
+        
+    }else{
+        reference = reference_in;
+    }
+    
+    // bypass
+    feedbackvalue = feedbackvalue_in;
+    
+    error[0] = reference - feedbackvalue;
+    // output = output + ( Kp + Ki + Kd )*error[0] + ( -Kp - 2.0*Kd )*error[1] + Kd*error[2];
+    output = Kp*error[0] + Ki*error_I;
+    
+    // Delay 1
+    error[1] = error[0];  
+    
+    // Integration
+    error_I += Ts*error[0];
+    
+    // Output satuation
+    /*
+    if(Outputlimit_bool && AntiWindUp_bool){
+        if( output >= outputLimits_H){
+            // output = output - (output - outputLimits_H)*Ka;
+            error_I -= Ka*(output - outputLimits_H); // Anti-windup
+            output = outputLimits_H;
+        }else if( output <= outputLimits_L){
+            // output = output - (output - outputLimits_L)*Ka;
+            error_I -= Ka*(output - outputLimits_L); // Anti-windup
+            output = outputLimits_L;
+        }
+    }else{
+        // output = output;
+    }
+    */
+            
+}
+void PID::Saturation_output(){
+        if( output >= outputLimits_H){
+            delta_output = outputLimits_H - output;
+            output = outputLimits_H;
+        }else if( output <= outputLimits_L){
+            delta_output = outputLimits_L - output;
+            output = outputLimits_L;
+        }else{
+            delta_output = 0.0;
+        }
+}
+void PID::Anti_windup(float delta){ // delta_V = Vs - V
+    // Anti-windup compensation
+    error_I += Ka*delta; // Anti-windup
+}
\ No newline at end of file

--- a/PID.h	Tue Oct 25 14:19:13 2016 +0000
+++ b/PID.h	Wed Oct 26 17:56:01 2016 +0000
@@ -8,6 +8,12 @@
 
         PID(float Kp_in, float Ki_in, float Kd_in,  float Sampletime_in);   
         void Compute(float reference_in, float feedbackvalue_in);
+        
+        // 
+        void Compute_noWindUP(float reference_in, float feedbackvalue_in);
+        void Saturation_output();
+        void Anti_windup(float delta); // delta_V = Vs - V
+        //
            
         void SetOutputLimits(float setoutputLimits_H, float setoutputLimits_L);
         void SetInputLimits(float setinputLimits_H, float setinputLimits_L); 
@@ -23,7 +29,8 @@
         
         float output;
         float reference;
-
+        float delta_output; // Error by saturating
+        
         float Ts;
 
     private: