Implement speedometer and encoder

src/main.cpp

@@ -2,6 +2,8 @@
 
 #include "config.h"
 #include "motorControl.h"
+#include "moveControl.h"
+#include "speedometer.h"
 
 #define CUSTOM_SETTINGS
 #define INCLUDE_GAMEPAD_MODULE
@@ -12,6 +14,9 @@ int8_t modSpeed(int8_t dir, int8_t speed);
 
 MotorControl left_motor;
 MotorControl right_motor;
+Speedometer speedometer_left;
+Speedometer speedometer_right;
+MoveControl moveController;
 
 uint64_t last_millis = 0;
 
@@ -22,20 +27,27 @@ void setup() {
   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);
+  speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B);
+
+  moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
 }
 
 void loop() {
-  if (millis() - last_millis > 300) {
+  if (millis() - last_millis > 700) {
     gamepadInput();
-    Serial.print("Motor Links: ");
-    left_motor.toString();
-    Serial.print("Motor Rechts: ");
-    right_motor.toString();
+    // left_motor.toString();
+    // right_motor.toString();
+    // Serial.printf("Speed L: %f, Speed R: %f \n", speedometer_left.getSpeed(), speedometer_right.getSpeed());
 
     last_millis = millis();
   }
+
   left_motor.runMotorControl();
   right_motor.runMotorControl();
+  speedometer_left.runSpeedometer();
+  speedometer_right.runSpeedometer();
+  moveController.runMoveControl();
 }
 
 void gamepadInput() {
@@ -62,6 +74,10 @@ void gamepadInput() {
 
   left_motor.setTargetSpeed(speed_left);
   right_motor.setTargetSpeed(speed_right);
+
+  // int a = GamePad.getAngle();
+  // int b = GamePad.getRadius();
+  // Serial.printf("Winkel: %d, Radius: %d \n", a, b);
 }
 
 int8_t modSpeed(int8_t dir, int8_t speed) {