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

cleaning

Browse Source 
refactore
all components now have a base class compnent for loop functions
This commit is contained in:
kleiax
2023-08-18 10:47:35 +02:00
parent 25d37e3970
commit 11d21e2e89
35 changed files with 303 additions and 511 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/SpecialMenus/driveModi/Autopilot/menuAutopilot.cpp
+1
View File
@@ -164,6 +164,7 @@ void MenuAutopilot::printPage() const {
}
} else {
lineOne = "Real Azi: ";
lineTwo = "";
lineOne.concat(this->driveManager->getNavigation()->getAzimuth());
}
break;
src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp
+2 -3
View File
@@ -77,9 +77,8 @@ void MenuTestMode::init() {
this->driveManager->changeModus(Modi::TestMode);
this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
testMode->setSpeed(50);
testMode->setRotationSpeed(100);
testMode->setMaxSpeed(1);
testMode->setMaxRotation(8);
auto dummy = []() {
std::cout << "Dummy in Action" <<std::endl;
src/driveModi/Modi/Autopilot/autopilot.cpp
+7 -18
View File
@@ -40,24 +40,9 @@ Autopilot::~Autopilot() {
// this->navigation->getNTRIPClient()->setActivated(false);
}
void Autopilot::loop() {
if (this->state < State::SelfDriving)
ManualControl::loop();
if (millis() - this->displayUpdateLastMillis > this->displayUpdateDelayMillis) {
this->updateDisplay = true;
this->displayUpdateLastMillis = millis();
}
if (millis() - this->loopLastMillis < loopDelayMillis)
return;
void Autopilot::run() {
this->routeInfo = this->navigation->getRouteInfo();
this->runAutopilot();
this->loopLastMillis = millis();
}
void Autopilot::runAutopilot() {
switch (this->state) {
case State::InsufficientAccuracy:
this->askNavigationForOrder();
@@ -75,12 +60,14 @@ void Autopilot::runAutopilot() {
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;
@@ -140,8 +127,8 @@ void Autopilot::init() {
this->courseCorrection.distance = 0;
this->updateDisplay = true;
this->maxRotationSpeed = 4.5;
this->maxForwardSpeed = 1;
this->maxRotationSpeed = 7;
this->maxForwardSpeed = 1.5;
}
void Autopilot::drive() {
@@ -221,6 +208,8 @@ void Autopilot::askNavigationForOrder() {
break;
case Navigation::Status::InsufficientAccuracy:
if (this->state == State::InsufficientAccuracy)
break;
this->lastState = this->state;
this->state = State::InsufficientAccuracy;
this->moveControl->setSpeed(0);
src/driveModi/Modi/Autopilot/autopilot.h
+1 -5
View File
@@ -117,7 +117,7 @@ class Autopilot : public ManualControl {
void drive();
void beginRotate();
void rotate();
void runAutopilot();
void run() override;
void checkButtonInput();
void askNavigationForOrder();
void selfDriving();
@@ -134,12 +134,8 @@ class Autopilot : public ManualControl {
bool updateDisplay = false;
bool loopMode = false;
uint8_t loopDelayMillis = 40;
uint8_t maxCourseDeviationBeforeAct = 5;
uint16_t displayUpdateDelayMillis = 1000;
uint16_t autopilotChangeDelayMillis = 500;
uint32_t displayUpdateLastMillis = 0;
uint32_t loopLastMillis = 0;
uint32_t lastAutopilotChangeMillis = 0;
int16_t rotationAimAzimuth;
src/driveModi/Modi/CaptureRoute/captureRoute.cpp
+2 -11
View File
@@ -23,17 +23,8 @@ CaptureRoute::~CaptureRoute() {
// this->navigation->getNTRIPClient()->setActivated(false);
}
void CaptureRoute::loop() {
ManualControl::loop();
if (millis() - this->lastMillis < this->delay)
return;
this->runCaptureRoute();
this->lastMillis = millis();
}
void CaptureRoute::runCaptureRoute() {
void CaptureRoute::run() {
ManualControl::run();
if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
this->status = this->navigation->addCurrentPosToRoute();
if (this->status == Navigation::Status::Updated) {
src/driveModi/Modi/CaptureRoute/captureRoute.h
+2 -18
View File
@@ -42,21 +42,6 @@ class CaptureRoute : public ManualControl {
*/
~CaptureRoute();
/**
* @brief Calls runCaptureRoute and ManualControl::loop
*
* Calls everytime the other loop but only calls runCaptureRoute
* if the delay is reached.
*
*/
void loop();
/**
* @brief Checks if a Point should be added to the Route
*
*/
void runCaptureRoute();
// /**
// * @brief Get the Navigation object
// *
@@ -87,14 +72,13 @@ class CaptureRoute : public ManualControl {
*/
bool shouldUpdate();
private:
void run() override;
Navigation* navigation;
RouteInfo routeInfo;
Point lastSavedPoint;
Navigation::Status status = Navigation::Status::Complete;
uint16_t delay = 200;
uint32_t lastMillis = 0;
bool updateDisplay = false;
};
src/driveModi/Modi/ManualControl/manualControl.cpp
+22 -21
View File
@@ -10,25 +10,16 @@
*/
#include "manualControl.h"
ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) {
this->moveControl = moveControl;
ManualControl::ManualControl(MoveControl* moveControl, const ControlPadInput *input)
: DriveModi(moveControl) {
this->input = input;
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
}
ManualControl::~ManualControl() {
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
}
void ManualControl::loop() {
if (this->caliCompass)
this->caliCompass->loop();
if (millis() - this->lastMillis < delay)
return;
void ManualControl::run() {
switch (this->inputMode) {
case InputMode::Analog :
this->analogControl();
@@ -41,8 +32,16 @@ void ManualControl::loop() {
default:
break;
}
}
this->lastMillis = millis();
void ManualControl::setCalibrateCompass(CalibrateCompass *caliCompass) {
if (caliCompass) {
this->caliCompass = caliCompass;
this->addChildComponent(this->caliCompass);
} else if (this->caliCompass) {
this->removeChildComponent(this->caliCompass);
this->caliCompass = caliCompass;
}
}
void ManualControl::switchInputMode() {
@@ -51,19 +50,21 @@ void ManualControl::switchInputMode() {
void ManualControl::analogControl() {
// Have to be int16_t to avoid overflow (int8_t = 255 - 127 = -128)
int16_t y = this->input->x - 127;
int16_t x = this->input->y - 127;
int16_t x = this->input->x - 127;
int16_t y = this->input->y - 127;
// if (x || y) {
// std::cout << "x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
// std::cout << "x: " << (int) x << " y: " << (int) y << std::endl;
// static int counter = 0;
// if (counter % 60 == 0) {
// std::cout << "Input: x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
// std::cout << "Output: x: " << (int) x << " y: " << (int) y << std::endl;
// }
// counter++;
double value_per_step = this->maxForwardSpeed * 2 / UINT8_MAX;
this->moveControl->setSpeed(-y * value_per_step);
this->moveControl->setSpeed(-x * value_per_step);
value_per_step = this->maxRotationSpeed * 2 / UINT8_MAX;
this->moveControl->setRotationSpeed(x * value_per_step);
this->moveControl->setRotationSpeed(y * value_per_step);
}
void ManualControl::digitalControl() {
src/driveModi/Modi/ManualControl/manualControl.h
+2 -52
View File
@@ -37,51 +37,9 @@ class ManualControl : public DriveModi {
};
ManualControl(MoveControl *moveControl, const ControlPadInput *input);
/**
* @brief Destroy the Manual Control object
* Set in moveControl speed and rotation to 0 and set DrivingStatus::Stop
*/
~ManualControl();
/**
* @brief Calls analogControl() to update all values.
*
* This function should be called every mainloop. If the delay is not reached, than the
* functions returns immediately.
* @see runSpeedometer()
* @see setDelay()
*/
void loop() override;
/**
* @brief Set the min delay between each loop
*
* @param delay_ time in Milliseconds
*/
void setDelay(uint8_t delay_) { delay = delay_; }
/**
* @brief Set the max speed
*
* @param maxSpeed in m/s
*/
void setMaxSpeed(double maxForwardSpeed) { maxForwardSpeed = maxForwardSpeed; }
void increaseMaxSpeed(double increase = 0.1) { maxForwardSpeed += increase; }
void decreaseMaxSpeed(double increase = 0.1) { maxForwardSpeed -= increase; }
double getMaxSpeed() { return this->maxForwardSpeed; }
/**
* @brief Set the max rotation
*
* @param maxRotation in rad/s (maybe)
*/
void setMaxRotation(double maxRotation) { maxRotationSpeed = maxRotation; }
void increaseMaxRotation(double increase = 0.1) { maxRotationSpeed += increase; }
void decreaseMaxRotation(double increase = 0.1) { maxRotationSpeed -= increase; }
double getMaxRotation() { return this->maxRotationSpeed; }
void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }
void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr);
void switchInputMode();
void setInputMode(InputMode mode) { this->inputMode = mode; }
@@ -92,22 +50,14 @@ class ManualControl : public DriveModi {
uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
protected:
MoveControl *moveControl;
void run() override;
const ControlPadInput* input;
double maxForwardSpeed = 1;
double maxRotationSpeed = 7;
private:
/**
* @brief Noramly called repeatedly by loop() to calcluate new values.
* Set new values for speed and rotation in moveControl
*/
void analogControl();
void digitalControl();
bool lastLoopTurned = false;
uint8_t delay = 10;
uint32_t lastMillis = 0;
CalibrateCompass* caliCompass = nullptr;
DirectionChangeWrapper* directionChangeWrapper = nullptr;
InputMode inputMode = InputMode::Analog;
src/driveModi/Modi/TestMode/testMode.cpp
+11 -24
View File
@@ -10,8 +10,8 @@
*/
#include "testMode.h"
TestMode::TestMode(MoveControl *moveControl, Navigation* navigation) {
this->moveControl = moveControl;
TestMode::TestMode(MoveControl *moveControl, Navigation* navigation)
: DriveModi(moveControl) {
this->navigation = navigation;
}
@@ -19,10 +19,7 @@ TestMode::~TestMode() {
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
}
void TestMode::loop() {
if (millis() - this->lastMillis < this->delay)
return;
void TestMode::run() {
if (this->maneuver == Maneuver::Turn) {
uint16_t delta = abs(this->azimuth - this->navigation->getAzimuth());
if (delta > this->degree)
@@ -35,16 +32,6 @@ void TestMode::loop() {
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
this->maneuver = Maneuver::None;
}
this->lastMillis = millis();
}
void TestMode::setSpeed(int16_t speed) {
this->speed = (double) speed / 100.0;
}
void TestMode::setRotationSpeed(int16_t speed) {
this->rotationSpeed = (double) speed / 100.0;
}
bool TestMode::drive(int16_t cm, int16_t degree) {
@@ -59,9 +46,9 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
this->moveControl->setSpeed(0);
if (degree < 0)
this->moveControl->setRotationSpeed(-this->rotationSpeed);
this->moveControl->setRotationSpeed(-this->maxRotationSpeed);
else if (degree > 0)
this->moveControl->setRotationSpeed(this->rotationSpeed);
this->moveControl->setRotationSpeed(this->maxRotationSpeed);
this->azimuth = this->navigation->getAzimuth();
this->degree = degree;
@@ -75,11 +62,11 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
this->moveControl->setRotationSpeed(0);
if (cm < 0)
this->moveControl->setSpeed(-this->speed);
this->moveControl->setSpeed(-this->maxForwardSpeed);
else if (cm > 0)
this->moveControl->setSpeed(this->speed);
this->moveControl->setSpeed(this->maxForwardSpeed);
this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->speed) * 1000;
this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->maxForwardSpeed) * 1000;
this->busy = true;
this->maneuver = Maneuver::Drive;
return true;
@@ -87,11 +74,11 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
//forward or backward and left or right
// TODO: Calculate roationspeed
if (cm < 0)
this->moveControl->setSpeed(-this->speed);
this->moveControl->setSpeed(-this->maxForwardSpeed);
else if (cm > 0)
this->moveControl->setSpeed(this->speed);
this->moveControl->setSpeed(this->maxForwardSpeed);
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->maxForwardSpeed) * 1000;
this->busy = true;
this->maneuver = Maneuver::Drive;
return true;
src/driveModi/Modi/TestMode/testMode.h
+1 -30
View File
@@ -47,28 +47,6 @@ class TestMode : public DriveModi {
TestMode(MoveControl *moveControl, Navigation* navigation);
~TestMode();
/**
* @brief Controls the maneuver.
*
* Needed for actions which take a longer time.
* Should be called ever main loop.
*/
void loop() override;
/**
* @brief Set the speed for the Maneuver Drive
*
* @param speed m/s / 100
*/
void setSpeed(int16_t speed);
/**
* @brief Set the rotation speed for the Maneuver Drive
*
* @param speed rad/s / 100
*/
void setRotationSpeed(int16_t speed);
bool drive(int16_t cm = 0, int16_t degree = 0);
/**
@@ -125,28 +103,21 @@ class TestMode : public DriveModi {
Speedometer* getSpeedometerRight() { return this->moveControl->getSpeedometerRight(); }
private:
void run() override;
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;
bool busy = false;
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
+12 -20
View File
@@ -23,8 +23,10 @@ DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const Co
this->navigation = new Navigation(spiPort, UBLOX_GNSS_SPI_CS);
if (wifi)
this->navigation->initNtrip(NTRIP_HOST, NTRIP_PORT, NTRIP_MOUNT_POINT, NTRIP_USER, NTRIP_PASSWORD);
// if (wifi)
// std::cout << "Wifi is true" << std::endl;
this->addChildComponent(this->moveControl);
this->addChildComponent(this->navigation);
this->activateOnlyChilds();
}
DriveManager::~DriveManager() {
@@ -32,18 +34,7 @@ DriveManager::~DriveManager() {
delete this->spiPort;
}
void DriveManager::loop() {
DebugTimes moveControlTime;
this->moveControl->loop();
moveControlTime.stopConsol("MoveControlTime", 5);
DebugTimes navigationTime;
this->navigation->loop();
navigationTime.stopConsol("NavigationTime", 20);
if (currentModusPtr)
this->currentModusPtr->loop();
}
void DriveManager::run() {}
void DriveManager::nextModus() {
changeModus(this->currentModus++);
@@ -52,14 +43,12 @@ void DriveManager::nextModus() {
void DriveManager::changeModus(Modi modus) {
this->currentModus = modus;
delete this->currentModusPtr;
if (this->currentModusPtr) {
this->removeChildComponent(this->currentModusPtr);
delete this->currentModusPtr;
}
//Reset Route to first point
this->navigation->startNavigation();
//Set moveControl to a safe state
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
switch (modus) {
case Modi::Off:
@@ -94,5 +83,8 @@ void DriveManager::changeModus(Modi modus) {
this->currentModusPtr = nullptr;
break;
}
if (this->currentModusPtr)
this->addChildComponent(this->currentModusPtr);
}
src/driveModi/driveManager.h
+4 -12
View File
@@ -22,6 +22,7 @@
#include "networkConfig.h"
#include "debugTimes.h"
#include "controlPadInput.h"
#include "component.h"
// All Drive Modi
#include "driveModi/Modi/ManualControl/manualControl.h"
@@ -43,7 +44,7 @@ enum class Modi {
TestMode
};
class DriveManager {
class DriveManager : public Component {
public:
/**
* @brief Construct a new Drive Manager object
@@ -60,17 +61,6 @@ class DriveManager {
*/
~DriveManager();
/**
* @brief Calls all loop functions
*
* Calls the MoveControl, Navigation loop function. The
* loop function from the DriveMode is called if it is set.
*
* This function should be called every main loop.
*
*/
void loop();
/**
* @brief Increment the DriveModi enum
* Than calls changeModus
@@ -116,6 +106,8 @@ class DriveManager {
Modi getDriveModi() { return this->currentModus; }
private:
void run() override;
Modi currentModus = Modi::Off;
MoveControl *moveControl;
const ControlPadInput* input;
src/driveModi/driveModi.cpp
+13
View File
@@ -0,0 +1,13 @@
#include "driveModi.h"
DriveModi::DriveModi(MoveControl* moveControl) {
this->moveControl = moveControl;
this->moveControl->setDrivingStatus(MoveControl::Status::Drive);
this->loopDelay = 40;
}
DriveModi::~DriveModi() {
this->moveControl->setSpeed(0);
this->moveControl->setRotationSpeed(0);
this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
}
src/driveModi/driveModi.h
+27 -7
View File
@@ -12,24 +12,44 @@
#ifndef DRIVEMODI_H
#define DRIVEMODI_H
#include "component.h"
#include "moveControl.h"
/**
* @brief Baseclass to build DriveModi
*
* This class must be inherited by other classes which want to be
* act as a DriveModi, because the DriveModi structure uses polymorphism.
*/
class DriveModi {
class DriveModi : public Component {
public:
DriveModi(){}
virtual ~DriveModi(){}
DriveModi(MoveControl* moveControl);
virtual ~DriveModi();
/**
* @brief This function is called by the DriveManager
* @brief Set the max speed
*
* All actions from a DriveMode have to called from
* this function or the PS3-Controller
* @param maxSpeed in m/s
*/
virtual void loop() = 0;
void setMaxSpeed(double maxForwardSpeed) { maxForwardSpeed = maxForwardSpeed; }
void increaseMaxSpeed(double increase = 0.1) { maxForwardSpeed += increase; }
void decreaseMaxSpeed(double increase = 0.1) { maxForwardSpeed -= increase; }
double getMaxSpeed() { return this->maxForwardSpeed; }
/**
* @brief Set the max rotation
*
* @param maxRotation in rad/s (maybe)
*/
void setMaxRotation(double maxRotation) { maxRotationSpeed = maxRotation; }
void increaseMaxRotation(double increase = 0.1) { maxRotationSpeed += increase; }
void decreaseMaxRotation(double increase = 0.1) { maxRotationSpeed -= increase; }
double getMaxRotation() { return this->maxRotationSpeed; }
protected:
MoveControl *moveControl;
double maxForwardSpeed = 1;
double maxRotationSpeed = 7;
};
#endif // DRIVEMODI_H
src/main.cpp
+4 -3
View File
@@ -135,19 +135,20 @@ void loop() {
if (wifiIsActive) {
DebugTimes wifiTime;
Network::checkWiFi();
#ifdef MQTT
bool err = Network::checkMQTT();
if (!err)
std::cout << "loop(): checkMQTT returns false" << std::endl;
#endif //MQTT
wifiTime.stopConsol("WiFi-Time", 10);
}
driveManager->loop();
controlPad->loop();
main_m->update();
lcdWrapper->loop();
mainBattery->loop();
bool newValues = mainBattery->loop();
if (newValues) {
if (mainBattery->isNewValue()) {
static uint8_t batteryLowCounter = 0;
if (mainBattery->isBatteryLow(10))
batteryLowCounter++;
src/moveControl.cpp
+10 -36
View File
@@ -11,6 +11,7 @@
#include "moveControl.h"
MoveControl::MoveControl() {
this->loopDelay = 20;
this->left_motor = new MotorControl();
this->right_motor = new MotorControl();
this->left_speedometer = new Speedometer(M_ENCODE_LEFT, WHEEL_DIAMETER, ENC_STEPS);
@@ -36,7 +37,12 @@ MoveControl::MoveControl() {
this->right_pid->SetMode(AUTOMATIC);
this->left_motor->init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
this->right_motor->init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
this->addChildComponent(this->left_motor);
this->addChildComponent(this->right_motor);
this->addChildComponent(this->left_speedometer);
this->addChildComponent(this->right_speedometer);
}
MoveControl::~MoveControl() {
@@ -52,39 +58,7 @@ MoveControl::~MoveControl() {
}
void MoveControl::loop() {
uint16_t left_motor_time = this->left_motor->loop();
uint16_t right_motor_time = this->right_motor->loop();
uint16_t left_speed_time = this->left_speedometer->loop();
uint16_t right_speed_time = this->right_speedometer->loop();
if (left_motor_time > 50
|| right_motor_time > 50
|| left_speed_time > 50
|| right_speed_time > 50) {
// Dont count overTimeCounter if one speedometer is in TestMode.
if (left_speed_time == UINT16_MAX || right_speed_time == UINT16_MAX)
this->overTimeCounter--;
this->overTimeCounter++;
if (this->overTimeCounter >= this->overTimeMax) {
char buf[128];
sprintf(buf, "left M: %d, right M %d, left S %d, right S %d in moveControl::loop\n",
left_motor_time, right_motor_time, left_speed_time, right_speed_time);
std::cout << buf;
this->overTimeCounter = 0;
}
}
if (millis() - this->lastMillis < this->delay)
return;
this->runMoveControl();
this->lastMillis = millis();
}
void MoveControl::runMoveControl() {
void MoveControl::run() {
this->updateCurrentWheelSpeed();
this->calcTargetWheelSpeed();
@@ -207,8 +181,8 @@ void MoveControl::regulateMotors() {
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_pid_out);
this->setSpeedometerDirection(this->right_speedometer, 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 :