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/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