new ntrip client

src/driveModi/Modi/TestMode/testMode.cpp

@@ -12,12 +12,9 @@
 
 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);
 }
 
@@ -27,98 +24,117 @@ void TestMode::loop() {
 
     if (millis() - this->actionStart > this->maneuverTime) {
         this->busy = false;
-        this->moveControl->setSpeed(0);
-        this->moveControl->setRotationSpeed(0);
+        this->moveControl->setDrivingStatus(DrivingStatus::stop);
     }
 }
 
-void TestMode::setSpeed(double speed) {
-    this->speed = speed;
+void TestMode::setSpeed(int16_t speed) {
+    this->speed = (double) speed / 100.0;
 }
 
-void TestMode::setRotationSpeed(double speed) {
-    this->rotationSpeed = speed;
+void TestMode::setRotationSpeed(int16_t speed) {
+    this->rotationSpeed = (double) speed / 100.0;
 }
 
-bool TestMode::driveForward(uint16_t cm) {
+bool TestMode::drive(int16_t cm, int16_t degree) {
     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;
+    if (true) {
+        std::cout << "It works... cm: " << cm << " degree: " << degree << std::endl;
+        return false;
+    }
+
     this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+    if (cm == 0) {
+        //Only left or right
+        this->moveControl->setSpeed(0);
 
-    this->busy = true;
-    return true;
-}
+        if (degree < 0)
+            this->moveControl->setRotationSpeed(-this->rotationSpeed);
+        else if (degree > 0)
+            this->moveControl->setRotationSpeed(this->rotationSpeed);
+        
+        //TODO: calc this shit
+        this->maneuverTime = 10;
+        this->busy = true;
+        return true;
 
-bool TestMode::driveBackward(uint16_t cm) {
-    if (this->busy)
-        return false;
+    } else if (degree == 0) {
+        //Only forward or backward
+        this->moveControl->setRotationSpeed(0);
 
-    this->moveControl->setRotationSpeed(0);
-    this->moveControl->setSpeed(-this->speed);
+        if (cm < 0)
+            this->moveControl->setSpeed(-this->speed);
+        else if (cm > 0)
+            this->moveControl->setSpeed(this->speed);
+        
+        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+        this->busy = true;
+        return true;
+    } else {
+        //forward or backward and left or right
+    }
 
-    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;
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);    
     return false;
 }
 
-bool TestMode::driveForwardRight(uint16_t cm, uint16_t degree) {
+bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    return false;
+
+    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return false;
+
+    if (true) {
+        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
+        return false;
+    }
+
+    this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setRawPowerLeft(powerPercentage);
+    this->maneuverTime = seconds * 1000;
+    this->busy = true;
 }
 
-bool TestMode::driveBackwardLeft(uint16_t cm, uint16_t degree) {
+bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    return false;
+
+    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return false;
+
+    if (true) {
+        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
+        return false;
+    }
+
+    this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setRawPowerRight(powerPercentage);
+    this->maneuverTime = seconds * 1000;
+    this->busy = true;
 }
 
-bool TestMode::driveBackwardRight(uint16_t cm, uint16_t degree) {
+bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
     if (this->busy)
         return false;
-    return false;
-}
 
+    if (powerPercentage >= 100 || powerPercentage <= -100)
+        return false;
+
+    if (true) {
+        std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
+        return false;
+    }
+
+    this->actionStart = millis();
+    this->moveControl->setDrivingStatus(DrivingStatus::raw);
+    this->moveControl->setRawPowerLeft(powerPercentage);
+    this->moveControl->setRawPowerRight(powerPercentage);
+    this->maneuverTime = seconds * 1000;
+    this->busy = true;
+}