Diff: main.cpp

Revision:

46:9f66021615f9

Parent:

45:e5b175383624

Child:

47:c04b0d51e1e3

--- a/main.cpp	Sat Oct 08 22:52:50 2016 +0000
+++ b/main.cpp	Sun Oct 09 00:58:52 2016 +0200
@@ -1,7 +1,9 @@
 #include "mbed.h"
 #include "Screens.hpp"
 #include "ResistiveTouchController.hpp"
+#include "CYS8218Controller.hpp"
 #include "Map.hpp"
+#include "MedianFilter.hpp"
 
 //--- Function Prototypes
 void GUIThread();
@@ -25,12 +27,14 @@
 float alpha = 0;
 float xSP = -20;
 float x = 0;
+MedianFilter xFilter(7);
 
 //-- Y Axis Variables
 float zeroBeta = 0;
 float beta = 0;
 float ySP = 50;
 float y = 0;
+MedianFilter yFilter(7);
 
 
 //-- Controller Variables
@@ -44,6 +48,9 @@
 //-- Sensors and Actuator objects
 ResistiveTouchController plate(A0,A1,A3,A4,A5);
 Map plateMapper(0,1024, -170,170);
+CYS8218Controller xMotor(D9);
+CYS8218Controller yMotor(D8);
+
 
 float tCal[3][2];
 int calPoint = 0;
@@ -156,6 +163,8 @@
 {
   ImageFromSD splash("/sd/Splash.txt");
   pc.baud(57600);
+  xMotor.SetZero(28.0);
+  yMotor.SetZero(25.0);
   nextScreen = Screen::MAIN_MENU_SCREEN;
 
   Thread::wait(5000);
@@ -166,12 +175,16 @@
 void ZeroPlate()
 {
 
-  pc.printf("Zero a : %f | Zero b : %f\n\r", zeroAlpha, zeroBeta);
-
+  xMotor.Set(zeroAlpha);
+  yMotor.Set(zeroBeta);
 
   if( saved)
   {
     // TODO Save zero position on servos
+    xMotor.SetZero(zeroAlpha);
+    yMotor.SetZero(zeroBeta);
+    zeroAlpha = 0;
+    zeroBeta = 0;
     nextScreen = Screen::MAIN_SETTINGS_SCREEN;
     saved = false;
   }