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/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
+23 -16
View File
@@ -78,6 +78,8 @@ void MenuTestMode::init() {
this->driveManager->changeModus(Modi::TestMode);
this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
testMode->setSpeed(15);
auto dummy = []() {
std::cout << "Dummy in Action" <<std::endl;
};
@@ -183,15 +185,16 @@ bool MenuTestMode::checkInput() {
}
void MenuTestMode::printManeuverTime() {
std::cout << "MenuTestMode::printManeuverTime: 1" << std::endl;
String lineTwo = "left ";
std::cout << "MenuTestMode::printManeuverTime: 2" << std::endl;
lineTwo.concat(this->testMode->getRemainingManeuverTime());
std::cout << "MenuTestMode::printManeuverTime: 3" << std::endl;
uint8_t maneuverTime = this->testMode->getRemainingManeuverTime();
if (maneuverTime) {
String lineTwo = "";
lineTwo.concat(maneuverTime);
lineTwo.concat(" secs");
std::cout << "MenuTestMode::printManeuverTime: 4" << std::endl;
this->print("Maneuver time", lineTwo);
std::cout << "MenuTestMode::printManeuverTime: 5" << std::endl;
this->print("Maneuver time:", lineTwo);
} else {
this->maneuverInAction = false;
this->printMenu();
}
}
MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) {
@@ -201,7 +204,8 @@ MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSin
}
MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) {
this->testMode = testMode;
this->menu = menu;
this->testMode = this->menu->getTestMode();
this->testModeFunctionDouble = testModeFunction;
}
@@ -209,13 +213,16 @@ void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
// This function calls a member function with a pointer
// Very helpful site:
// https://isocpp.org/wiki/faq/pointers-to-members#fnptr-vs-memfnptr-types
std::cout << "MenuTestModeWrapper::action: 0" << std::endl;
if (length == 2 && this->testModeFunctionDouble)
(this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
else if (length == 1 && this->testModeFunctionSingle)
(this->testMode->*this->testModeFunctionSingle)(values[0]);
std::cout << "MenuTestModeWrapper::action: 1" << std::endl;
bool res = false;
if (length == 2 && this->testModeFunctionDouble)
res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
else if (length == 1 && this->testModeFunctionSingle)
res = (this->testMode->*this->testModeFunctionSingle)(values[0]);
if (res) {
this->menu->printManeuverTime();
std::cout << "MenuTestModeWrapper::action: 2" << std::endl;
this->menu->maneuverStarted();
}
}
src/SpecialMenus/driveModi/TestMode/menuTestMode.h
+1
View File
@@ -34,6 +34,7 @@ class MenuTestMode : public MenuDriveMode {
void update() override;
TestMode* getTestMode() const { return this->testMode; }
void maneuverStarted() { this->maneuverInAction = true; }
private:
void init();
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;
src/driveModi/Modi/TestMode/testMode.h
+21 -2
View File
@@ -11,12 +11,24 @@
#ifndef TEST_MODE_H
#define TEST_MODE_H
#include <iostream>
#include "moveControl.h"
#include "navigation.h"
#include "driveModi/driveModi.h"
class TestMode : public DriveModi {
public:
TestMode(MoveControl *MoveControl);
enum Maneuver {
None,
LeftEngine,
RightEngine,
BothEngine,
Turn,
Drive
};
TestMode(MoveControl *moveControl, Navigation* navigation);
~TestMode();
void loop() override;
@@ -34,11 +46,16 @@ class TestMode : public DriveModi {
uint8_t getRemainingManeuverTime();
bool getBusy() const { return this->busy; }
Maneuver getManeuver() const { return this->maneuver; }
private:
bool engineInit(int16_t powerPercentage, int16_t seconds);
MoveControl *moveControl;
Navigation* navigation;
Maneuver maneuver = Maneuver::None;
int16_t maneuverValueOne = 0;
int16_t maneuverValueTwo = 0;
double speed = 0;
double rotationSpeed = 0;
@@ -47,11 +64,13 @@ class TestMode : public DriveModi {
bool abort = false;
uint8_t delay = 10;
uint16_t azimuth;
int16_t degree;
uint32_t maneuverTime = 0;
uint32_t lastMillis = 0;
uint32_t actionStart = 0;
};
#endif // TEST_MODE_H
src/driveModi/driveManager.cpp
+1 -1
View File
@@ -85,7 +85,7 @@ void DriveManager::changeModus(Modi modus) {
break;
case Modi::TestMode: {
this->currentModusPtr = new TestMode(this->moveControl);
this->currentModusPtr = new TestMode(this->moveControl, this->navigation);
}
break;
src/main.cpp
+30 -18
View File
@@ -66,6 +66,8 @@ void callbackControllerConnect(void);
void callbackControllerDisconnect(void);
void i2cScanner(void);
void makeMenu(void);
void restart(void);
void disconnectController(void);
void setup() {
@@ -139,9 +141,19 @@ void loop() {
driveManager->loop();
lcdWrapper->loop();
DebugTimes mainBatteryTime;
mainBattery->loop();
mainBatteryTime.stopConsol("MainBatteryTime", 5);
if (mainBattery->getBatteryPercent() <= 10) {
moveController.setDrivingStatus(DrivingStatus::stop);
moveController.setSpeed(0);
moveController.setRotationSpeed(0);
lcd->setCursor(0, 0);
lcd->printf("Low Battery...");
lcd->setCursor(0, 1);
lcd->print("Please turn off.");
while (1);
}
if (disconnectPs3) {
DebugTimes disconnectPS3Time;
@@ -249,22 +261,6 @@ void makeMenu() {
std::cout << "Dummy in Action" <<std::endl;
};
auto restart = [&]() {
lcdWrapper->clear();
lcdWrapper->setCursor(0, 0);
lcdWrapper->print("Rebooting ...");
ESP.restart();
};
auto disconnectController = [&]() {
disconnectPs3 = true;
lcdWrapper->clear();
lcdWrapper->setCursor(0, 0);
lcdWrapper->print("Controller is");
lcdWrapper->setCursor(0, 1);
lcdWrapper->print("disconnected.");
};
// Create Menu
main_m = new Menu();
Menu* mode_m = new Menu();
@@ -339,3 +335,19 @@ void makeMenu() {
pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
}
void restart(void) {
lcdWrapper->clear();
lcdWrapper->setCursor(0, 0);
lcdWrapper->print("Rebooting ...");
ESP.restart();
}
void disconnectController(void) {
disconnectPs3 = true;
lcdWrapper->clear();
lcdWrapper->setCursor(0, 0);
lcdWrapper->print("Controller is");
lcdWrapper->setCursor(0, 1);
lcdWrapper->print("disconnected.");
}