Diff: main.cpp

Revision:

47:c04b0d51e1e3

Parent:

46:9f66021615f9

Child:

49:a907d4f447a9

--- a/main.cpp	Sun Oct 09 00:58:52 2016 +0200
+++ b/main.cpp	Mon Oct 10 08:19:48 2016 +0200
@@ -13,6 +13,7 @@
 void Manual();
 void Automatic();
 void Calibrate();
+
 //-----------------------
 
 //-- GUI Thread Variables and Objects
@@ -27,6 +28,8 @@
 float alpha = 0;
 float xSP = -20;
 float x = 0;
+float xError = 0;
+float xPrevError = 0;
 MedianFilter xFilter(7);
 
 //-- Y Axis Variables
@@ -34,6 +37,8 @@
 float beta = 0;
 float ySP = 50;
 float y = 0;
+float yError = 0;
+float yPrevError = 0;
 MedianFilter yFilter(7);
 
 
@@ -41,13 +46,15 @@
 float P = 0;
 float I = 0;
 float D = 0;
-float Ts = 4;
+float Ts = 0.004;
+Timer cycleTimer;
 //-----------------------
 
 
 //-- Sensors and Actuator objects
 ResistiveTouchController plate(A0,A1,A3,A4,A5);
-Map plateMapper(0,1024, -170,170);
+Map xMapper(0,1024, -170,170);
+Map yMapper(0,1024, -136,136);
 CYS8218Controller xMotor(D9);
 CYS8218Controller yMotor(D8);
 
@@ -68,9 +75,7 @@
 bool saved = false;
 
 // TODO Delete these variables
-float t = 0;
 Serial pc(USBTX,USBRX);
-DigitalIn test(D3);
 
 int main()
 {
@@ -92,6 +97,13 @@
       case Screen::CALIBRATE_SCREEN :
         Calibrate();
         break;
+      case Screen::MANUAL_SETPOINT_SCREEN :
+        Automatic();
+        break;
+      default :
+        xMotor.Off();
+        yMotor.Off();
+        break;
     }
   }
 }
@@ -161,6 +173,7 @@
 
 void Init()
 {
+  // TODO Read values from sd card
   ImageFromSD splash("/sd/Splash.txt");
   pc.baud(57600);
   xMotor.SetZero(28.0);
@@ -176,13 +189,13 @@
 {
 
   xMotor.Set(zeroAlpha);
-  yMotor.Set(zeroBeta);
+  yMotor.Set(-zeroBeta);
 
   if( saved)
   {
-    // TODO Save zero position on servos
+    // TODO Save zero position on sd card
     xMotor.SetZero(zeroAlpha);
-    yMotor.SetZero(zeroBeta);
+    yMotor.SetZero(-zeroBeta);
     zeroAlpha = 0;
     zeroBeta = 0;
     nextScreen = Screen::MAIN_SETTINGS_SCREEN;
@@ -194,7 +207,9 @@
 
 void Manual()
 {
-  // TODO Add manual control logic
+  xMotor.set(alpha);
+  yMotor.set(beta);
+  Thread::wait(10);
 }
 
 void Automatic()
@@ -202,11 +217,36 @@
   //TODO Add automatic control mode logic
   ballOnPlate = plate.TouchDetected();
 
-  if( ballOnPlate )
+  if( ballOnPlate && started )
   {
+    cycleTimer.start();
     plate.Measure();
-    x = plateMapper.Calculate(floor(plate.X()));
-    y = plateMapper.Calculate(floor(plate.Y()));
+    x = xMapper.Calculate(floor(plate.X()));
+    y = yMapper.Calculate(floor(plate.Y()));
+
+    xError = xSP - x;
+    yError = ySP - y;
+
+    alpha = P*xError + D*(xError-xPrevError);
+    beta = P*yError + D*(yError-yPrevError);
+
+    xPrevError = xError;
+    yPrevError = yError;
+
+    alpha = ( alpha > 10 ? 10 : (alpha < -10 ? -10 : alpha));
+    beta = ( beta > 10 ? 10 : (beta < -10 ? -10 : beta) );
+
+    xMotor.set(alpha);
+    yMotor.set(beta);
+
+    cycleTimer.stop();
+    measuredTs = cycleTimer.read_ms();
+    cycleTimer.reset();
+  }
+  else
+  {
+    xMotor.Off();
+    yMotor.Off();
   }
 
 }
@@ -228,6 +268,7 @@
         {
           while( test.read() );
 
+          //TODO Test calibration function
           calPoint++;
           calTimer.stop();
           calTimer.reset();