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

clangtidy corrections part 1

Browse Source
This commit is contained in:
kleiax
2023-10-11 17:35:40 +02:00
parent 77a52b82c3
commit 23d854a826
36 changed files with 2383 additions and 1165 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/SpecialMenus/Route/menuRoute.cpp
+127 -56
View File
@@ -1,36 +1,44 @@
/**
* @file menuRoute.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief Contains the implementation of the classes MenuActionRoute, MenuRoute and MenuRouteWrapper.
* @brief Contains the implementation of the classes MenuActionRoute, MenuRoute and MenuRouteWrapper.
* @version 0.1
* @date 2022-12-28
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "menuRoute.h"
MenuActionRoute::MenuActionRoute(MenuRoute* menuRoute, DataFunction dataFunction) {
MenuActionRoute::MenuActionRoute(MenuRoute *menuRoute, DataFunction dataFunction)
{
this->menuRoute = menuRoute;
this->dataFunction = dataFunction;
}
void MenuActionRoute::action() {
void MenuActionRoute::action()
{
(this->menuRoute->*this->dataFunction)(0);
}
MenuRoute::MenuRoute(Route* route) {
MenuRoute::MenuRoute(Route *route)
{
this->route = route;
}
MenuRoute::~MenuRoute() {
MenuRoute::~MenuRoute()
{
if (isInit)
{
delete this->mainMenu;
}
}
void MenuRoute::printMenu() {
if (!this->isInit) {
void MenuRoute::printMenu()
{
if (!this->isInit)
{
this->isInit = true;
this->init();
}
@@ -38,72 +46,103 @@ void MenuRoute::printMenu() {
this->mainMenu->printMenu();
}
void MenuRoute::down() {
if (blockInput) return;
void MenuRoute::down()
{
if (blockInput)
{
return;
}
this->mainMenu->down();
}
void MenuRoute::up() {
if (blockInput) return;
void MenuRoute::up()
{
if (blockInput)
{
return;
}
this->mainMenu->up();
}
void MenuRoute::right() {
if (blockInput) return;
void MenuRoute::right()
{
if (blockInput)
{
return;
}
this->mainMenu->right();
}
void MenuRoute::left() {
if (blockInput) return;
void MenuRoute::left()
{
if (blockInput)
{
return;
}
if (this->mainMenu->isInSubmenu())
{
this->mainMenu->left();
}
else
{
this->parentMenu->printMenu();
}
}
void MenuRoute::yes() {
if (blockInput) return;
void MenuRoute::yes()
{
if (blockInput)
{
return;
}
this->mainMenu->yes();
}
void MenuRoute::no() {
if (blockInput) return;
void MenuRoute::no()
{
if (blockInput)
{
return;
}
if (this->mainMenu->isInSubmenu())
{
this->mainMenu->no();
}
else
{
this->left();
}
}
void MenuRoute::init() {
void MenuRoute::init()
{
// auto dummy = []() {
// std::cout << "Dummy in Action" <<std::endl;
// };
this->mainMenu = new Menu;
MenuRoutePoints* pointsMenu = new MenuRoutePoints(this->route);
auto *pointsMenu = new MenuRoutePoints(this->route);
this->mainMenu->setLcd(this->lcd);
pointsMenu->setLcd(this->lcd);
auto importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute));
auto exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute));
auto deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute));
auto clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute);
auto *importWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::importRoute));
auto *exportWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::exportRoute));
auto *deleteWrapper = new MenuIntInput(1, new MenuRouteWrapper(this, &MenuRoute::deleteRoute));
auto *clearWrapper = new MenuActionRoute(this, &MenuRoute::clearRoute);
importWrapper->setLcd(this->lcd);
importWrapper->setMinMax(0, 100);
importWrapper->setMinMax(0, MenuRoute::maxRouteNumber);
importWrapper->setPrintParentMenu(false);
importWrapper->setEntry(0, "Import Route");
exportWrapper->setLcd(this->lcd);
exportWrapper->setMinMax(0, 100);
exportWrapper->setMinMax(0, MenuRoute::maxRouteNumber);
exportWrapper->setPrintParentMenu(false);
exportWrapper->setEntry(0, "Export Route");
deleteWrapper->setLcd(this->lcd);
deleteWrapper->setMinMax(0, 100);
deleteWrapper->setMinMax(0, MenuRoute::maxRouteNumber);
deleteWrapper->setPrintParentMenu(false);
deleteWrapper->setEntry(0, "Delete Route");
@@ -114,13 +153,15 @@ void MenuRoute::init() {
this->mainMenu->addEntry(new MenuAction("Delete", deleteWrapper));
}
void MenuRoute::importRoute(uint8_t routeNumber) {
void MenuRoute::importRoute(uint8_t routeNumber)
{
this->blockInput = true;
String lineOne = "";
String lineTwo = "";
if (WiFi.status() != WL_CONNECTED) {
if (WiFi.status() != WL_CONNECTED)
{
lineOne = "Not connected to";
lineTwo = "the WiFi.";
this->print(lineOne, lineTwo);
@@ -128,7 +169,8 @@ void MenuRoute::importRoute(uint8_t routeNumber) {
return;
}
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) {
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE)
{
lineOne = "Not enough mem";
lineTwo = "for Json obj";
this->print(lineOne, lineTwo);
@@ -139,7 +181,7 @@ void MenuRoute::importRoute(uint8_t routeNumber) {
WiFiClient client;
HTTPClient http;
DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE);
String host = "http://rover.kleiax.de/api/";
host.concat(routeNumber);
http.begin(client, host);
@@ -149,14 +191,19 @@ void MenuRoute::importRoute(uint8_t routeNumber) {
lineTwo = "Code: ";
if (httpResponseCode == 202)
{
deserializeJson(doc, http.getStream());
}
else
{
lineOne = "HTTP Error";
}
lineTwo.concat(httpResponseCode);
this->print(lineOne, lineTwo);
if (httpResponseCode <= 0) {
if (httpResponseCode <= 0)
{
this->blockInput = false;
return;
}
@@ -164,7 +211,8 @@ void MenuRoute::importRoute(uint8_t routeNumber) {
uint16_t totalPoints = doc["amountPoints"];
this->route->clear();
for (uint16_t i = 0; i < totalPoints; i++) {
for (uint16_t i = 0; i < totalPoints; i++)
{
Point::Coordinates coords;
coords.lat = doc["points"][i][0].as<double>();
coords.lon = doc["points"][i][1].as<double>();
@@ -175,14 +223,16 @@ void MenuRoute::importRoute(uint8_t routeNumber) {
http.end();
}
void MenuRoute::exportRoute(uint8_t routeNumber) {
void MenuRoute::exportRoute(uint8_t routeNumber)
{
this->blockInput = true;
uint16_t totalPoints = this->route->getRouteInfo().totalPoints;
String lineOne = "";
String lineTwo = "";
if (WiFi.status() != WL_CONNECTED) {
if (WiFi.status() != WL_CONNECTED)
{
lineOne = "Not connected to";
lineTwo = "the WiFi.";
this->print(lineOne, lineTwo);
@@ -190,7 +240,8 @@ void MenuRoute::exportRoute(uint8_t routeNumber) {
return;
}
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) {
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE)
{
lineOne = "Not enough mem";
lineTwo = "for Json obj";
this->print(lineOne, lineTwo);
@@ -205,7 +256,8 @@ void MenuRoute::exportRoute(uint8_t routeNumber) {
Point::Coordinates coords = this->route->startRoute().getCoordinates();
doc["points"][0][0] = coords.lat;
doc["points"][0][1] = coords.lon;
for (uint16_t i = 1; i < totalPoints; i++) {
for (uint16_t i = 1; i < totalPoints; i++)
{
coords = this->route->getNextPoint().getCoordinates();
doc["points"][i][0] = coords.lat;
doc["points"][i][1] = coords.lon;
@@ -215,30 +267,36 @@ void MenuRoute::exportRoute(uint8_t routeNumber) {
WiFiClient client;
HTTPClient http;
http.begin(client, "http://rover.kleiax.de/api/");
http.addHeader("Content-Type", "application/json");
int httpResponseCode = http.POST(jsonData);
if (httpResponseCode == 201)
{
lineOne = "Export complete";
}
else
{
lineOne = "HTTP Error";
}
lineTwo = "Code: ";
lineTwo.concat(httpResponseCode);
this->print(lineOne, lineTwo);
this->blockInput = false;
http.end();
}
void MenuRoute::deleteRoute(uint8_t routeNumber) {
void MenuRoute::deleteRoute(uint8_t routeNumber)
{
this->blockInput = true;
String lineOne = "";
String lineTwo = "";
if (WiFi.status() != WL_CONNECTED) {
if (WiFi.status() != WL_CONNECTED)
{
lineOne = "Not connected to";
lineTwo = "the WiFi.";
this->print(lineOne, lineTwo);
@@ -246,7 +304,8 @@ void MenuRoute::deleteRoute(uint8_t routeNumber) {
return;
}
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE) {
if (ESP.getMaxAllocHeap() < JSON_DOCUMENT_SIZE_ROUTE)
{
lineOne = "Not enough mem";
lineTwo = "for Json obj";
this->print(lineOne, lineTwo);
@@ -257,7 +316,7 @@ void MenuRoute::deleteRoute(uint8_t routeNumber) {
WiFiClient client;
HTTPClient http;
DynamicJsonDocument doc(JSON_DOCUMENT_SIZE_ROUTE);
String host = "http://rover.kleiax.de/api/";
host.concat(routeNumber);
http.begin(client, host);
@@ -267,9 +326,13 @@ void MenuRoute::deleteRoute(uint8_t routeNumber) {
lineTwo = "Code: ";
if (httpResponseCode == 202)
{
deserializeJson(doc, http.getStream());
}
else
{
lineOne = "HTTP Error";
}
lineTwo.concat(httpResponseCode);
this->print(lineOne, lineTwo);
@@ -278,29 +341,37 @@ void MenuRoute::deleteRoute(uint8_t routeNumber) {
http.end();
}
void MenuRoute::clearRoute(uint8_t none) {
void MenuRoute::clearRoute(uint8_t none)
{
//The parameter exist only for compability
none;
this->route->clear();
this->print("Currente route", "deleted...");
}
MenuRouteWrapper::MenuRouteWrapper(MenuRoute* menuRoute, DataFunction dataFunction) {
this->menuRoute = menuRoute;
this->dataFunction = dataFunction;
MenuRouteWrapper::MenuRouteWrapper(MenuRoute *menuRoute, DataFunction dataFunction)
: menuRoute {menuRoute}, dataFunction {dataFunction}
{
}
void MenuRouteWrapper::action(int16_t* values, uint8_t length) {
if (length < 1) {
void MenuRouteWrapper::action(int16_t *values, uint8_t length)
{
if (length < 1)
{
std::cout << "Error in MenuRouteWrapper::action" << std::endl;
return;
}
if (values[0] < 0)
{
values[0] = 0;
}
if (values[0] > UINT8_MAX)
{
values[0] = UINT8_MAX;
}
(this->menuRoute->*this->dataFunction)(values[0]);
}
src/SpecialMenus/Route/menuRoute.h
+6
View File
@@ -126,6 +126,12 @@ class MenuRoute : public MenuControl {
bool isInit = false;
bool blockInput = false;
static constexpr uint8_t maxRouteNumber = 100;
static constexpr uint16_t httpValidExport = 201;
static constexpr uint16_t httpValidImport = 202;
static constexpr uint16_t httpValidDelete = 202;
};
/**
src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
+77 -62
View File
@@ -10,8 +10,9 @@
*/
#include "menuTestMode.h"
MenuTestMode::MenuTestMode(DriveManager* driveManager) {
this->driveManager = driveManager;
MenuTestMode::MenuTestMode(DriveManager* driveManager)
: driveManager {driveManager}
{
}
MenuTestMode::~MenuTestMode() {
@@ -30,73 +31,77 @@ void MenuTestMode::printMenu() {
}
void MenuTestMode::down() {
if (this->checkInput())
if (this->checkInput()) {
this->mainMenu->down();
}
}
void MenuTestMode::up() {
if (this->checkInput())
if (this->checkInput()) {
this->mainMenu->up();
}
}
void MenuTestMode::right() {
if (this->checkInput())
if (this->checkInput()) {
this->mainMenu->right();
}
}
void MenuTestMode::left() {
if (this->mainMenu->isInSubmenu())
if (this->checkInput())
if (this->checkInput()) {
this->mainMenu->left();
}
else {
this->testMode->abortManeuver();
this->mainMenu->left();
}
else
else {
this->parentMenu->printMenu();
}
}
void MenuTestMode::yes() {
if (this->checkInput())
if (this->checkInput()) {
this->mainMenu->yes();
}
}
void MenuTestMode::no() {
if (this->mainMenu->isInSubmenu())
if (this->mainMenu->isInSubmenu()) {
if (this->checkInput())
this->mainMenu->no();
else {
this->testMode->abortManeuver();
this->mainMenu->no();
}
else
this->left();
}}
else{
this->left();}
}
void MenuTestMode::init() {
this->testMode = new TestMode();
this->driveManager->changeModus(this->testMode);
this->testMode->setSpeeds(DrivingSpeeds{1, 8});
auto dummy = []() {
std::cout << "Dummy in Action" <<std::endl;
};
// Create menu
this->mainMenu = new Menu;
auto engineMenu = new Menu;
auto lightMenu = new Menu;
auto encoderMenu = new Menu;
auto *engineMenu = new Menu;
auto *lightMenu = new Menu;
auto *encoderMenu = new Menu;
// Sound
// Ping google
auto drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
auto engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));
auto engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine));
auto engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine));
auto encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft());
auto encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight());
auto *drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
auto *engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));
auto *engineRightMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::rightEngine));
auto *engineBothMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::bothEngine));
auto *encoderLeftMenu = new SpeedometerTest(this->testMode->getSpeedometerLeft());
auto *encoderRightMenu = new SpeedometerTest(this->testMode->getSpeedometerRight());
// Set menus on LCD
@@ -112,47 +117,52 @@ void MenuTestMode::init() {
encoderRightMenu->setLcd(this->lcd);
// Other menu config
static constexpr uint8_t percentMax = 100;
static constexpr uint8_t degreeMax = 360;
static constexpr uint8_t secondsMax = 120;
static constexpr uint8_t steps = 5;
drivingMenu->setEntry(0, "Centimeter", 100);
drivingMenu->setMinMaxSteps(0, -1000, 1000, 25);
drivingMenu->setMinMaxSteps(0, -1000, 1000, steps * 5);
drivingMenu->setEntry(1, "Degree");
drivingMenu->setMinMaxSteps(1, -360, 360, 15);
drivingMenu->setMinMaxSteps(1, -degreeMax, degreeMax, steps * 3);
drivingMenu->setPrintParentMenu(false);
engineLeftMenu->setEntry(0, "Percentage");
engineLeftMenu->setMinMaxSteps(0, -100, 100, 5);
engineLeftMenu->setEntry(1, "Seconds", 5);
engineLeftMenu->setMinMaxSteps(1, 0, 120, 1);
engineLeftMenu->setMinMaxSteps(0, -percentMax, percentMax, steps);
engineLeftMenu->setEntry(1, "Seconds", steps);
engineLeftMenu->setMinMaxSteps(1, 0, secondsMax, 1);
engineLeftMenu->setPrintParentMenu(false);
engineRightMenu->setEntry(0, "Percentage");
engineRightMenu->setMinMaxSteps(0, -100, 100, 5);
engineRightMenu->setEntry(1, "Seconds", 5);
engineRightMenu->setMinMaxSteps(1, 0, 120, 1);
engineRightMenu->setMinMaxSteps(0, -percentMax, percentMax, steps);
engineRightMenu->setEntry(1, "Seconds", steps);
engineRightMenu->setMinMaxSteps(1, 0, secondsMax, 1);
engineRightMenu->setPrintParentMenu(false);
engineBothMenu->setEntry(0, "Percentage");
engineBothMenu->setMinMaxSteps(0, -100, 100, 5);
engineBothMenu->setEntry(1, "Seconds", 5);
engineBothMenu->setMinMaxSteps(1, 0, 120, 1);
engineBothMenu->setMinMaxSteps(0, -percentMax, percentMax, steps);
engineBothMenu->setEntry(1, "Seconds", steps);
engineBothMenu->setMinMaxSteps(1, 0, secondsMax, 1);
engineBothMenu->setPrintParentMenu(false);
// Entrys for the menus
MenuAction* engineAction = new MenuAction("Engine", engineMenu);
MenuAction* engineLeftAction = new MenuAction("Left", engineLeftMenu);
MenuAction* engineRightAction = new MenuAction("Right", engineRightMenu);
MenuAction* engineBothAction = new MenuAction("Both", engineBothMenu);
auto *engineAction = new MenuAction("Engine", engineMenu);
auto *engineLeftAction = new MenuAction("Left", engineLeftMenu);
auto *engineRightAction = new MenuAction("Right", engineRightMenu);
auto *engineBothAction = new MenuAction("Both", engineBothMenu);
MenuAction* drivingAction = new MenuAction("Driving", drivingMenu);
auto *drivingAction = new MenuAction("Driving", drivingMenu);
MenuAction* encoderAction = new MenuAction("Encoder", encoderMenu);
MenuAction* encoderLeftAction = new MenuAction("Left", encoderLeftMenu);
MenuAction* encoderRightAction = new MenuAction("Right", encoderRightMenu);
auto *encoderAction = new MenuAction("Encoder", encoderMenu);
auto *encoderLeftAction = new MenuAction("Left", encoderLeftMenu);
auto *encoderRightAction = new MenuAction("Right", encoderRightMenu);
MenuAction* lightAction = new MenuAction("Light", lightMenu);
MenuAction* lightFlashAction = new MenuAction("Flash", dummy);
MenuAction* lightFadeAction = new MenuAction("Fade", dummy);
MenuAction* lightRedAction = new MenuAction("Red", dummy);
MenuAction* lightGreenAction = new MenuAction("Green", dummy);
MenuAction* lightBlueAction = new MenuAction("Blue", dummy);
MenuAction* lightOffAction = new MenuAction("Off", dummy);
auto *lightAction = new MenuAction("Light", lightMenu);
auto *lightFlashAction = new MenuAction("Flash", dummy);
auto *lightFadeAction = new MenuAction("Fade", dummy);
auto *lightRedAction = new MenuAction("Red", dummy);
auto *lightGreenAction = new MenuAction("Green", dummy);
auto *lightBlueAction = new MenuAction("Blue", dummy);
auto *lightOffAction = new MenuAction("Off", dummy);
// Add entrys to the menus
this->mainMenu->addEntry(engineAction);
@@ -176,11 +186,11 @@ void MenuTestMode::init() {
}
void MenuTestMode::update() {
if (this->maneuverInAction)
if (this->maneuverInAction) {
if (millis() - this->lastUpdateTime > updateDelay) {
this->printManeuverTime();
this->lastUpdateTime = millis();
}
}}
}
bool MenuTestMode::checkInput() {
@@ -190,8 +200,9 @@ bool MenuTestMode::checkInput() {
return false;
}
if (this->maneuverInAction)
if (this->maneuverInAction) {
this->maneuverInAction = false;
}
return true;
}
@@ -209,16 +220,18 @@ void MenuTestMode::printManeuverTime() {
}
}
MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction) {
this->menu = menu;
this->testMode = this->menu->getTestMode();
this->testModeFunctionSingle = testModeFunction;
MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionSingle testModeFunction)
: menu {menu},
testModeFunctionSingle {testModeFunction},
testMode {menu->getTestMode()}
{
}
MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction) {
this->menu = menu;
this->testMode = this->menu->getTestMode();
this->testModeFunctionDouble = testModeFunction;
MenuTestModeWrapper::MenuTestModeWrapper(MenuTestMode* menu, TestModeFunctionDouble testModeFunction)
: menu {menu},
testModeFunctionDouble {testModeFunction},
testMode {menu->getTestMode()}
{
}
void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
@@ -228,10 +241,12 @@ void MenuTestModeWrapper::action(int16_t* values, uint8_t length) {
bool res = false;
if (length == 2 && this->testModeFunctionDouble)
if (length == 2 && static_cast<bool>(this->testModeFunctionDouble)) {
res = (this->testMode->*this->testModeFunctionDouble)(values[0], values[1]);
else if (length == 1 && this->testModeFunctionSingle)
}
else if (length == 1 && static_cast<bool>(this->testModeFunctionSingle)) {
res = (this->testMode->*this->testModeFunctionSingle)(values[0]);
}
if (res) {
this->menu->printManeuverTime();
src/SpecialMenus/driveModi/TestMode/menuTestMode.h
+2 -2
View File
@@ -70,8 +70,8 @@ class MenuTestMode : public MenuControl {
bool checkInput();
DriveManager* driveManager;
TestMode* testMode;
Menu* mainMenu;
TestMode* testMode = nullptr;
Menu* mainMenu = nullptr;
bool isInit = false;
bool maneuverInAction = false;
src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp
+5 -3
View File
@@ -11,8 +11,9 @@
#include "speedometerTest.h"
SpeedometerTest::SpeedometerTest(Speedometer* speedometer) {
this->speedometer = speedometer;
SpeedometerTest::SpeedometerTest(Speedometer* speedometer)
: speedometer {speedometer}
{
this->speedometer->setDirection(Speedometer::Forward);
}
@@ -46,8 +47,9 @@ void SpeedometerTest::left() {
}
void SpeedometerTest::no() {
if (this->state == State::Running)
if (this->state == State::Running){
speedometer->calibrationMeasurementStop();
}
this->state = State::Off;
this->parentMenu->printMenu();
src/SpecialMenus/driveModi/menuDriveMode.cpp
+17 -14
View File
@@ -11,27 +11,28 @@
#include "menuDriveMode.h"
MenuDriveMode::MenuDriveMode(DriveManager* driveManager) {
this->driveManager = driveManager;
MenuDriveMode::MenuDriveMode(DriveManager* driveManager)
: driveManager {driveManager}
{
}
void MenuDriveMode::left() {
if (this->activeMenu) {
if (this->activeMenu)
if (static_cast<bool>(this->activeMenu)) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->left();
}
return;
}
this->firstPrint = true;
this->configureOnLeave();
if (this->menuSpeed)
delete this->menuSpeed;
this->driveManager->changeModus();
MenuInformationSites::left();
}
void MenuDriveMode::right() {
if (this->activeMenu) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->right();
return;
}
@@ -39,7 +40,7 @@ void MenuDriveMode::right() {
}
void MenuDriveMode::up() {
if (this->activeMenu) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->up();
return;
}
@@ -47,7 +48,7 @@ void MenuDriveMode::up() {
}
void MenuDriveMode::down() {
if (this->activeMenu) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->down();
return;
}
@@ -55,7 +56,7 @@ void MenuDriveMode::down() {
}
void MenuDriveMode::yes() {
if (this->activeMenu) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->yes();
return;
}
@@ -63,7 +64,7 @@ void MenuDriveMode::yes() {
}
void MenuDriveMode::no() {
if (this->activeMenu) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->no();
return;
}
@@ -75,7 +76,7 @@ void MenuDriveMode::prepareReenterMenu() {
}
void MenuDriveMode::printMenu() {
if (this->activeMenu) {
if (static_cast<bool>(this->activeMenu)) {
this->activeMenu->printMenu();
return;
}
@@ -88,11 +89,12 @@ void MenuDriveMode::init() {
}
void MenuDriveMode::activateSpeedMenu() {
if (!this->driveManager->isActive())
if (!this->driveManager->isActive()) {
return;
}
DrivingSpeeds& speeds = this->driveManager->getDriveModiPtr()->getSpeedsRef();
MenuIntInput* menu = new MenuIntInput(2, new MenuSpeed(speeds));
auto *menu = new MenuIntInput(2, new MenuSpeed(speeds));
menu->setMinMax(5, UINT8_MAX);
menu->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10);
@@ -112,8 +114,9 @@ void MenuDriveMode::enterSensorMenu() {
}
void MenuDriveMode::enterMenu() {
if (!this->activeMenu)
if (!static_cast<bool>(this->activeMenu)) {
return;
}
this->activeMenu->printMenu();
this->activeMenu->setParentMenu(this);
src/driveModi/Modi/Autopilot/autopilot.cpp
+167 -118
View File
@@ -4,138 +4,161 @@
* @brief Contains the implementation of the class Autopilot
* @version 0.1
* @date 2022-02-02
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "autopilot.h"
DirectionChangeSignal::DirectionChangeSignal(Autopilot* pilot) {
this->pilot = pilot;
DirectionChangeSignal::DirectionChangeSignal(Autopilot *pilot)
: pilot {pilot}
{
this->action();
}
DirectionChangeSignal::~DirectionChangeSignal() {
CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass();
if (calcAzimuth)
DirectionChangeSignal::~DirectionChangeSignal()
{
CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
if (static_cast<bool>(calcAzimuth))
{
calcAzimuth->disableCalcAzimuth();
}
}
void DirectionChangeSignal::action() {
CalcAzimuth* calcAzimuth = pilot->getSensorData()->getCalcCompass();
if (calcAzimuth)
void DirectionChangeSignal::action()
{
CalcAzimuth *calcAzimuth = pilot->getSensorData()->getCalcCompass();
if (static_cast<bool>(calcAzimuth))
{
calcAzimuth->drivingDirectionChange(pilot->getSensorData()->getCurrentPos());
}
}
Autopilot::Autopilot() {
}
Autopilot::~Autopilot() {
Autopilot::~Autopilot()
{
delete this->directionChangeSignal;
// this->navigation->getNTRIPClient()->setActivated(false);
}
void Autopilot::run() {
void Autopilot::run()
{
this->routeInfo = this->navigation->getRouteInfo();
switch (this->state) {
case State::InsufficientAccuracy:
this->askNavigationForOrder();
return;
switch (this->state)
{
case State::InsufficientAccuracy:
this->askNavigationForOrder();
return;
case State::NoRoute:
return;
case State::NoRoute:
return;
case State::None:
return;
case State::None:
return;
case State::NavigationStarted:
this->askNavigationForOrder();
this->checkButtonInput();
break;
case State::NavigationStarted:
this->askNavigationForOrder();
this->checkButtonInput();
break;
case State::GetToStartPoint:
ManualControl::run();
this->askNavigationForOrder();
if (this->routeInfo.currentPoint >= 2)
this->state = State::SelfDrivingAvailable;
break;
case State::GetToStartPoint:
ManualControl::run();
this->askNavigationForOrder();
if (this->routeInfo.currentPoint >= 2){
this->state = State::SelfDrivingAvailable;
}
break;
case State::SelfDrivingAvailable:
ManualControl::run();
this->askNavigationForOrder();
this->checkButtonInput();
break;
case State::SelfDrivingAvailable:
ManualControl::run();
this->askNavigationForOrder();
this->checkButtonInput();
break;
case State::SelfDriving:
this->askNavigationForOrder();
this->checkButtonInput();
this->selfDriving();
break;
case State::SelfDriving:
this->askNavigationForOrder();
this->checkButtonInput();
this->selfDriving();
break;
case SelfDrivingRotate:
this->checkButtonInput();
this->rotate();
break;
case SelfDrivingRotate:
this->checkButtonInput();
this->rotate();
break;
case State::TargetReached:
if (this->loopMode)
this->restartLoop();
break;
default:
break;
case State::TargetReached:
if (this->loopMode){
this->restartLoop();
}
break;
default:
break;
}
}
void Autopilot::restart() {
void Autopilot::restart()
{
this->init();
}
bool Autopilot::shouldUpdate() {
if (this->updateDisplay) {
bool Autopilot::shouldUpdate()
{
if (this->updateDisplay)
{
this->updateDisplay = false;
return true;
}
return false;
}
void Autopilot::testRotate(int16_t degree) {
if (!degree)
void Autopilot::testRotate(int16_t degree)
{
if (!static_cast<bool>(degree))
{
return;
}
this->courseCorrection.correction = degree;
this->beginRotate();
}
void Autopilot::init() {
void Autopilot::init()
{
if (this->navigation->startNavigation())
this->state = State::NavigationStarted;
else
this->state = State::NoRoute;
this->routeInfo = this->navigation->getRouteInfo();
this->sensorData->getNtripClient()->setAutoReconnect(true);
{
this->state = State::NavigationStarted;
}
else
{
this->state = State::NoRoute;
}
this->routeInfo = this->navigation->getRouteInfo();
this->sensorData->getNtripClient()->setAutoReconnect(true);
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection, true);
this->courseCorrection.correction = 0;
this->courseCorrection.distance = 0;
this->updateDisplay = true;
this->courseCorrection.correction = 0;
this->courseCorrection.distance = 0;
this->updateDisplay = true;
}
void Autopilot::drive() {
void Autopilot::drive()
{
DrivingSpeeds speeds = {0, 0};
if (this->courseCorrection.distance >= this->minRemainingDistance)
{
speeds.x = this->maxSpeeds.x;
}
this->moveControl->setSpeeds(speeds);
}
void Autopilot::beginRotate() {
if (this->state != State::SelfDrivingRotate) {
void Autopilot::beginRotate()
{
if (this->state != State::SelfDrivingRotate)
{
this->lastState = this->state;
this->state = State::SelfDrivingRotate;
this->rotationAimAzimuth = this->getSensorData()->getRealAzimuth() + this->courseCorrection.correction;
@@ -143,35 +166,40 @@ void Autopilot::beginRotate() {
DrivingSpeeds speeds = {0, 0};
if (this->courseCorrection.correction > 0)
{
speeds.rot = -this->maxSpeeds.rot;
}
else
{
speeds.rot = this->maxSpeeds.rot;
}
this->moveControl->setSpeeds(speeds);
}
}
void Autopilot::rotate() {
if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct) {
void Autopilot::rotate()
{
if (abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct)
{
this->endRotate();
return;
}
if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3)
&&
((this->courseCorrection.correction > 0
&& this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth())
|| (this->courseCorrection.correction < 0
&& this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
if ((abs(this->getSensorData()->getRealAzimuth() - this->rotationAimAzimuth) < this->maxCourseDeviationBeforeAct * 3) &&
((this->courseCorrection.correction > 0 && this->rotationAimAzimuth < this->getSensorData()->getRealAzimuth()) || (this->courseCorrection.correction < 0 && this->rotationAimAzimuth > this->getSensorData()->getRealAzimuth())))
{
this->endRotate();
std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
}
}
}
void Autopilot::endRotate() {
void Autopilot::endRotate()
{
if (this->state != State::SelfDrivingRotate)
{
return;
}
this->state = this->lastState;
this->sensorData->getCalcCompass()->drivingDirectionChange(this->sensorData->getCurrentPos());
@@ -180,65 +208,85 @@ void Autopilot::endRotate() {
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
}
void Autopilot::checkButtonInput() {
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)
&& millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis) {
if (this->state == State::SelfDrivingAvailable)
void Autopilot::checkButtonInput()
{
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action) && millis() - this->lastAutopilotChangeMillis > this->autopilotChangeDelayMillis)
{
if (this->state == State::SelfDrivingAvailable)
{
this->state = State::SelfDriving;
}
else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
{
this->state = State::SelfDrivingAvailable;
}
else if (this->state == State::NavigationStarted)
{
this->state = State::GetToStartPoint;
}
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
}
}
void Autopilot::askNavigationForOrder() {
void Autopilot::askNavigationForOrder()
{
this->lastOrderStatus = this->navigation->getCourseCorrection(this->courseCorrection);
switch (this->lastOrderStatus) {
case Navigation::Status::Complete:
this->state = State::TargetReached;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
case Navigation::Status::InsufficientAccuracy:
if (this->state == State::InsufficientAccuracy)
break;
this->lastState = this->state;
this->state = State::InsufficientAccuracy;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
switch (this->lastOrderStatus)
{
case Navigation::Status::Complete:
this->state = State::TargetReached;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
case Navigation::Status::Unchanged:
if (this->state == State::InsufficientAccuracy)
this->state = this->lastState;
case Navigation::Status::InsufficientAccuracy:
if (this->state == State::InsufficientAccuracy){
break;
}
this->lastState = this->state;
this->state = State::InsufficientAccuracy;
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
break;
case Navigation::Status::Updated:
if (this->state == State::InsufficientAccuracy)
this->state = this->lastState;
break;
default:
break;
case Navigation::Status::Unchanged:
if (this->state == State::InsufficientAccuracy){
this->state = this->lastState;
}
break;
case Navigation::Status::Updated:
if (this->state == State::InsufficientAccuracy){
this->state = this->lastState;
}
break;
default:
break;
}
}
void Autopilot::selfDriving() {
void Autopilot::selfDriving()
{
if (this->state != State::SelfDriving)
{
return;
}
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBeforeAct)
{
this->beginRotate();
}
else
{
this->drive();
}
}
void Autopilot::restartLoop() {
void Autopilot::restartLoop()
{
this->navigation->startNavigation();
this->state = State::SelfDriving;
this->routeInfo = this->navigation->getRouteInfo();
@@ -246,7 +294,8 @@ void Autopilot::restartLoop() {
this->updateDisplay = true;
}
void Autopilot::afterActivate() {
void Autopilot::afterActivate()
{
this->setInputMode(ManualControl::InputMode::Digital);
this->directionChangeSignal = new DirectionChangeSignal(this);
this->setDirectionChangeCallback(this->directionChangeSignal);
src/driveModi/Modi/Autopilot/autopilot.h
+5 -13
View File
@@ -41,14 +41,6 @@ class Autopilot : public ManualControl {
TargetReached
};
/**
* @brief Construct a new Autopilot object
*
* @param moveControl for ManualControl
* @param navigation for route instructions
*/
Autopilot();
/**
* @brief Destroy the Autopilot object
*
@@ -104,13 +96,13 @@ class Autopilot : public ManualControl {
void restartLoop();
void afterActivate() override;
Navigation* navigation;
CourseCorrection courseCorrection;
RouteInfo routeInfo;
Navigation* navigation = nullptr;
CourseCorrection courseCorrection{0, 0};
RouteInfo routeInfo{0, 0};
State state = State::None;
State lastState = State::None;
Navigation::Status lastOrderStatus;
DirectionChangeSignal* directionChangeSignal;
DirectionChangeSignal* directionChangeSignal = nullptr;
bool updateDisplay = false;
bool loopMode = false;
@@ -119,7 +111,7 @@ class Autopilot : public ManualControl {
uint16_t autopilotChangeDelayMillis = 500;
uint32_t lastAutopilotChangeMillis = 0;
int16_t rotationAimAzimuth;
int16_t rotationAimAzimuth = 0;
double minRemainingDistance = 0.25;
};
src/driveModi/Modi/CalibrateCompass/calibrateCompassM.cpp
+2 -2
View File
@@ -13,10 +13,10 @@
void CalibrateCompassM::setCalibrateCompass(CalibrateCompass *caliCompass) {
if (caliCompass) {
if (static_cast<bool>(caliCompass)) {
this->caliCompass = caliCompass;
this->addChildComponent(this->caliCompass);
} else if (this->caliCompass) {
} else if (static_cast<bool>(this->caliCompass)) {
this->removeChildComponent(this->caliCompass);
this->caliCompass = caliCompass;
}
src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+27 -16
View File
@@ -4,27 +4,32 @@
* @brief Contains the implementation of the class CaptureRoute
* @version 0.1
* @date 2022-02-15
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "driveModi/Modi/CaptureRoute/captureRoute.h"
#include "captureRoute.h"
CaptureRoute::~CaptureRoute() {
if (this->navigation)
delete this->navigation;
CaptureRoute::~CaptureRoute()
{
delete this->navigation;
if (this->sensorData->getNtripClient())
if (static_cast<bool>(this->sensorData->getNtripClient()))
{
this->sensorData->getNtripClient()->setActivated(false);
}
}
void CaptureRoute::run() {
void CaptureRoute::run()
{
ManualControl::run();
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action))
{
this->status = this->navigation->addCurrentPosToRoute();
if (this->status == Navigation::Status::Updated) {
if (this->status == Navigation::Status::Updated)
{
this->lastSavedPoint = this->navigation->getCurrentPosition();
this->routeInfo = navigation->getRouteInfo();
this->updateDisplay = true;
@@ -32,21 +37,27 @@ void CaptureRoute::run() {
}
}
void CaptureRoute::afterActivate() {
void CaptureRoute::afterActivate()
{
this->navigation = new Navigation(this->sensorData);
this->navigation->getRoute()->clear();
this->sensorData->getNtripClient()->setAutoReconnect(true);
this->routeInfo = navigation->getRouteInfo();
this->navigation->getRoute()->clear();
this->sensorData->getNtripClient()->setAutoReconnect(true);
this->routeInfo = navigation->getRouteInfo();
}
double CaptureRoute::getDistanceToLastPoint() const {
double CaptureRoute::getDistanceToLastPoint() const
{
if (!this->lastSavedPoint.isInit())
{
return 0;
}
return this->lastSavedPoint.distanceTo(this->navigation->getCurrentPosition());
}
bool CaptureRoute::shouldUpdate() {
if (this->updateDisplay) {
bool CaptureRoute::shouldUpdate()
{
if (this->updateDisplay)
{
this->updateDisplay = false;
return true;
}
src/driveModi/Modi/ManualControl/manualControl.cpp
+53 -31
View File
@@ -4,67 +4,89 @@
* @brief Implementation of the class manualControl.h
* @version 0.1
* @date 2021-12-13
*
*
* @copyright Copyright (c) 2021
*
*
*/
#include "manualControl.h"
void ManualControl::run() {
switch (this->inputMode) {
case InputMode::Analog :
this->analogControl();
break;
void ManualControl::run()
{
switch (this->inputMode)
{
case InputMode::Analog:
this->analogControl();
break;
case InputMode::Digital :
this->digitalControl();
break;
default:
break;
case InputMode::Digital:
this->digitalControl();
break;
default:
break;
}
}
void ManualControl::switchInputMode() {
void ManualControl::switchInputMode()
{
this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
}
void ManualControl::analogControl() {
void ManualControl::analogControl()
{
// Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
int16_t x = this->input->x - 127;
int16_t y = this->input->y - 127;
const int16_t xAxis = this->input->x - 127;
const int16_t yAxis = this->input->y - 127;
double value_per_step = this->maxSpeeds.x * 2 / UINT8_MAX;
this->moveControl->setSpeed(-x * value_per_step);
this->moveControl->setSpeed(-xAxis * value_per_step);
value_per_step = this->maxSpeeds.rot * 2 / UINT8_MAX;
this->moveControl->setRotationSpeed(y * value_per_step);
this->moveControl->setRotationSpeed(yAxis * value_per_step);
}
void ManualControl::digitalControl() {
int16_t y = this->input->x - 127;
int16_t x = this->input->y - 127;
void ManualControl::digitalControl()
{
static constexpr uint8_t deadzone = 120;
const int16_t yAxis = this->input->x - 127;
const int16_t xAxis = this->input->y - 127;
if (y > 120)
if (yAxis > deadzone)
{
this->moveControl->setSpeed(-this->maxSpeeds.x);
else if (y < -120)
}
else if (yAxis < -deadzone)
{
this->moveControl->setSpeed(this->maxSpeeds.rot);
}
else
{
this->moveControl->setSpeed(0);
}
if (x > 120)
if (xAxis > deadzone)
{
this->moveControl->setRotationSpeed(this->maxSpeeds.rot);
else if (x < -120)
}
else if (xAxis < -deadzone)
{
this->moveControl->setRotationSpeed(-this->maxSpeeds.rot);
}
else
{
this->moveControl->setRotationSpeed(0);
}
if (this->directionChangeWrapper && (x > 120 || x < -120))
if (static_cast<bool>(this->directionChangeWrapper) && (xAxis > deadzone || xAxis < -deadzone))
{
this->lastLoopTurned = true;
else if (this->lastLoopTurned) {
if (this->directionChangeWrapper)
}
else if (this->lastLoopTurned)
{
if (static_cast<bool>(this->directionChangeWrapper))
{
this->directionChangeWrapper->action();
}
this->lastLoopTurned = false;
}
}
}
src/driveModi/Modi/TestMode/testMode.cpp
+71 -42
View File
@@ -4,20 +4,24 @@
* @brief Contains the implementation of the class TestMode.
* @version 0.1
* @date 2022-09-08
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "testMode.h"
void TestMode::run() {
if (this->maneuver == Maneuver::Turn) {
void TestMode::run()
{
if (this->maneuver == Maneuver::Turn)
{
uint16_t delta = abs(this->azimuth - this->getSensorData()->getRealAzimuth());
if (delta > this->degree)
if (delta > this->degree) {
this->abort = true;
}
}
if (this->busy && millis() - this->actionStart > this->maneuverTime || this->abort) {
if (this->busy && millis() - this->actionStart > this->maneuverTime || this->abort)
{
this->busy = false;
this->abort = false;
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
@@ -25,62 +29,76 @@ void TestMode::run() {
}
}
bool TestMode::drive(int16_t cm, int16_t degree) {
if (this->busy)
bool TestMode::drive(int16_t cmDistance, int16_t degree)
{
if (this->busy){
return false;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
if (cm == 0) {
//Only left or right
if (cmDistance == 0)
{
// Only left or right
this->moveControl->setSpeed(0);
if (degree < 0)
if (degree < 0){
this->moveControl->setRotationSpeed(-this->maxSpeeds.rot);
else if (degree > 0)
}
else if (degree > 0){
this->moveControl->setRotationSpeed(this->maxSpeeds.rot);
}
this->azimuth = this->getSensorData()->getRealAzimuth();
this->degree = degree;
this->maneuverTime = 5 * 1000;
this->busy = true;
return true;
} else if (degree == 0) {
//Only forward or backward
}
if (degree == 0)
{
// Only forward or backward
this->moveControl->setRotationSpeed(0);
if (cm < 0)
if (cmDistance < 0){
this->moveControl->setSpeed(-this->maxSpeeds.x);
else if (cm > 0)
}
else if (cmDistance > 0){
this->moveControl->setSpeed(this->maxSpeeds.x);
this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxSpeeds.x) * 1000;
}
this->maneuverTime = static_cast<uint32_t>(((abs(cmDistance) / 100.0) / this->maxSpeeds.x) * 1000);
this->busy = true;
this->maneuver = Maneuver::Drive;
return true;
} else {
//forward or backward and left or right
// TODO: Calculate roationspeed
if (cm < 0)
}
else
{
// forward or backward and left or right
// TODO: Calculate roationspeed
if (cmDistance < 0){
this->moveControl->setSpeed(-this->maxSpeeds.x);
else if (cm > 0)
}
else if (cmDistance > 0){
this->moveControl->setSpeed(this->maxSpeeds.x);
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->maxSpeeds.x) * 1000;
}
this->maneuverTime = static_cast<uint32_t>(((cmDistance / 100.0) / this->maxSpeeds.x) * 1000);
this->busy = true;
this->maneuver = Maneuver::Drive;
return true;
}
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
return false;
}
bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
if (this->engineInit(powerPercentage, seconds)) {
bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds)
{
if (this->engineInit(powerPercentage, seconds))
{
this->moveControl->setRawPowerLeft(powerPercentage);
this->maneuver = Maneuver::LeftEngine;
std::cout << "TestMode::leftEngine" << std::endl;
@@ -89,8 +107,10 @@ bool TestMode::leftEngine(int16_t powerPercentage, int16_t seconds) {
return false;
}
bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
if (this->engineInit(powerPercentage, 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;
@@ -98,8 +118,10 @@ bool TestMode::rightEngine(int16_t powerPercentage, int16_t seconds) {
return false;
}
bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
if (this->engineInit(powerPercentage, seconds)) {
bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds)
{
if (this->engineInit(powerPercentage, seconds))
{
this->moveControl->setRawPowerLeft(powerPercentage);
this->moveControl->setRawPowerRight(powerPercentage);
this->maneuver = Maneuver::BothEngine;
@@ -108,23 +130,30 @@ bool TestMode::bothEngine(int16_t powerPercentage, int16_t seconds) {
return false;
}
void TestMode::abortManeuver() {
void TestMode::abortManeuver()
{
this->abort = true;
}
uint8_t TestMode::getRemainingManeuverTime() const {
if (this->busy)
return (uint8_t) ((this->maneuverTime - (millis() - this->actionStart)) / 1000);
uint8_t TestMode::getRemainingManeuverTime() const
{
if (this->busy){
return static_cast<uint8_t>((this->maneuverTime - (millis() - this->actionStart)) / 1000);
}
return 0;
}
bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds) {
if (this->busy)
bool TestMode::engineInit(int16_t powerPercentage, int16_t seconds)
{
if (this->busy){
return false;
if (powerPercentage > 100 || powerPercentage < -100)
}
if (powerPercentage > TestMode::maxPercentage || powerPercentage < -TestMode::maxPercentage){
return false;
if (seconds < 0)
}
if (seconds < 0){
return false;
}
this->actionStart = millis();
this->moveControl->setDrivingStatus(MoveControl::Status::Raw);
src/driveModi/Modi/TestMode/testMode.h
+83 -80
View File
@@ -4,11 +4,11 @@
* @brief Contains a class to test different functions from the rover.
* @version 0.1
* @date 2022-09-08
*
*
* @copyright Copyright (c) 2022
*
*
*/
#ifndef TEST_MODE_H
#ifndef TEST_MODE_H
#define TEST_MODE_H
#include <iostream>
@@ -16,97 +16,100 @@
#include "moveControl.h"
#include "driveModi/driveModi.h"
/**
* @brief Test different functions from the rover.
*
*
* You can test engine, engine controller, light and speed meter.
*/
class TestMode : public DriveModi {
public:
/**
* @brief Different states to test the engine
*
*/
enum Maneuver {
None,
LeftEngine,
RightEngine,
BothEngine,
Turn,
Drive
};
class TestMode : public DriveModi
{
public:
/**
* @brief Different states to test the engine
*
*/
enum Maneuver
{
None,
LeftEngine,
RightEngine,
BothEngine,
Turn,
Drive
};
bool drive(int16_t cm = 0, int16_t degree = 0);
bool drive(int16_t cmDistance = 0, int16_t degree = 0);
/**
* @brief Sets the left Engine to a specific power value.
*
* This start a maneuver with the given time.
*
* @param powerPercentage from 0% to 100%
* @param seconds time to run the engine
* @return true success
* @return false failure
*/
bool leftEngine(int16_t powerPercentage, int16_t seconds);
/**
* @brief Sets the left Engine to a specific power value.
*
* This start a maneuver with the given time.
*
* @param powerPercentage from 0% to 100%
* @param seconds time to run the engine
* @return true success
* @return false failure
*/
bool leftEngine(int16_t powerPercentage, int16_t seconds);
/**
* @brief See leftEngine
*
* @param powerPercentage
* @param seconds
* @return true
* @return false
*/
bool rightEngine(int16_t powerPercentage, int16_t seconds);
/**
* @brief See leftEngine
*
* @param powerPercentage
* @param seconds
* @return true
* @return false
*/
bool rightEngine(int16_t powerPercentage, int16_t seconds);
/**
* @brief See leftEngine
*
* @param powerPercentage
* @param seconds
* @return true
* @return false
*/
bool bothEngine(int16_t powerPercentage, int16_t seconds);
/**
* @brief See leftEngine
*
* @param powerPercentage
* @param seconds
* @return true
* @return false
*/
bool bothEngine(int16_t powerPercentage, int16_t seconds);
/**
* @brief Abort the running maneuver.
*
* Stops all movement. This is the only possibility to cancel a maneuver
* before the time is up.
*/
void abortManeuver();
/**
* @brief Abort the running maneuver.
*
* Stops all movement. This is the only possibility to cancel a maneuver
* before the time is up.
*/
void abortManeuver();
uint8_t getRemainingManeuverTime() const;
uint8_t getRemainingManeuverTime() const;
/**
* @brief Returns true if a maneuver is running
*
* @return true
* @return false
*/
bool getBusy() const { return this->busy; }
Maneuver getManeuver() const { return this->maneuver; }
Speedometer* getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); }
Speedometer* getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); }
/**
* @brief Returns true if a maneuver is running
*
* @return true
* @return false
*/
bool getBusy() const { return this->busy; }
Maneuver getManeuver() const { return this->maneuver; }
Speedometer *getSpeedometerLeft() const { return this->moveControl->getSpeedometerLeft(); }
Speedometer *getSpeedometerRight() const { return this->moveControl->getSpeedometerRight(); }
private:
void run() override;
bool engineInit(int16_t powerPercentage, int16_t seconds);
private:
void run() override;
bool engineInit(int16_t powerPercentage, int16_t seconds);
Maneuver maneuver = Maneuver::None;
int16_t maneuverValueOne = 0;
int16_t maneuverValueTwo = 0;
Maneuver maneuver = Maneuver::None;
int16_t maneuverValueOne = 0;
int16_t maneuverValueTwo = 0;
bool busy = false;
bool abort = false;
uint16_t azimuth;
int16_t degree;
uint32_t maneuverTime = 0;
uint32_t actionStart = 0;
bool busy = false;
bool abort = false;
uint16_t azimuth;
int16_t degree;
uint32_t maneuverTime = 0;
uint32_t actionStart = 0;
static constexpr int8_t maxPercentage = 100;
};
#endif // TEST_MODE_H
src/driveModi/driveManager.cpp
+17 -15
View File
@@ -4,44 +4,46 @@
* @brief Implementation of the class DriveManager
* @version 0.1
* @date 2021-12-14
*
*
* @copyright Copyright (c) 2021
*
*
*/
#include "driveModi/driveManager.h"
DriveManager::DriveManager(MoveControl *moveControl, const SensorData* sensorData, const ControlPadInput *input) {
this->driveModiParams.input = input;
this->driveModiParams.moveControl = moveControl;
this->driveModiParams.sensorData = sensorData;
DriveManager::DriveManager(MoveControl *moveControl, const SensorData *sensorData, const ControlPadInput *input)
: driveModiParams{moveControl, input, sensorData}
{
this->init();
}
DriveManager::DriveManager(DriveModiParams params){
this->driveModiParams = params;
DriveManager::DriveManager(DriveModiParams params)
: driveModiParams{params}
{
this->init();
}
void DriveManager::changeModus(DriveModi* modus) {
if (this->currentModusPtr) {
void DriveManager::changeModus(DriveModi *modus)
{
if (static_cast<bool>(this->currentModusPtr))
{
this->removeChildComponent(this->currentModusPtr);
delete this->currentModusPtr;
this->currentModusPtr = nullptr;
}
this->currentModusPtr = modus;
if (this->currentModusPtr) {
if (static_cast<bool>(this->currentModusPtr))
{
this->currentModusPtr->activate(this->driveModiParams);
this->currentModusPtr->setSpeeds(this->drivingSpeeds);
this->addChildComponent(this->currentModusPtr);
}
}
void DriveManager::init() {
void DriveManager::init()
{
this->addChildComponent(this->driveModiParams.moveControl);
this->activateOnlyChilds();
}
src/driveModi/driveModi.cpp
+1 -5
View File
@@ -1,9 +1,5 @@
#include "driveModi.h"
DriveModi::DriveModi() {
this->loopDelay = 0;
}
DriveModi::~DriveModi() {
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
@@ -26,6 +22,6 @@ void DriveModi::activate(DriveModiParams params){
void DriveModi::init() {
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
this->loopDelay = 40;
this->loopDelay = DriveModi::defaultDelay;
this->afterActivate();
}
src/driveModi/driveModi.h
+6 -5
View File
@@ -31,7 +31,6 @@ struct DriveModiParams {
*/
class DriveModi : public Component {
public:
DriveModi();
virtual ~DriveModi();
const SensorData* getSensorData() const { return this->sensorData; }
@@ -46,12 +45,14 @@ class DriveModi : public Component {
protected:
virtual void afterActivate() {}
MoveControl *moveControl;
MoveControl *moveControl = nullptr;
DrivingSpeeds maxSpeeds = {1, 7};
const ControlPadInput* input;
const SensorData* sensorData;
const ControlPadInput* input = nullptr;
const SensorData* sensorData = nullptr;
private:
void init();
void init();
static constexpr uint8_t defaultDelay = 40;
};
#endif // DRIVEMODI_H
src/main.cpp
+157 -99
View File
@@ -2,15 +2,15 @@
* @file main.cpp
* @author Alexander Klein (alex@kleiax.de)
* @brief The main file.
*
*
* Sets up Network stuff, PS3-Controller, Menu and Lcd
* Handles Controller Input
*
*
* @version 0.1
* @date 2022-02-15
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include <Arduino.h>
@@ -50,61 +50,71 @@
#include "SpecialMenus/CalibrateBattery/menuCalibrateBattery.h"
MoveControl moveController;
Network* network;
DriveManager* driveManager;
Menu* main_m;
LiquidCrystal_I2C* lcd;
LcdWrapper* lcdWrapper;
OutputBuf* outputBuf;
DebugMqtt* debugMqtt = nullptr;
Battery* mainBattery;
SPIClass* spiPort;
SensorData* sensorData;
ControlPad* controlPad;
Network *network;
DriveManager *driveManager;
Menu *main_m;
LiquidCrystal_I2C *lcd;
LcdWrapper *lcdWrapper;
OutputBuf *outputBuf;
DebugMqtt *debugMqtt = nullptr;
Battery *mainBattery;
SPIClass *spiPort;
SensorData *sensorData;
ControlPad *controlPad;
bool wifiIsActive;
constexpr uint16_t displayUpdateDelay = 500;
void i2cScanner(void);
void makeMenu(void);
void restart(void);
void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len);
void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status);
void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
NetworkAdresses setIPs(void);
void i2cScanner();
void makeMenu();
void restart();
void receiveCallback(const uint8_t *mac, const uint8_t *incomingData, int len);
void sendCallback(const uint8_t *mac_addr, esp_now_send_status_t status);
void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows);
NetworkAdresses setIPs();
void setup() {
Serial.begin(115200);
void setup()
{
Serial.begin(Settings::baudRate);
// DebugTimes::setConsolOutput(true);
DebugTimes setupTime;
spiPort = new SPIClass(HSPI);
spiPort->begin(PinNumbers::spiSck, PinNumbers::spiCipo, PinNumbers::spiCopi, PinNumbers::gnssSpiCs);
mainBattery = new Battery(35);
mainBattery = new Battery(PinNumbers::battery);
controlPad = new ControlPad();
Wire.begin(21, 19);
Wire.setClock(400000);
Wire.begin(PinNumbers::sda, PinNumbers::scl);
Wire.setClock(Settings::i2cSpeed);
i2cScanner();
lcd = new LiquidCrystal_I2C(0x3F,16,2);
lcd = new LiquidCrystal_I2C(0x3F, 16, 2);
lcd->init();
lcd->clear();
lcd->noBacklight();
network = new Network(NetworkConfig::ssid, NetworkConfig::password, setIPs());
network = new Network(static_cast<const char*>(NetworkConfig::ssid),
static_cast<const char*>(NetworkConfig::password),
setIPs());
network->activateEspNow(receiveCallback, sendCallback);
if (NetworkConfig::mqtt) {
if (network->activateMqtt(MqttConfig::user, MqttConfig::password)) {
if (NetworkConfig::mqtt)
{
if (network->activateMqtt(static_cast<const char*>(MqttConfig::user),
static_cast<const char*>(MqttConfig::password)))
{
DebugMqtt::init(network->getMqttClient(), Loglevel::debug);
debugMqtt = new DebugMqtt("Console");
outputBuf = new OutputBuf(debugMqtt);
outputBuf->activateMqtt(true);
} else {
}
else
{
outputBuf = new OutputBuf();
}
} else
}
else
{
outputBuf = new OutputBuf();
}
std::cout.rdbuf(outputBuf);
std::cout << "Welcome to Kleiax-Rover" << std::endl;
@@ -115,13 +125,19 @@ void setup() {
sensorData = new SensorData();
sensorData->enableGnss(spiPort, PinNumbers::gnssSpiCs);
sensorData->enableRealCompass();
sensorData->enableNtrip(NtripConfig::host, NtripConfig::port, NtripConfig::mountPoint, NtripConfig::user, NtripConfig::password);
sensorData->enableNtrip(static_cast<const char*>(NtripConfig::host),
NtripConfig::port,
static_cast<const char*>(NtripConfig::mountPoint),
static_cast<const char*>(NtripConfig::user),
static_cast<const char*>(NtripConfig::password));
// sensorData->enableGyroskop();
driveManager = new DriveManager(&moveController, sensorData, controlPad->getControlPadDataPtr());
char wifiIndicator = 'X';
if (network->isWifiConnected())
{
wifiIndicator = '-';
}
lcd->backlight();
lcd->printf("%c Kleiax-Rover %c", wifiIndicator, wifiIndicator);
lcd->setCursor(0, 1);
@@ -134,7 +150,8 @@ void setup() {
setupTime.stopConsol("Setup");
}
void loop() {
void loop()
{
network->loop();
sensorData->loop();
driveManager->loop();
@@ -143,86 +160,113 @@ void loop() {
lcdWrapper->loop();
mainBattery->loop();
// new Value ervery 0.5s
if (mainBattery->isNewValue()) {
// new Value ervery 0.5s
if (mainBattery->isNewValue())
{
static uint8_t batteryLowCounter = 0;
if (mainBattery->isBatteryLow(10))
static constexpr uint8_t minVoltage = 10;
if (mainBattery->isBatteryLow(minVoltage))
{
batteryLowCounter++;
}
else
{
batteryLowCounter = 0;
}
if (batteryLowCounter >= 10) {
if (batteryLowCounter >= 10)
{
moveController.emergencyStop();
uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100);
const auto voltage = static_cast<uint16_t>(mainBattery->getBatteryVoltage() * 100);
lcd->setCursor(0, 0);
lcd->printf("Low Battery: %u", voltage);
lcd->setCursor(0, 1);
lcd->print("Please turn off.");
while (true);
while (true)
{
}
}
}
}
void i2cScanner() {
void i2cScanner()
{
constexpr uint8_t checkForLength = 16;
constexpr uint8_t maxAdresses = UINT8_MAX / 2;
std::cout << "\nI2C Scanner" << std::endl;
byte error, address;
int nDevices;
byte error = 0;
byte address = 0;
int nDevices = 0;
std::cout << "Scanning..." << std::endl;
nDevices = 0;
for(address = 1; address < 127; address++ ) {
for (address = 1; address < maxAdresses; address++)
{
Wire.beginTransmission(address);
error = Wire.endTransmission();
if (error == 0) {
if (error == 0)
{
std::cout << "I2C device found at address 0x";
if (address<16)
if (address < checkForLength)
{
std::cout << "0";
std::cout << std::hex << (int) address << std::dec << std::endl;
}
std::cout << std::hex << static_cast<int>(address) << std::dec << std::endl;
// std::cout << (int) address << std::endl;
nDevices++;
}
else if (error==4) {
else if (error == 4)
{
std::cout << "Unknow error at address 0x";
if (address<16)
if (address < checkForLength)
{
std::cout << "0";
std::cout << std::hex << (int) address << std::dec << std::endl;
}
}
std::cout << std::hex << static_cast<int>(address) << std::dec << std::endl;
}
}
if (nDevices == 0)
std::cout << "No I2C devices found\n" << std::endl;
{
std::cout << "No I2C devices found\n"
<< std::endl;
}
else
std::cout << "done\n" << std::endl;
{
std::cout << "done\n"
<< std::endl;
}
}
void makeMenu() {
auto dummy = []() {
std::cout << "Dummy in Action" <<std::endl;
void makeMenu()
{
auto dummy = []()
{
std::cout << "Dummy in Action" << std::endl;
};
// Create Menu
main_m = new Menu();
main_m->setLcd(lcdWrapper);
auto mode_m = new Menu();
auto set_m = new Menu();
auto pid_m = new Menu();
auto pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
auto pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
auto speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef()));
auto man_m = new MenuManualControl(driveManager);
auto cap_m = new MenuCaptureRoute(driveManager);
auto auto_m = new MenuAutopilot(driveManager);
auto testM_m = new MenuTestMode(driveManager);
auto comp_m = new MenuCalibrateCompass(driveManager);
auto sys_m = new MenuSysteminformation(mainBattery);
auto sen_m = new MenuSensorData(sensorData);
//TODO: Wie bekommt jeder die dumme Route?
auto rout_m = new MenuRoute(new Route());
auto bat_m = new MenuCalibrateBattery(mainBattery);
auto *mode_m = new Menu();
auto *set_m = new Menu();
auto *pid_m = new Menu();
auto *pidl_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(0)));
auto *pidr_m = new MenuIntInput(3, new MenuPidSettings(moveController.getPID(1)));
auto *speed_m = new MenuIntInput(2, new MenuSpeed(driveManager->getDrivingSpeedsRef()));
auto *man_m = new MenuManualControl(driveManager);
auto *cap_m = new MenuCaptureRoute(driveManager);
auto *auto_m = new MenuAutopilot(driveManager);
auto *testM_m = new MenuTestMode(driveManager);
auto *comp_m = new MenuCalibrateCompass(driveManager);
auto *sys_m = new MenuSysteminformation(mainBattery);
auto *sen_m = new MenuSensorData(sensorData);
// TODO: Wie bekommt jeder die dumme Route?
auto *rout_m = new MenuRoute(new Route());
auto *bat_m = new MenuCalibrateBattery(mainBattery);
auto_m->setUpdateDelay(1000);
sys_m->setUpdateDelay(1500);
sen_m->setUpdateDelay(500);
auto_m->setUpdateDelay(displayUpdateDelay);
sys_m->setUpdateDelay(displayUpdateDelay);
sen_m->setUpdateDelay(displayUpdateDelay);
// Entry for the main menu
main_m->addEntry(new MenuAction("Mode", mode_m));
@@ -253,14 +297,14 @@ void makeMenu() {
// Other config
pidl_m->setMinMax(0, UINT8_MAX);
pidl_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(0)->GetKp());
pidl_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(0)->GetKi());
pidl_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(0)->GetKd());
pidl_m->setEntry(0, "P-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKp()));
pidl_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKi()));
pidl_m->setEntry(2, "D-Part", static_cast<uint8_t>(moveController.getPID(0)->GetKd()));
pidr_m->setMinMax(0, UINT8_MAX);
pidr_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(1)->GetKp());
pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
pidr_m->setEntry(0, "P-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKp()));
pidr_m->setEntry(1, "I-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKi()));
pidr_m->setEntry(2, "D-Part", static_cast<uint8_t>(moveController.getPID(1)->GetKd()));
speed_m->setMinMax(5, UINT8_MAX);
speed_m->setEntry(0, "x m/s e-1", driveManager->getDrivingSpeedsRef().x * 10);
@@ -271,42 +315,56 @@ void makeMenu() {
controlPad->setMenuControl(main_m);
}
void restart() {
void restart()
{
lcdWrapper->clear();
lcdWrapper->setCursor(0, 0);
lcdWrapper->print("Rebooting ...");
ESP.restart();
}
void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len) {
void receiveCallback(const uint8_t *mac, const uint8_t *incomingData, int len)
{
if (len != sizeof(ControlPadInput))
{
return;
}
controlPad->insertData(incomingData);
}
void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) {
void sendCallback(const uint8_t *mac_addr, esp_now_send_status_t status)
{
if (status != ESP_NOW_SEND_SUCCESS)
{
std::cout << "sendCallback - Delivery Fail" << std::endl;
}
}
void lcdWrapperCallback (const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows) {
void lcdWrapperCallback(const char data[][LcdWrapper::totalRows], uint8_t lines, uint8_t rows)
{
if (!controlPad->isControlPadConnected())
{
return;
}
const esp_err_t result = esp_now_send(network->getBroadcastAddress(), reinterpret_cast<const uint8_t *>(data), lines * rows);
esp_err_t result = esp_now_send(network->getBroadcastAddress(), (uint8_t *) data, lines * rows);
if (result != ESP_OK)
{
Serial.println("Error sending the data");
}
}
NetworkAdresses setIPs() {
NetworkAdresses setIPs()
{
NetworkAdresses adresses;
adresses.localIP.fromString(NetworkConfig::ip);
adresses.subnet.fromString(NetworkConfig::subnet);
adresses.gateway.fromString(NetworkConfig::gateway);
adresses.dnsServer.fromString(NetworkConfig::dns);
if (NetworkConfig::mqtt) {
adresses.mqttServer.fromString(MqttConfig::server);
adresses.localIP.fromString(static_cast<const char *>(NetworkConfig::ip));
adresses.subnet.fromString(static_cast<const char *>(NetworkConfig::subnet));
adresses.gateway.fromString(static_cast<const char *>(NetworkConfig::gateway));
adresses.dnsServer.fromString(static_cast<const char *>(NetworkConfig::dns));
if (NetworkConfig::mqtt)
{
adresses.mqttServer.fromString(static_cast<const char *>(MqttConfig::server));
adresses.mqttPort = MqttConfig::port;
}
src/moveControl.cpp
+111 -77
View File
@@ -4,41 +4,40 @@
* @brief Contains an implementation of the class MoveControl
* @version 0.1
* @date 2022-02-15
*
*
* @copyright Copyright (c) 2022
*
*
*/
#include "moveControl.h"
MoveControl::MoveControl() {
this->loopDelay = 20;
this->left_motor = new MotorControl();
this->right_motor = new MotorControl();
this->left_speedometer = new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
this->right_speedometer = new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps);
this->left_pid = new PID( &this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
Settings::Pid::Left::P,
Settings::Pid::Left::I,
Settings::Pid::Left::D,
DIRECT);
this->right_pid = new PID( &this->wheelspeed_right,
&this->right_pid_out,
&this->wheelspeed_right_target,
Settings::Pid::Right::P,
Settings::Pid::Right::I,
Settings::Pid::Right::D,
DIRECT);
MoveControl::MoveControl()
: Component(MoveControl::loopDelay),
left_motor{new MotorControl()},
right_motor{new MotorControl()},
left_speedometer{new Speedometer(PinNumbers::LeftMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
right_speedometer{new Speedometer(PinNumbers::RightMotor::encoder, Settings::wheelDiameter, Settings::encoderSteps)},
left_pid{new PID(&this->wheelspeed_left,
&this->left_pid_out,
&this->wheelspeed_left_target,
Settings::Pid::Left::P,
Settings::Pid::Left::I,
Settings::Pid::Left::D,
DIRECT)},
right_pid{new PID(&this->wheelspeed_right,
&this->right_pid_out,
&this->wheelspeed_right_target,
Settings::Pid::Right::P,
Settings::Pid::Right::I,
Settings::Pid::Right::D,
DIRECT)}
{
this->left_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->left_pid->SetSampleTime(Settings::Pid::sampleTime);
this->left_pid->SetMode(AUTOMATIC);
this->right_pid->SetOutputLimits(Settings::Pid::outMin, Settings::Pid::outMax);
this->right_pid->SetSampleTime(Settings::Pid::sampleTime);
this->right_pid->SetMode(AUTOMATIC);
this->right_pid->SetMode(AUTOMATIC);
this->left_motor->init(PinNumbers::LeftMotor::pwm, PinNumbers::LeftMotor::pmwChannel, PinNumbers::LeftMotor::dir1, PinNumbers::LeftMotor::dir2);
this->right_motor->init(PinNumbers::RightMotor::pwm, PinNumbers::RightMotor::pmwChannel, PinNumbers::RightMotor::dir1, PinNumbers::RightMotor::dir2);
@@ -46,10 +45,11 @@ MoveControl::MoveControl() {
this->addChildComponent(this->left_motor);
this->addChildComponent(this->right_motor);
this->addChildComponent(this->left_speedometer);
this->addChildComponent(this->right_speedometer);
this->addChildComponent(this->right_speedometer);
}
MoveControl::~MoveControl() {
MoveControl::~MoveControl()
{
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
@@ -59,10 +59,10 @@ MoveControl::~MoveControl() {
delete this->right_speedometer;
delete this->left_pid;
delete this->right_pid;
}
void MoveControl::run() {
void MoveControl::run()
{
this->updateCurrentWheelSpeed();
this->calcTargetWheelSpeed();
@@ -72,7 +72,8 @@ void MoveControl::run() {
this->regulateMotors();
}
void MoveControl::setDrivingStatus(Status status) {
void MoveControl::setDrivingStatus(Status status)
{
this->setSpeed(0);
this->setRotationSpeed(0);
this->setRawPowerLeft(0);
@@ -97,7 +98,8 @@ void MoveControl::setDrivingStatus(Status status) {
// }
}
void MoveControl::emergencyStop() {
void MoveControl::emergencyStop()
{
this->left_motor->emergencyStop();
this->right_motor->emergencyStop();
this->setSpeed(0);
@@ -107,8 +109,10 @@ void MoveControl::emergencyStop() {
this->driving_status = Status::Stop;
}
void MoveControl::setSpeed(double speed) {
if (speed < 0.2 && speed > -0.2) {
void MoveControl::setSpeed(double speed)
{
if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
{
this->drivingSpeeds.x = 0;
return;
}
@@ -116,8 +120,10 @@ void MoveControl::setSpeed(double speed) {
this->drivingSpeeds.x = speed;
}
void MoveControl::setRotationSpeed(double speed) {
if (speed < 0.1 && speed > -0.1){
void MoveControl::setRotationSpeed(double speed)
{
if (speed < MoveControl::minSpeed && speed > -MoveControl::minSpeed)
{
this->drivingSpeeds.rot = 0;
return;
}
@@ -125,92 +131,120 @@ void MoveControl::setRotationSpeed(double speed) {
this->drivingSpeeds.rot = speed;
}
void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds) {
void MoveControl::setSpeeds(DrivingSpeeds drivingSpeeds)
{
this->setSpeed(drivingSpeeds.x);
this->setRotationSpeed(drivingSpeeds.rot);
}
void MoveControl::setRawPowerLeft(int16_t power) {
if (power <= 100 && power >= -100)
this->rawPowerLeft = power;
void MoveControl::setRawPowerLeft(int16_t power)
{
if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage)
{
this->rawPowerLeft = static_cast<int8_t>(power);
}
}
void MoveControl::setRawPowerRight(int16_t power) {
if (power <= 100 && power >= -100)
this->rawPowerRight = power;
void MoveControl::setRawPowerRight(int16_t power)
{
if (power <= MoveControl::maxPercentage && power >= -MoveControl::maxPercentage)
{
this->rawPowerRight = static_cast<int8_t>(power);
}
}
void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
PID* selectedPID = nullptr;
void MoveControl::setPidTunings(uint8_t side, double pPart, double iPart, double dPart)
{
PID *selectedPID = nullptr;
if (side == 0)
{
selectedPID = this->left_pid;
}
else if (side == 1)
{
selectedPID = this->right_pid;
}
selectedPID->SetTunings(p, i, d);
selectedPID->SetTunings(pPart, iPart, dPart);
}
PID* MoveControl::getPID(uint8_t side) const {
PID *MoveControl::getPID(uint8_t side) const
{
if (side == 0)
{
return this->left_pid;
else if (side == 1)
}
if (side == 1)
{
return this->right_pid;
}
return nullptr;
}
void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value)
{
if (value > 0)
{
speedometer->setDirection(Speedometer::Direction::Forward);
}
else if (value < 0)
{
speedometer->setDirection(Speedometer::Direction::Backward);
}
else
{
speedometer->setDirection(Speedometer::Direction::None);
}
}
void MoveControl::calcTargetWheelSpeed() {
void MoveControl::calcTargetWheelSpeed()
{
/* original formula:
(1 / r) / 1 b \ / x \ = / Xl \
\ 1 -b / \ T / \ Xr / */
// (1 / r) * 1
constexpr double A = 1.0 / (Settings::wheelDiameter / 2);
constexpr double A1r1 = 1.0 / (Settings::wheelDiameter / 2);
// (1 / r) * b
constexpr double B = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);
constexpr double B1rb = (1.0 / (Settings::wheelDiameter / 2)) * (Settings::wheelDistance / 2);
this->wheelspeed_right_target = (A * this->drivingSpeeds.x + B * this->drivingSpeeds.rot);
this->wheelspeed_left_target = (A * this->drivingSpeeds.x + (-B) * this->drivingSpeeds.rot);
this->wheelspeed_right_target = (A1r1 * this->drivingSpeeds.x + B1rb * this->drivingSpeeds.rot);
this->wheelspeed_left_target = (A1r1 * this->drivingSpeeds.x + (-B1rb) * this->drivingSpeeds.rot);
}
void MoveControl::regulateMotors() {
switch (this->driving_status) {
case Status::Stop :
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
this->setSpeedometerDirection(this->left_speedometer, 0);
this->setSpeedometerDirection(this->right_speedometer, 0);
break;
void MoveControl::regulateMotors()
{
switch (this->driving_status)
{
case Status::Stop:
this->left_motor->setTargetPower(0);
this->right_motor->setTargetPower(0);
this->setSpeedometerDirection(this->left_speedometer, 0);
this->setSpeedometerDirection(this->right_speedometer, 0);
break;
case Status::Drive :
this->left_motor->setTargetPower( (int8_t) this->left_pid_out);
this->right_motor->setTargetPower( (int8_t) this->right_pid_out);
this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
break;
case Status::Drive:
this->left_motor->setTargetPower(static_cast<int8_t>(this->left_pid_out));
this->right_motor->setTargetPower(static_cast<int8_t>(this->right_pid_out));
this->setSpeedometerDirection(this->left_speedometer, this->left_motor->getPower());
this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
break;
case Status::Raw :
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
break;
case Status::Raw:
this->left_motor->setTargetPower(this->rawPowerLeft);
this->right_motor->setTargetPower(this->rawPowerRight);
this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
default:
Serial.println("Wrong drivingState in MoveControl::regulateMotors");
break;
}
}
void MoveControl::updateCurrentWheelSpeed() {
void MoveControl::updateCurrentWheelSpeed()
{
this->wheelspeed_left = this->left_speedometer->getSpeedRad();
this->wheelspeed_right = this->right_speedometer->getSpeedRad();
}