Diff: main.cpp

Revision:

50:0e0cf6b350f7

Parent:

49:a907d4f447a9

Child:

54:75dc5c3b7cfc

--- a/main.cpp	Wed Oct 12 00:06:26 2016 +0000
+++ b/main.cpp	Tue Oct 18 02:11:07 2016 +0200
@@ -30,6 +30,7 @@
 float x = 0;
 float xError = 0;
 float xPrevError = 0;
+float xSummation = 0;
 MedianFilter xFilter(7);
 
 //-- Y Axis Variables
@@ -39,6 +40,7 @@
 float y = 0;
 float yError = 0;
 float yPrevError = 0;
+float ySummation = 0;
 MedianFilter yFilter(7);
 
 
@@ -114,6 +116,7 @@
   {
     if( nextScreen != currentScreen )
     {
+      started = false;
       Thread::wait(100);
 
       if( activeScreen != NULL)
@@ -221,18 +224,23 @@
   {
     cycleTimer.start();
     plate.Measure();
-    
+
     //x = xMapper.Calculate(floor(plate.X()));
     //y = yMapper.Calculate(floor(plate.Y()));
-    
+
     x = xMapper.Calculate(floor(xFilter.Process(plate.X())));
     y = yMapper.Calculate(floor(yFilter.Process(plate.Y())));
 
     xError = xSP - x;
     yError = ySP - y;
 
-    alpha = P*xError + D*(xError-xPrevError);
-    beta = P*yError + D*(yError-yPrevError);
+    xSummation += xError;
+    ySummation += yError;
+
+
+
+    alpha = P*xError + D*(xError-xPrevError) + I*xSummation;
+    beta = P*yError + D*(yError-yPrevError) + I*ySummation;
 
     xPrevError = xError;
     yPrevError = yError;