Revision 6:bfe745b152fa, committed 2014-08-07

Comitter:

alpesh

Date:

Thu Aug 07 09:19:16 2014 +0000

Parent:

5:4468f8ff5ec6

Child:

7:4cd7be306626

Commit message:

ErrorOutput

Changed in this revision

--- a/main.cpp	Thu Aug 07 08:52:19 2014 +0000
+++ b/main.cpp	Thu Aug 07 09:19:16 2014 +0000
@@ -154,12 +154,15 @@
     return (direction * 0.04)/((double)interruptPeriod / 1000000.0);
 }
 
+double Error = 0;
+
 void SerialTransmit()
 {        
     double tempPos = position;
     double tempTime = RunningTime.read();
     double tempPow = currentPower;
     double tempSpeed = GetSpeed();
+    double tempError = Error;
     int outChar = 0;
     
     if (tempPos < 0.0)
@@ -367,6 +370,58 @@
         pc.putc(outChar + 48);
     }
     
+    pc.putc('\t');
+    outChar = 0;
+    
+    if (tempError < 0.0)
+    {
+        pc.putc('-');
+        tempError *= -1;
+    }
+    if (tempError >= 100.0)
+    {
+        outChar = tempError / 100;
+        pc.putc(outChar + 48);
+        tempError -= outChar * 100.0;
+    }
+    if (tempError >= 10.0)
+    {
+        outChar = tempError / 10;
+        pc.putc(outChar + 48);
+        tempError -= outChar * 10.0;
+    }
+    else if(outChar > 0)
+    {
+        pc.putc('0');
+    }
+    if (tempError >= 1.0)
+    {
+        outChar = tempError;
+        pc.putc(outChar + 48);
+        tempError -= outChar;
+    }
+    else
+    {
+        pc.putc('0');
+    }
+    if (tempError >= 0.1)
+    {
+        pc.putc('.');
+        outChar = tempError * 10;
+        pc.putc(outChar + 48);
+        tempError -= (double)outChar / 10.0;
+    }
+    else
+    {
+        pc.putc('.');
+        pc.putc('0');
+    }
+    if (tempError >= 0.01)
+    {
+        outChar = tempError * 100;
+        pc.putc(outChar + 48);
+    }
+    
     pc.putc(10);
     pc.putc(13);
 }
@@ -392,12 +447,12 @@
 }
 
 double TargetSpeed=50.0; //Millimeter per second
-double Error =0;
+
 double KpTerm;
 double ErrorInt;
 double ErrorDer;
 
-double KpGain = 0.001;
+double KpGain = 0.000009;
 double KiGain = 0.0;
 double KdGain = 0.0;
 
@@ -422,7 +477,7 @@
         
                 KpTerm = Error * KpGain;
         
-                //ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
+                ErrorDer = (Error - PreviousError[errorcounter]) / timeStep;           
         
                 ErrorInt = ErrorInt + Error * timeStep;
         
@@ -449,9 +504,9 @@
 
                 errorcounter ++;
         
-                if (counter > 9)
+                if (errorcounter > 9)
                 {
-                    counter = 0;   
+                    errorcounter = 0;   
                 }
 
                 PreviousError[errorcounter] = Error;
@@ -479,10 +534,8 @@
     
     previousTime = RunningTime.read_us();
 
-    while(1)
+    while(Enabled)
     {
-        
-        
         while(position < 200)
         {
             SerialTransmit();
@@ -490,13 +543,6 @@
             Controller();
         }
         
-        TargetSpeed = -50.0;
-        
-        while(position > 10)
-        {
-            SerialTransmit();
-            
-            Controller();
-        }
+        Disable();
     }
 }
\ No newline at end of file