moving files

src/main.cpp

@@ -6,8 +6,8 @@
 
 #include "config.h"
 
-#include "hardware/motorControl.h"
-#include "hardware/speedometer.h"
+#include "motorControl.h"
+#include "speedometer.h"
 #include "driveModi/autopilot.h"
 #include "driveModi/captureRoute.h"
 #include "driveModi/manualControl.h"
@@ -82,7 +82,6 @@ void setup() {
     configTime(3600, 3600, "2.de.pool.ntp.org");
 
     DebugMqtt::init(&mqtt_client, Loglevel::debug);
-    DebugMqtt::initRealMillis();
 
     Ps3.attach(callbackControllerAction);
     Ps3.attachOnConnect(callbackControllerConnect);
@@ -91,11 +90,11 @@ void setup() {
     Ps3.begin();
 
     // Init hardware
-    left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left");
-    right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right");
+    left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
+    right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
 
-    speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, "left");
-    speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right");
+    speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
+    speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);
 
     moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
 
@@ -104,11 +103,6 @@ void setup() {
     captureRoute.init(&route, &moveController, &left_motor, &right_motor);
     autopilot.init(&route, &moveController);
     consolControl.init(&moveController);
-
-    //Test checkTimes function
-    DebugTimes testDebugTimes;
-    delay(2);
-    testDebugTimes.stop("Test checkTimes");
 }
 
 void loop() {
@@ -125,18 +119,11 @@ void loop() {
     }
   } else {
     // Client connected
-    DebugTimes mqttClientLoop;
     mqtt_client.loop();
-    mqttClientLoop.stop("mqttloop", 100);
   }   
-
-    DebugTimes runMotorLeft;
+  
     left_motor.runMotorControl();
-    runMotorLeft.stop("left_motor", 100);
-
-    DebugTimes runMotorRight;
     right_motor.runMotorControl();
-    runMotorRight.stop("right_motor", 100);
     
     speedometer_left.runSpeedometer();
     speedometer_right.runSpeedometer();
@@ -144,11 +131,8 @@ void loop() {
     // route.runRoute();
 
     switch (driveMode) {
-        case manualControl_e: {
-            DebugTimes manControlDebug;
+        case manualControl_e:
             manualControl.runManualControl();
-            manControlDebug.stop("manualControl run", 100);
-            }
             break;
 
         case captureRoute_e: