From 2c1aed89f1ef0720adc6f7543d5f34c960364015 Mon Sep 17 00:00:00 2001
From: Alexander Klein <alex@kleiax.de>
Date: Tue, 23 May 2023 23:55:09 +0200
Subject: [PATCH] included calibrateCompass to the manualDrive menu

---
 lib/calibrateCompass/calibrateCompass.cpp     | 10 +++-
 lib/calibrateCompass/calibrateCompass.h       |  4 +-
 .../driveModi/ManualDrive/menuManualDrive.cpp | 49 +++++++++++++++++++
 .../driveModi/ManualDrive/menuManualDrive.h   |  5 +-
 .../Modi/ManualControl/manualControl.cpp      |  3 ++
 .../Modi/ManualControl/manualControl.h        |  4 ++
 6 files changed, 71 insertions(+), 4 deletions(-)

diff --git a/lib/calibrateCompass/calibrateCompass.cpp b/lib/calibrateCompass/calibrateCompass.cpp
index 1fc89e1..8895e88 100644
--- a/lib/calibrateCompass/calibrateCompass.cpp
+++ b/lib/calibrateCompass/calibrateCompass.cpp
@@ -66,6 +66,9 @@ void CalibrateCompass::loop() {
 }
 
 void CalibrateCompass::start() {
+    if (this->state != State::Ready)
+        return;
+
     this->clearData();
     this->state = State::Calibrating;
     this->compass->setCalibration(0, 0, 0, 0, 0, 0);
@@ -85,8 +88,13 @@ void CalibrateCompass::useData() {
                                     )
 }
 
+void CalibrateCompass::resetData() {
+    this->reset();
+    this->compass->setCalibration(0, 0, 0, 0, 0, 0);
+}
+
 void CalibrateCompass::reset() {
-    this->start();
+    this->clearData();
     this->state = State::Ready;
 }
 
diff --git a/lib/calibrateCompass/calibrateCompass.h b/lib/calibrateCompass/calibrateCompass.h
index 51b59f4..ad7d6f5 100644
--- a/lib/calibrateCompass/calibrateCompass.h
+++ b/lib/calibrateCompass/calibrateCompass.h
@@ -16,7 +16,6 @@
 class CalibrateCompass {
     public:
         enum State {
-            None,
             Ready,
             Calibrating,
             Finished
@@ -31,6 +30,7 @@ class CalibrateCompass {
         void loop();
         void start();
         void useData();
+        void resetData();
         void reset();
 
         State getState() const { return this->state; }
@@ -38,7 +38,7 @@ class CalibrateCompass {
 
     private:
         QMC5883LCompass* compass;
-        State state = State::None;
+        State state;
         CallibrationData data;
 
         void clearData();
diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
index 55b354d..311e684 100644
--- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
+++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.cpp
@@ -10,6 +10,15 @@
  */
 #include "menuManualDrive.h"
 
+MenuManualControl::MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {
+    this->caliCompass = new CalibrateCompass(this->driveManager->getNavigation()->getCompass());
+}
+
+MenuManualControl::~MenuManualControl() {
+    delete this->caliCompass;
+    this->manualControl->setCalibrateCompass();
+}
+
 void MenuManualControl::printPage() const {
     String lineOne = "";
     String lineTwo = "";
@@ -53,6 +62,28 @@ void MenuManualControl::printPage() const {
             lineTwo.concat(this->manualControl->getDutycycleRight());
             break;
         
+        case 7:
+            switch (this->caliCompass->getState()) {
+                case CalibrateCompass::State::Ready :
+                    lineOne = "Start compass";
+                    lineTwo = "calibration";
+                    break;
+
+                case CalibrateCompass::State::Calibrating :
+                    lineOne = "Calibrating...";
+                    lineTwo = "Move around";
+                    break;
+
+                case CalibrateCompass::State::Finished:
+                    lineOne = "Calibration";
+                    lineTwo = "finished";
+                    break;
+                
+                default:
+                    break;
+            }
+            break;
+
         default:
             this->printDefault();
         return;
@@ -87,6 +118,24 @@ void MenuManualControl::runCommand() const {
         case 4:
             this->manualControl->increaseMaxRotation();
             break;
+
+        case 7:
+            switch (this->caliCompass->getState()) {
+                case CalibrateCompass::State::Ready :
+                    this->manualControl->setCalibrateCompass(this->caliCompass);
+                    this->caliCompass->start();
+                    break;
+
+                case CalibrateCompass::State::Finished:
+                    this->manualControl->setCalibrateCompass();
+                    this->caliCompass->useData();
+                    this->caliCompass->reset();
+                    break;
+                
+                default:
+                    break;
+            }
+            break;
         
         default:
             break;
diff --git a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h
index 95e5d84..71094a7 100644
--- a/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h
+++ b/src/SpecialMenus/driveModi/ManualDrive/menuManualDrive.h
@@ -13,6 +13,7 @@
 #define MENU_MANUAL_DRIVE_H
 
 #include "SpecialMenus/driveModi/menuDriveMode.h"
+#include "calibrateCompass.h"
 
 /**
  * @brief A class to print informations about the class ManualControl
@@ -25,7 +26,8 @@ class MenuManualControl : public MenuDriveMode {
          * 
          * @param driveManager 
          */
-        MenuManualControl(DriveManager* driveManager) : MenuDriveMode(driveManager) {}
+        MenuManualControl(DriveManager* driveManager);
+        ~MenuManualControl();
 
         /**
     	 * @brief Prints the Information to display and console
@@ -43,6 +45,7 @@ class MenuManualControl : public MenuDriveMode {
 
     private:
         ManualControl* manualControl;
+        CalibrateCompass* caliCompass;
 };
 
 #endif // MENU_MANUAL_DRIVE_H
diff --git a/src/driveModi/Modi/ManualControl/manualControl.cpp b/src/driveModi/Modi/ManualControl/manualControl.cpp
index 8a34db1..5169459 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.cpp
+++ b/src/driveModi/Modi/ManualControl/manualControl.cpp
@@ -23,6 +23,9 @@ ManualControl::~ManualControl() {
 }
 
 void ManualControl::loop() {
+    if (caliCompass)
+        this->caliCompass->loop();
+
     if (millis() - this->lastMillis < delay) {
         return;
     }
diff --git a/src/driveModi/Modi/ManualControl/manualControl.h b/src/driveModi/Modi/ManualControl/manualControl.h
index d478c44..2219cb5 100644
--- a/src/driveModi/Modi/ManualControl/manualControl.h
+++ b/src/driveModi/Modi/ManualControl/manualControl.h
@@ -14,6 +14,7 @@
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
 #include "controlPadInput.h"
+#include "calibrateCompass.h"
 
 /**
  * @brief Drive the Rover with a Joystick
@@ -76,6 +77,8 @@ class ManualControl : public DriveModi {
         void decreaseMaxRotation(double increase = 0.1) { max_rotation -= increase; }
         double getMaxRotation() { return this->max_rotation; }
 
+        void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }
+
         uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
         uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
 
@@ -88,6 +91,7 @@ class ManualControl : public DriveModi {
         uint8_t delay = 10;
         double max_speed = 1;
         double max_rotation = 7;
+        CalibrateCompass* caliCompass = nullptr;
 };
 
 #endif // MANUAL_CONTROL_H