ntrip, testMode, zed9 gnss modul

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -25,11 +25,11 @@ void Autopilot::loop() {
     if (!this->selfDriving && this->navigationStarted)
         ManualControl::loop();
 
-    if (millis() - this->last_millis < delay)
+    if (millis() - this->lastMillis < delay)
         return;
 
     this->runAutopilot();
-    this->last_millis = millis();
+    this->lastMillis = millis();
 }
 
 void Autopilot::runAutopilot() {
@@ -76,7 +76,7 @@ void Autopilot::setSelfDriving(bool val) {
 
     this->selfDriving = val;
     this->moveControl->setSpeed(0);
-    this->moveControl->setRotationspeed(0);
+    this->moveControl->setRotationSpeed(0);
 }
 
 void Autopilot::setSpeedInRelToDistance(double distance) {
@@ -97,7 +97,7 @@ void Autopilot::setRotInRelToDistance(int16_t course) {
         steps = 1;
 
     if (course == 0)
-        this->moveControl->setRotationspeed(0);
+        this->moveControl->setRotationSpeed(0);
     else
-        this->moveControl->setRotationspeed(steps);
+        this->moveControl->setRotationSpeed(steps);
 }

src/driveModi/Modi/Autopilot/autopilot.h

@@ -127,7 +127,7 @@ class Autopilot : public ManualControl {
         bool selfDrivingAvailable = false;
         bool updateDisplay = false;
 
-        uint16_t last_millis = 0;
+        uint16_t lastMillis = 0;
         uint8_t delay = 40;
 };
 

src/driveModi/Modi/Autopilot/autopilotConfig.h

@@ -47,7 +47,7 @@
 /**
  * @brief Course correction factor
  * This value is used by the setRotInRelToDistance function and is used to calculate
- * the value for setRotationspeed.
+ * the value for setRotationSpeed.
  */
 #define COURSE_CORRECTION_FACTOR 30
 

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -20,11 +20,11 @@ CaptureRoute::CaptureRoute(MoveControl* moveControl, Navigation* navigation)
 void CaptureRoute::loop() {
     ManualControl::loop();
 
-    if (millis() - this->last_millis < delay) {
+    if (millis() - this->lastMillis < delay) {
         return;
     }
     this->runCaptureRoute();
-    this->last_millis = millis();
+    this->lastMillis = millis();
 }
 
 void CaptureRoute::runCaptureRoute() {

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -75,7 +75,7 @@ class CaptureRoute : public ManualControl {
         Navigation* navigation;
         RouteInfo routeInfo;
 
-        uint32_t last_millis = 0;
+        uint32_t lastMillis = 0;
         uint16_t delay = 200;
 
         bool updateDisplay = false;

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -17,16 +17,16 @@ ManualControl::ManualControl(MoveControl *moveControl) {
 
 ManualControl::~ManualControl() {
     this->moveControl->setSpeed(0);
-    this->moveControl->setRotationspeed(0);
+    this->moveControl->setRotationSpeed(0);
     this->moveControl->setDrivingStatus(DrivingStatus::stop);
 }
 
 void ManualControl::loop() {
-    if (millis() - this->last_millis < delay) {
+    if (millis() - this->lastMillis < delay) {
         return;
     }
     this->runManualControl();
-    this->last_millis = millis();
+    this->lastMillis = millis();
 }
 
 void ManualControl::runManualControl() {
@@ -37,5 +37,5 @@ void ManualControl::runManualControl() {
     this->moveControl->setSpeed((y * -1) * value_per_step);
 
     value_per_step = this->max_rotation * 2 / 256;
-    this->moveControl->setRotationspeed(-x * value_per_step);
-}
+    this->moveControl->setRotationSpeed(-x * value_per_step);
+}

src/driveModi/Modi/ManualControl/manualControl.h

@@ -22,7 +22,7 @@
  * 
  * @see MoveControl
  */
-class ManualControl : public DriveModi{
+class ManualControl : public DriveModi {
     public:
         ManualControl(MoveControl *moveControl);
 
@@ -40,7 +40,7 @@ class ManualControl : public DriveModi{
          * @see runSpeedometer()
          * @see setDelay()
          */
-        void loop();
+        void loop() override;
 
         /**
          * @brief Noramly called repeatedly by loop() to calcluate new values.
@@ -73,7 +73,7 @@ class ManualControl : public DriveModi{
         MoveControl *moveControl;
 
     private:
-        uint32_t last_millis = 0;
+        uint32_t lastMillis = 0;
         uint8_t delay = 10;
         double max_speed = 1;
         double max_rotation = 7;

src/driveModi/Modi/TestMode/testMode.cpp

@@ -0,0 +1,124 @@
+/**
+ * @file testMode.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-09-08
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#include "testMode.h"
+
+TestMode::TestMode(MoveControl *moveControl) {
+    this->moveControl = moveControl;
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+}
+
+TestMode::~TestMode() {
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationSpeed(0);
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+}
+
+void TestMode::loop() {
+    if (millis() - this->lastMillis < this->delay)
+        return;
+
+    if (millis() - this->actionStart > this->maneuverTime) {
+        this->busy = false;
+        this->moveControl->setSpeed(0);
+        this->moveControl->setRotationSpeed(0);
+    }
+}
+
+void TestMode::setSpeed(double speed) {
+    this->speed = speed;
+}
+
+void TestMode::setRotationSpeed(double speed) {
+    this->rotationSpeed = speed;
+}
+
+bool TestMode::driveForward(uint16_t cm) {
+    if (this->busy)
+        return false;
+    
+    this->moveControl->setRotationSpeed(0);
+    this->moveControl->setSpeed(this->speed);
+
+    this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+    this->actionStart = millis();
+
+    this->busy = true;
+    return true;
+}
+
+bool TestMode::driveBackward(uint16_t cm) {
+    if (this->busy)
+        return false;
+
+    this->moveControl->setRotationSpeed(0);
+    this->moveControl->setSpeed(-this->speed);
+
+    this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+    this->actionStart = millis();
+
+    this->busy = true;
+    return true;
+}
+
+bool TestMode::turnLeft(uint16_t degree) {
+    if (this->busy)
+        return false;
+
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationSpeed(this->rotationSpeed);
+
+    //TODO: calc this shit
+    this->maneuverTime = 10;
+    this->actionStart = millis();
+
+    this->busy = true;
+    return true;
+}
+
+bool TestMode::turnRight(uint16_t degree) {
+    if (this->busy)
+        return false;
+
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationSpeed(this->rotationSpeed);
+
+    //TODO: calc this shit
+    this->maneuverTime = 10;
+    this->actionStart = millis();
+
+    this->busy = true;
+    return true;
+}
+
+bool TestMode::driveForwardLeft(uint16_t cm, uint16_t degree) {
+    if (this->busy)
+        return false;
+    return false;
+}
+
+bool TestMode::driveForwardRight(uint16_t cm, uint16_t degree) {
+    if (this->busy)
+        return false;
+    return false;
+}
+
+bool TestMode::driveBackwardLeft(uint16_t cm, uint16_t degree) {
+    if (this->busy)
+        return false;
+    return false;
+}
+
+bool TestMode::driveBackwardRight(uint16_t cm, uint16_t degree) {
+    if (this->busy)
+        return false;
+    return false;
+}
+

src/driveModi/Modi/TestMode/testMode.h

@@ -0,0 +1,54 @@
+/**
+ * @file testMode.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2022-09-08
+ * 
+ * @copyright Copyright (c) 2022
+ * 
+ */
+#ifndef TEST_MODE_H 
+#define TEST_MODE_H
+
+#include "moveControl.h"
+#include "driveModi/driveModi.h"
+
+class TestMode : public DriveModi {
+    public:
+        TestMode(MoveControl *MoveControl);
+        ~TestMode();
+
+        void loop() override;
+
+        void setSpeed(double speed);
+        void setRotationSpeed(double speed);
+
+        bool driveForward(uint16_t cm);
+        bool driveBackward(uint16_t cm);
+
+        bool turnLeft(uint16_t degree);
+        bool turnRight(uint16_t degree);
+
+        bool driveForwardLeft(uint16_t cm, uint16_t degree);
+        bool driveForwardRight(uint16_t cm, uint16_t degree);
+        bool driveBackwardLeft(uint16_t cm, uint16_t degree);
+        bool driveBackwardRight(uint16_t cm, uint16_t degree);
+
+    private:
+        MoveControl *moveControl;
+
+        double speed = 0;
+        double rotationSpeed = 0;
+
+        bool busy = false;
+        
+        uint8_t delay = 10;
+
+        uint32_t maneuverTime = 0;
+        uint32_t lastMillis = 0;
+        uint32_t actionStart = 0;
+
+};
+
+#endif // TEST_MODE_H

src/driveModi/driveManager.cpp

@@ -1,7 +1,7 @@
 /**
  * @file driveManager.cpp
  * @author Alexander Klein (alex@kleiax.de)
- * @brief Implemention of the class DriveManager
+ * @brief Implementation of the class DriveManager
  * @version 0.1
  * @date 2021-12-14
  * 
@@ -15,9 +15,11 @@ Modi& operator++(Modi& m, int) {
     return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
 }
 
-DriveManager::DriveManager(MoveControl *moveControl) {
+DriveManager::DriveManager(MoveControl *moveControl, bool wifi) {
     this->moveControl = moveControl;
     this->navigation = new Navigation();
+    if (wifi)
+        this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
 }
 
 DriveManager::~DriveManager() {
@@ -42,7 +44,7 @@ void DriveManager::changeModus(Modi modus) {
     delete this->currentModusPtr;
 
     this->moveControl->setSpeed(0);
-    this->moveControl->setRotationspeed(0);
+    this->moveControl->setRotationSpeed(0);
     this->moveControl->setDrivingStatus(DrivingStatus::stop);
     
     switch (modus) {
@@ -69,8 +71,9 @@ void DriveManager::changeModus(Modi modus) {
             this->currentModusPtr = nullptr;
             break;
 
-        case Modi::TestMode:
-            this->currentModusPtr = nullptr;
+        case Modi::TestMode: {
+                this->currentModusPtr = new TestMode(this->moveControl);
+            }
             break;
         
         default:

src/driveModi/driveManager.h

@@ -22,6 +22,7 @@
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
 #include "navigation.h"
+#include "networkConfig.h"
 
 // All Drive Modi
 #include "driveModi/Modi/ManualControl/manualControl.h"
@@ -52,7 +53,7 @@ class DriveManager {
          * 
          * @param moveControl 
          */
-        DriveManager(MoveControl *moveControl);
+        DriveManager(MoveControl *moveControl, bool wifi = false);
 
         /**
          * @brief Destroy the Drive Manager object