added digital control to manualControl

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -26,14 +26,30 @@ void ManualControl::loop() {
     if (this->caliCompass)
         this->caliCompass->loop();
 
-    if (millis() - this->lastMillis < delay) {
+    if (millis() - this->lastMillis < delay)
         return;
+    
+    switch (this->inputMode) {
+        case InputMode::Analog :
+            this->analogControl();
+            break;
+
+        case InputMode::Digital :
+            this->digitalControl();
+            break;
+        
+        default:
+            break;
     }
-    this->runManualControl();
+
     this->lastMillis = millis();
 }
 
-void ManualControl::runManualControl() {
+void ManualControl::switchInputMode() {
+    this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
+}
+
+void ManualControl::analogControl() {
     // Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
     int16_t y = this->input->x - 127;
     int16_t x = this->input->y - 127;
@@ -49,3 +65,25 @@ void ManualControl::runManualControl() {
     value_per_step = this->max_rotation * 2 / 256;
     this->moveControl->setRotationSpeed(x * value_per_step);
 }
+
+void ManualControl::digitalControl() {
+    int16_t y = this->input->x - 127;
+    int16_t x = this->input->y - 127;
+
+    if (y > 120)
+        this->moveControl->setSpeed(-this->max_speed);
+    else if (y < -120)
+        this->moveControl->setSpeed(this->max_speed);
+    else
+        this->moveControl->setSpeed(0);
+
+    if (x > 120)
+        this->moveControl->setRotationSpeed(this->max_rotation);
+    else if (x < -120)
+        this->moveControl->setRotationSpeed(-this->max_rotation);
+    else
+        this->moveControl->setRotationSpeed(0);
+
+    if (this->directionChangeWrapper && (x > 120 || x < -120))
+        this->directionChangeWrapper->action();
+}