Diff: main.cpp

Revision:

23:e9e2cd9c1fd1

Parent:

22:9f7dae024a81

Child:

24:214f2d426484

--- a/main.cpp	Fri Aug 15 10:26:49 2014 +0000
+++ b/main.cpp	Fri Aug 15 13:29:14 2014 +0000
@@ -27,6 +27,15 @@
 double SpeedInterval = 0.0;
 int LastPosition = 0;
 
+double vMax = 300;
+double vStep = 50;
+double pStep = 0.5;
+double G1 = vStep / pStep;
+double G2 = (vMax - vStep)/(1.0-pStep);
+double Glin = vMax;
+double pLinStep = pStep * G1 / Glin;
+double TargetPwm;
+
 char counter = 0;
 
 volatile bool SinHigh = false;
@@ -110,15 +119,37 @@
         return;
     }
     
+    // Linear clamping
     
-    Linear clamping
+    // Compute the corrected pwm value to linearise the velocity profile
+    double correctedPwm;
     
-    
+    if (fabs(power) < pLinStep)
+    {
+        correctedPwm = (fabs(power) * Glin) / G1;  
+    }
+    else
+    {
+        correctedPwm = pStep + (fabs(power) - pLinStep) * Glin / G2;   
+    }
     
-    currentPower = power;
+    // Make sure our corrected value has the correct sign.
+    if(power < 0) correctedPwm *= -1.0;
+    
+     if (correctedPwm > 1.0) 
+        {
+            correctedPwm = 1.0;
+        }
     
-    PhaseA = (power + 1.0) / 2;
-    PhaseB = 1.0 - ((power + 1.0) / 2);
+        else if (correctedPwm < -1.0)
+        {
+            correctedPwm = -1.0;
+        }
+    
+    currentPower = correctedPwm;
+    
+    PhaseA = (correctedPwm + 1.0) / 2;
+    PhaseB = 1.0 - ((correctedPwm + 1.0) / 2);
 }
 
 void Enable()
@@ -271,6 +302,13 @@
     pc.putc(13);
 }
 
+void SerialNewFile()
+{
+    pc.putc(28);
+    pc.putc(10);
+    pc.putc(13);
+}
+
 void Home()
 {
     if (!Enabled)
@@ -351,7 +389,8 @@
 double PwmChange=0;
 
 double pwm;
-double TargetPwm;
+
+
 double TargetVelocity;
 
 double PosiState;
@@ -359,12 +398,7 @@
 
 int PreviousTime = 0;
 
-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 ()
 {
@@ -413,28 +447,7 @@
         {
             TargetPwm = -1.0;
         }
-
-
-        // Compute the corrected pwm value to linearise the velocity profile
-        double correctedPwm;
         
-        if (fabs(TargetPwm) < pStep)
-        {
-            correctedPwm = (fabs(TargetPwm) * Glin) / G1;  
-        }
-        
-        if (fabs(TargetPwm) > pStep)
-        {
-            correctedPwm = pStep + (fabs(TargetPwm) - pStep) * Glin / G2;   
-        }
-        
-        // Make sure our corrected value has the correct sign.
-        if(TargetPwm < 0) correctedPwm *= -1.0;
-
-        SetPower(correctedPwm);
-        
-        
-
         errorcounter ++;
 
         if (errorcounter > 9)
@@ -443,7 +456,6 @@
         }
 
         PosPreviousError[errorcounter] = PosError;
- //       VelPreviousError[errorcounter] = VelError;
    // }
         
 }
@@ -474,7 +486,7 @@
 
     while(Enabled)
     {
-        double pow = 0.4;
+        double pow = 0.0;
         while(pow < 1.0)
        // PosKpGain = 10.0;
        // while(PosKpGain < 50.0)
@@ -489,13 +501,25 @@
                                 
                 float iterationStart = RunningTime.read();
                 
-                while(RunningTime.read()-iterationStart < 10.0)
+                    
+                SetPower(pow);
+                
+                while(position < 21000 && RunningTime.read()-iterationStart < 30.0)
                 {
                     SerialTransmit();
-                    
-                    Controller();
                 }
                 
+                iterationStart = RunningTime.read();
+                SerialNewFile();
+                
+                SetPower(-pow);
+                
+                while(position > 1000 && RunningTime.read()-iterationStart < 30.0)
+                {
+                    SerialTransmit();
+                }
+                
+                SerialNewFile();
                 Home();
           //  }
         }