Revision 22:9f7dae024a81, committed 2014-08-15

Comitter:

pyrostew

Date:

Fri Aug 15 10:26:49 2014 +0000

Parent:

20:04432d03b46c

Child:

23:e9e2cd9c1fd1

Commit message:

Compute the correct pwm value to linearise the velocity profile

Changed in this revision

--- a/main.cpp	Fri Aug 15 09:11:50 2014 +0000
+++ b/main.cpp	Fri Aug 15 10:26:49 2014 +0000
@@ -100,7 +100,6 @@
 
 void SetPower(double power)
 {
-    currentPower = power;
     if(!Enabled)
     {
         return;
@@ -111,6 +110,13 @@
         return;
     }
     
+    
+    Linear clamping
+    
+    
+    
+    currentPower = power;
+    
     PhaseA = (power + 1.0) / 2;
     PhaseB = 1.0 - ((power + 1.0) / 2);
 }
@@ -346,7 +352,6 @@
 
 double pwm;
 double TargetPwm;
-double newTargetPwm;
 double TargetVelocity;
 
 double PosiState;
@@ -357,6 +362,9 @@
 double vMax = 300;
 double vStep = 50;
 double pStep = 0.5;
+double G1 = vStep / pStep;
+double G2 = (vMax - vStep)/(1.0-pStep);
+double Glin = vMax;
 
 void Controller ()
 {
@@ -394,34 +402,38 @@
         PosIntError = PosKiGain * PosiState;
         
         TargetPwm = (PosKpGain * PosError + PosKdGain * PosDifError + PosIntError);
-        
-        if (TargetPwm < pStep)
-        
-            {
-                newTargetPwm = (TargetPwm * 300) / 100;  
-            }
+
         
-        if (TargetPwm > pStep)
-            {
-                newTargetPwm = pStep + (TargetPwm - pStep) * 100 / 500;   
-            }
-        
-        if (newTargetPwm > 1.0) 
+        if (TargetPwm > 1.0) 
         {
-            pwm = 1.0;
+            TargetPwm = 1.0;
         }
     
-        else if (newTargetPwm < -1.0)
+        else if (TargetPwm < -1.0)
         {
-            pwm = -1.0;
+            TargetPwm = -1.0;
+        }
+
+
+        // Compute the corrected pwm value to linearise the velocity profile
+        double correctedPwm;
+        
+        if (fabs(TargetPwm) < pStep)
+        {
+            correctedPwm = (fabs(TargetPwm) * Glin) / G1;  
         }
-    
-        else
+        
+        if (fabs(TargetPwm) > pStep)
         {
-            pwm = newTargetPwm;
+            correctedPwm = pStep + (fabs(TargetPwm) - pStep) * Glin / G2;   
         }
-    
-        SetPower(pwm);
+        
+        // Make sure our corrected value has the correct sign.
+        if(TargetPwm < 0) correctedPwm *= -1.0;
+
+        SetPower(correctedPwm);
+        
+        
 
         errorcounter ++;
 
@@ -456,6 +468,60 @@
     Home();
     //Enable();
     
+ errorcounter = 0;
+    PosPreviousError[errorcounter]=0;
+    PreviousTime = RunningTime.read_us();
+
+    while(Enabled)
+    {
+        double pow = 0.4;
+        while(pow < 1.0)
+       // PosKpGain = 10.0;
+       // while(PosKpGain < 50.0)
+        {
+            pow += 0.05;
+            //PosKpGain += 1.0;
+            
+          //  VelKpGain = 0.003;
+          //  while (VelKpGain < 0.008)
+          //  {
+          //      VelKpGain += 0.001;
+                                
+                float iterationStart = RunningTime.read();
+                
+                while(RunningTime.read()-iterationStart < 10.0)
+                {
+                    SerialTransmit();
+                    
+                    Controller();
+                }
+                
+                Home();
+          //  }
+        }
+        
+        Disable();
+    }
+}
+
+
+/* Change this throttle value range to 1.0 and 0.0 for car speed
+m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);
+
+            double throttle = m_throttle + m_throttlechange;
+
+            if (new_throttle > 1.0) {
+                m_throttle = 1.0;
+            } else if (new_throttle < 0.0) {
+                m_throttle = 0.0;
+            } else {
+                m_throttle = new_throttle;
+            }*/
+
+
+    /*
+    
+    
     errorcounter = 0;
     PosPreviousError[errorcounter]=0;
     
@@ -492,7 +558,7 @@
         Disable();
     }
 }
-
+*/
 /* Change this throttle value range to 1.0 and 0.0 for car speed
 m_throttlechange = (m_kpGain * m_error + m_kdGain * m_PosDifError + m_PosIntError);