Revision 13:18c376e5dc9a, committed 2014-08-13

Comitter:

alpesh

Date:

Wed Aug 13 08:31:24 2014 +0000

Parent:

12:814db1249a19

Child:

14:67466da6663d

Commit message:

Implemented 2 PID (Position and Velocity). Also, clamping mechanism for Integral Term

Changed in this revision

--- a/main.cpp	Wed Aug 13 06:43:02 2014 +0000
+++ b/main.cpp	Wed Aug 13 08:31:24 2014 +0000
@@ -196,7 +196,8 @@
     //return SmoothedInterruptPeriod();
 }
 
-double Error = 0;
+double PosError = 0;    //This has been defined here as it's being used in the serial transmit function
+double VelError = 0;
 
 void SerialOut(double outputValue)
 {
@@ -262,9 +263,13 @@
     }
 }
 
-double KpGain = 0.0;
-double KiGain = 0.0;
-double KdGain = 0.0;
+double PosKpGain = 0.0;
+double PosKiGain = 0.0;
+double PosKdGain = 0.0;
+
+double VelKpGain = 0.0;
+double VelKiGain = 0.0;
+double VelKdGain = 0.0;
 
 void SerialTransmit()
 {    
@@ -284,19 +289,19 @@
     
     pc.putc('\t');
     
-    SerialOut(Error);
+    SerialOut(PosError);
     
     pc.putc('\t');
     
-    SerialOut(KpGain);
+    SerialOut(PosKpGain);
     
     pc.putc('\t');
     
-    SerialOut(KiGain);
+    SerialOut(PosKiGain);
     
     pc.putc('\t');
     
-    SerialOut(KdGain);
+    SerialOut(PosKdGain);
     
     pc.putc(10);
     pc.putc(13);
@@ -322,52 +327,116 @@
     RGHCosInterrupt.enable_irq();
 }
 
-double SetPoint = 50.0; //Millimeter per second
+
+double SetPoint = 50.0; //Target Position in Millimeter per second
 
-double KpTerm;
-double ErrorInt;
-double ErrorDer;
+double PosProError;
+double PosIntError;
+double PosDifError;
+
+double VelProError;
+double VelIntError;
+double VelDifError;
 
 int errorcounter;
-double PreviousError [10];
+double PosPreviousError [10];
+double VelPreviousError [10];
 
 double PwmChange=0;
 
 double pwm;
-double NewPwm;
+double TargetPwm;
+double TargetVelocity;
 
-int previousTime = 0;
+double PosiState;
+double VeliState;
+
+int PreviousTime = 0;
+
+double vMax = 60;
 
 void Controller ()
 {
-    if (position <= SetPoint || 1)
-    {
-        int timeStep = RunningTime.read_us() - previousTime;
-        previousTime = RunningTime.read_us();
+    
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+    //Position PID
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+    
+    //if (position <= SetPoint || 1)
+    
     
-        Error = SetPoint - position;
+    //{
+        int timeStep = RunningTime.read_us() - PreviousTime;
+        PreviousTime = RunningTime.read_us();
+        
+        double integral_velmax = vMax/VelKiGain;
+        double integral_velmin = -vMax/VelKiGain ;
+           
+        PosError = SetPoint - position;
+
+        PosProError = PosError * PosKpGain;
+
+        PosDifError = (PosError - PosPreviousError[errorcounter]) / timeStep;           
 
-        KpTerm = Error * KpGain;
-
-        ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
-
-        ErrorInt = ErrorInt + Error * timeStep;
-
-        NewPwm = (KpTerm + KiGain * ErrorInt + KdGain * ErrorDer);
-
-        if (NewPwm > 1.0) 
+        
+        PosiState += PosError;
+        
+        if (PosiState > integral_velmax)     
+        {
+            PosiState = integral_velmax;   
+        }
+        else if (PosiState < integral_velmin)   
+        {
+            PosiState = integral_velmin;
+        }
+        PosIntError = PosKiGain * PosiState;
+        
+        TargetVelocity = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
+        
+    
+    ///////////////////////////////////////////////////////////////////////////////////////////////
+    //Velocity PID
+    ///////////////////////////////////////////////////////////////////////////////////////////////     
+       
+       double integral_pwmmax = 1.0/VelKiGain;
+       double integral_pwmmin = -1.0/VelKiGain;
+       
+        VelError = TargetVelocity - GetSpeed();
+        
+        VelProError = VelError * VelKpGain;
+        
+        VelDifError = (VelError - VelPreviousError[errorcounter]) / timeStep; 
+        
+        //Calculate the integral state with appropriate limiting
+        
+        VeliState += VelError;
+        
+        if (VeliState > integral_pwmmax)
+        {
+            VeliState = integral_pwmmax;
+        }
+        else if (VeliState < integral_pwmmin)
+        {
+            VeliState = integral_pwmmin;
+        }
+    
+        VelIntError = VelKiGain * VeliState;
+        
+        TargetPwm = (VelKpGain * VelError + VelKdGain * VelDifError + VelIntError);
+    
+        if (TargetPwm > 1.0) 
         {
             pwm = 1.0;
         }
     
-        else if (NewPwm < -1.0)
+        else if (TargetPwm < -1.0)
         {
             pwm = -1.0;
         }
     
         else
         {
-            pwm = NewPwm;
+            pwm = TargetPwm;
         }
     
         SetPower(pwm);
@@ -379,8 +448,12 @@
             errorcounter = 0;   
         }
 
-        PreviousError[errorcounter] = Error;
-    }
+        PosPreviousError[errorcounter] = PosError;
+        VelPreviousError[errorcounter] = VelError;
+   // }
+        
+    
+  
 }
 
 int main()
@@ -399,23 +472,23 @@
     //Enable();
     
     errorcounter = 0;
-    PreviousError[errorcounter]=0;
+    PosPreviousError[errorcounter]=0;
     
-    previousTime = RunningTime.read_us();
+    PreviousTime = RunningTime.read_us();
 
     while(Enabled)
     {
         //double pow = 0.0;
-        while(KpGain < 1.0)
+        while(PosKpGain < 1.0)
         {
-            KpGain += 0.01;
-            while(KiGain < 1.0)
+            PosKpGain += 0.01;
+            while(PosKiGain < 1.0)
             {
-                KiGain += 0.01;
+                PosKiGain += 0.01;
                 
-                while(KdGain < 1.0)
+                while(PosKdGain < 1.0)
                 {
-                    KdGain += 0.01;
+                    PosKdGain += 0.01;
                     
                     float iterationStart = RunningTime.read();
                     
@@ -436,7 +509,7 @@
 }
 
 /* Change this throttle value range to 1.0 and 0.0 for car speed
-m_throttlechange = (m_kpGain * m_error + m_kdGain * m_ErrorDer + m_ErrorInt);
+m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
 
             double throttle = m_throttle + m_throttlechange;