kleiax/Bachelorarbeit-Rover
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases 1 Wiki Activity

bugFix testModeMenu implement duration print

Browse Source
This commit is contained in:
kleiax
2023-01-18 10:32:49 +01:00
parent fef40ab4b4
commit 801a474b49
6 changed files with 109 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/driveModi/Modi/TestMode/testMode.cpp
+31 -16
View File
@@ -10,8 +10,9 @@
*/
#include "testMode.h"
TestMode::TestMode(MoveControl *moveControl) {
TestMode::TestMode(MoveControl *moveControl, Navigation* navigation) {
this->moveControl = moveControl;
this->navigation = navigation;
}
TestMode::~TestMode() {
@@ -22,11 +23,20 @@ void TestMode::loop() {
if (millis() - this->lastMillis < this->delay)
return;
if (this->maneuver == Maneuver::Turn) {
uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
if (delta > this->degree)
this->abort = true;
}
if (millis() - this->actionStart > this->maneuverTime || this->abort) {
this->busy = false;
this->abort = false;
this->moveControl->setDrivingStatus(DrivingStatus::stop);
this->maneuver = Maneuver::None;
}
this->lastMillis = millis();
}
void TestMode::setSpeed(int16_t speed) {
@@ -41,13 +51,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
if (this->busy)
return false;
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);
@@ -57,8 +63,10 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
else if (degree > 0)
this->moveControl->setRotationSpeed(this->rotationSpeed);
//TODO: calc this shit
this->maneuverTime = 10;
this->azimuth = this->navigation->getAzimuth();
this->degree = degree;
this->maneuverTime = 5 * 1000;
this->busy = true;
return true;
@@ -73,9 +81,20 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
this->busy = true;
this->maneuver = Maneuver::Drive;
return true;
} else {
//forward or backward and left or right
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;
this->maneuver = Maneuver::Drive;
return true;
}
this->moveControl->setDrivingStatus(DrivingStatus::stop);
@@ -83,9 +102,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
}
bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
std::cout << "TestMode::leftEngine: 1" << std::endl;
if (this->engineInit(powerPercentage, seconds)) {
this->moveControl->setRawPowerLeft(powerPercentage);
this->maneuver = Maneuver::LeftEngine;
return true;
}
return false;
@@ -94,6 +113,7 @@ bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
if (this->engineInit(powerPercentage, seconds)) {
this->moveControl->setRawPowerRight(powerPercentage);
this->maneuver = Maneuver::RightEngine;
return true;
}
return false;
@@ -102,7 +122,8 @@ bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
if (this->engineInit(powerPercentage, seconds)) {
this->moveControl->setRawPowerLeft(powerPercentage);
this->moveControl->setRawPowerRight(powerPercentage);;
this->moveControl->setRawPowerRight(powerPercentage);
this->maneuver = Maneuver::BothEngine;
return true;
}
return false;
@@ -121,17 +142,11 @@ uint8_t TestMode::getRemainingManeuverTime() {
bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
if (this->busy)
return false;
if (powerPercentage >= 100 || powerPercentage <= -100)
return false;
if (seconds < 0)
return false;
if (true) {
std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(DrivingStatus::raw);
this->maneuverTime = seconds * 1000;