ntrip, testMode, zed9 gnss modul

2022-09-13 20:31:52 +02:00
parent c7cb0d3895
commit af6cb98898
43 changed files with 907 additions and 262 deletions
@@ -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;
+}
+