diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp index 99434ad..d2c9a07 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp @@ -12,12 +12,7 @@ MenuTestMode::MenuTestMode(DriveManager* driveManager) : MenuDriveMode(driveManager) { - // this->testMode = testMode; - // this->values[0] = 0; - // this->values[1] = 0; - // strcpy(this->names[0], "Centimeter"); - // strcpy(this->names[1], "Degree"); } MenuTestMode::~MenuTestMode() { @@ -36,31 +31,45 @@ void MenuTestMode::printMenu() { } void MenuTestMode::down() { - this->mainMenu->down(); + if (this->checkInput()) + this->mainMenu->down(); } void MenuTestMode::up() { - this->mainMenu->up(); + if (this->checkInput()) + this->mainMenu->up(); } void MenuTestMode::right() { - this->mainMenu->right(); + if (this->checkInput()) + this->mainMenu->right(); } void MenuTestMode::left() { if (this->mainMenu->isInSubmenu()) - this->mainMenu->left(); + if (this->checkInput()) + this->mainMenu->left(); + else { + this->testMode->abortManeuver(); + this->mainMenu->left(); + } else this->parentMenu->printMenu(); } void MenuTestMode::yes() { - this->mainMenu->yes(); + if (this->checkInput()) + this->mainMenu->yes(); } void MenuTestMode::no() { if (this->mainMenu->isInSubmenu()) - this->mainMenu->no(); + if (this->checkInput()) + this->mainMenu->no(); + else { + this->testMode->abortManeuver(); + this->mainMenu->no(); + } else this->left(); } @@ -81,10 +90,10 @@ void MenuTestMode::init() { // Ping google // WheelEncoder - MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::drive)); - MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::leftEngine)); - MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::rightEngine)); - MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this->testMode, &TestMode::bothEngine)); + MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive)); + MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine)); + MenuIntInput* engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine)); + MenuIntInput* engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine)); // Set menus on LCD @@ -101,18 +110,22 @@ void MenuTestMode::init() { drivingMenu->setMinMaxSteps(0, -1000, 1000, 25); drivingMenu->setEntry(1, "Degree"); drivingMenu->setMinMaxSteps(1, -360, 360, 15); + drivingMenu->setPrintParentMenu(false); engineLeftMenu->setEntry(0, "Percentage"); engineLeftMenu->setMinMaxSteps(0, -100, 100, 5); - engineLeftMenu->setEntry(1, "Seconds", 10); + engineLeftMenu->setEntry(1, "Seconds", 5); engineLeftMenu->setMinMaxSteps(1, 0, 120, 1); + engineLeftMenu->setPrintParentMenu(false); engineRightMenu->setEntry(0, "Percentage"); engineRightMenu->setMinMaxSteps(0, -100, 100, 5); - engineRightMenu->setEntry(1, "Seconds", 10); + engineRightMenu->setEntry(1, "Seconds", 5); engineRightMenu->setMinMaxSteps(1, 0, 120, 1); + engineRightMenu->setPrintParentMenu(false); engineBothMenu->setEntry(0, "Percentage"); engineBothMenu->setMinMaxSteps(0, -100, 100, 5); - engineBothMenu->setEntry(1, "Seconds", 10); + engineBothMenu->setEntry(1, "Seconds", 5); engineBothMenu->setMinMaxSteps(1, 0, 120, 1); + engineBothMenu->setPrintParentMenu(false); // Entrys for the menus @@ -148,12 +161,46 @@ void MenuTestMode::init() { lightMenu->addEntry(lightOffAction); } -MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction) { - this->testMode = testMode; +void MenuTestMode::update() { + if (this->maneuverInAction) + if (millis() - this->lastUpdateTime > updateDelay) { + this->printManeuverTime(); + this->lastUpdateTime = millis(); + } +} + +bool MenuTestMode::checkInput() { + if (this->testMode->getBusy()) { + this->printManeuverTime(); + this->maneuverInAction = true; + return false; + } + + if (this->maneuverInAction) + this->maneuverInAction = false; + + return true; +} + +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; + lineTwo.concat("secs"); + std::cout << "MenuTestMode::printManeuverTime: 4" << std::endl; + this->print("Maneuver time", lineTwo); + std::cout << "MenuTestMode::printManeuverTime: 5" << std::endl; +} + +MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) { + this->menu = menu; + this->testMode = this->menu->getTestMode(); this->testModeFunctionSingle = testModeFunction; } -MenuTestModeWrapper::MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction) { +MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) { this->testMode = testMode; this->testModeFunctionDouble = testModeFunction; } @@ -162,8 +209,13 @@ 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; + this->menu->printManeuverTime(); + std::cout << "MenuTestModeWrapper::action: 2" << std::endl; } diff --git a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h index 93f8cf6..fd9bc5c 100644 --- a/src/SpecialMenus/driveModi/TestMode/menuTestMode.h +++ b/src/SpecialMenus/driveModi/TestMode/menuTestMode.h @@ -22,6 +22,7 @@ class MenuTestMode : public MenuDriveMode { MenuTestMode(DriveManager *driveManager); ~MenuTestMode(); void printMenu(); + void printManeuverTime(); void down() override; void up() override; @@ -30,26 +31,36 @@ class MenuTestMode : public MenuDriveMode { void yes() override; void no() override; + void update() override; + + TestMode* getTestMode() const { return this->testMode; } + private: void init(); + bool checkInput(); TestMode* testMode; Menu* mainMenu; bool isInit = false; + bool maneuverInAction = false; + + uint32_t lastUpdateTime = 0; + uint16_t updateDelay = 1000; }; typedef bool (TestMode::*TestModeFunctionSingle)(int16_t); typedef bool (TestMode::*TestModeFunctionDouble)(int16_t, int16_t); class MenuTestModeWrapper : public MenuIntInputWrapper { public: - MenuTestModeWrapper(TestMode* testMode, TestModeFunctionSingle testModeFunction); - MenuTestModeWrapper(TestMode* testMode, TestModeFunctionDouble testModeFunction); + MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction); + MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction); void action(int16_t* values, uint8_t length) override; private: - TestMode* testMode; + MenuTestMode* menu; + TestMode* testMode; TestModeFunctionSingle testModeFunctionSingle = nullptr; TestModeFunctionDouble testModeFunctionDouble = nullptr; }; diff --git a/src/driveModi/Modi/TestMode/testMode.cpp b/src/driveModi/Modi/TestMode/testMode.cpp index b97a246..c5ba056 100644 --- a/src/driveModi/Modi/TestMode/testMode.cpp +++ b/src/driveModi/Modi/TestMode/testMode.cpp @@ -22,8 +22,9 @@ void TestMode::loop() { if (millis() - this->lastMillis < this->delay) return; - if (millis() - this->actionStart > this->maneuverTime) { + if (millis() - this->actionStart > this->maneuverTime || this->abort) { this->busy = false; + this->abort = false; this->moveControl->setDrivingStatus(DrivingStatus::stop); } } @@ -82,59 +83,58 @@ bool TestMode::drive(int16_t cm, int16_t degree) { } bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) { - if (this->busy) - return false; - - if (powerPercentage >= 100 || powerPercentage <= -100) - return false; - - if (true) { - std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl; - return false; + std::cout << "TestMode::leftEngine: 1" << std::endl; + if (this->engineInit(powerPercentage, seconds)) { + this->moveControl->setRawPowerLeft(powerPercentage); + return true; } - - this->actionStart = millis(); - this->moveControl->setDrivingStatus(DrivingStatus::raw); - this->moveControl->setRawPowerLeft(powerPercentage); - this->maneuverTime = seconds * 1000; - this->busy = true; + return false; } bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) { - if (this->busy) - return false; - - if (powerPercentage >= 100 || powerPercentage <= -100) - return false; - - if (true) { - std::cout << "It works... %: " << powerPercentage << " s: " << seconds << std::endl; - return false; + if (this->engineInit(powerPercentage, seconds)) { + this->moveControl->setRawPowerRight(powerPercentage); + return true; } - - this->actionStart = millis(); - this->moveControl->setDrivingStatus(DrivingStatus::raw); - this->moveControl->setRawPowerRight(powerPercentage); - this->maneuverTime = seconds * 1000; - this->busy = true; + return false; } bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) { + if (this->engineInit(powerPercentage, seconds)) { + this->moveControl->setRawPowerLeft(powerPercentage); + this->moveControl->setRawPowerRight(powerPercentage);; + return true; + } + return false; +} + +void TestMode::abortManeuver() { + this->abort = true; +} + +uint8_t TestMode::getRemainingManeuverTime() { + if (busy) + return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000); + return 0; +} + +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; - 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; + return true; } diff --git a/src/driveModi/Modi/TestMode/testMode.h b/src/driveModi/Modi/TestMode/testMode.h index 818e784..736e60f 100644 --- a/src/driveModi/Modi/TestMode/testMode.h +++ b/src/driveModi/Modi/TestMode/testMode.h @@ -30,13 +30,21 @@ class TestMode : public DriveModi { bool rightEngine(int16_t powerPercentage, int16_t seconds); bool bothEngine(int16_t powerPercentage, int16_t seconds); + void abortManeuver(); + + uint8_t getRemainingManeuverTime(); + bool getBusy() const { return this->busy; } + private: + bool engineInit(int16_t powerPercentage, int16_t seconds); + MoveControl *moveControl; double speed = 0; double rotationSpeed = 0; bool busy = false; + bool abort = false; uint8_t delay = 10;