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

Merge branch 'main' of git.kleiax.de:kleiax/Projektarbeit-Rover into main

Browse Source
This commit is contained in:
kleiax
2023-08-14 07:15:26 +02:00
parent 147eea1f92 bad84b1ef3
commit bf4c1dd49f
39 changed files with 763 additions and 315 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/driveModi/Modi/Autopilot/autopilot.cpp
+86 -19
View File
@@ -12,13 +12,31 @@
#include "driveModi/Modi/Autopilot/autopilot.h"
#include "autopilot.h"
DirectionChangeSignal::DirectionChangeSignal(Navigation* navigation) {
this->navigation = navigation;
this->action();
}
DirectionChangeSignal::~DirectionChangeSignal() {
navigation->dissableCalcAzimuth();
}
void DirectionChangeSignal::action() {
this->navigation->drivingDirectionChange();
}
Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
: ManualControl(moveControl, input) {
this->setInputMode(ManualControl::InputMode::Digital);
this->directionChangeSignal = new DirectionChangeSignal(navigation);
this->setDirectionChangeCallback(this->directionChangeSignal);
this->navigation = navigation;
this->init();
}
Autopilot::~Autopilot() {
delete this->directionChangeSignal;
// this->navigation->getNTRIPClient()->setActivated(false);
}
@@ -53,6 +71,7 @@ void Autopilot::runAutopilot() {
case State::NavigationStarted:
this->askNavigationForOrder();
this->checkButtonInput();
break;
case State::GetToStartPoint:
@@ -72,6 +91,11 @@ void Autopilot::runAutopilot() {
this->selfDriving();
break;
case SelfDrivingRotate:
this->checkButtonInput();
this->rotate();
break;
case State::TargetReached:
break;
@@ -92,6 +116,14 @@ bool Autopilot::shouldUpdate() {
return false;
}
void Autopilot::testRotate(int16_t degree) {
if (!degree)
return;
this->courseCorrection.correction = degree;
this->beginRotate();
}
void Autopilot::init() {
if (this->navigation->startNavigation())
this->state = State::NavigationStarted;
@@ -115,28 +147,62 @@ void Autopilot::drive() {
this,moveControl->setSpeed(0);
}
void Autopilot::rotate() {
this->moveControl->setSpeed(0);
if (this->courseCorrection.correction > 0)
this->moveControl->setRotationSpeed(this->rotationSpeed);
else
void Autopilot::beginRotate() {
if (this->state != State::SelfDrivingRotate) {
this->lastState = this->state;
this->state = State::SelfDrivingRotate;
this->rotationAimAzimuth = this->navigation->getAzimuth() + this->courseCorrection.correction;
this->rotationAimAzimuth = Navigation::fixDegree(this->rotationAimAzimuth);
this->moveControl->setRotationSpeed(-this->rotationSpeed);
this->moveControl->setSpeed(0);
if (this->courseCorrection.correction > 0)
this->moveControl->setRotationSpeed(-this->rotationSpeed);
else
this->moveControl->setRotationSpeed(this->rotationSpeed);
}
}
void Autopilot::rotate() {
if (abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 5) {
this->endRotate();
return;
}
if ((abs(this->navigation->getAzimuth() - this->rotationAimAzimuth) < 15)
&&
((this->courseCorrection.correction > 0
&& this->rotationAimAzimuth < this->navigation->getAzimuth())
|| (this->courseCorrection.correction < 0
&& this->rotationAimAzimuth > this->navigation->getAzimuth())))
{
this->endRotate();
std::cout << "Autopilot::rotate: Rover rotated too far" << std::endl;
}
}
void Autopilot::endRotate() {
if (this->state != State::SelfDrivingRotate)
return;
this->state = this->lastState;
this->navigation->drivingDirectionChange();
this->moveControl->setRotationSpeed(0);
this->moveControl->emergencyStop();
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) {
if (this->state == State::SelfDrivingAvailable)
this->state = State::SelfDriving;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
} else if (this->state == State::SelfDriving) {
else if (this->state == State::SelfDriving || this->state == State::SelfDrivingRotate)
this->state = State::SelfDrivingAvailable;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
}
else if (this->state == State::NavigationStarted)
this->state = State::GetToStartPoint;
this->updateDisplay = true;
this->lastAutopilotChangeMillis = millis();
}
}
@@ -172,10 +238,11 @@ void Autopilot::askNavigationForOrder() {
}
void Autopilot::selfDriving() {
if (this->lastOrderStatus == Navigation::Status::Updated) {
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
this->rotate();
else
this->drive();
}
if (this->state != State::SelfDriving)
return;
if (abs(this->courseCorrection.correction) >= this->maxCourseDeviationBerforeAct)
this->beginRotate();
else
this->drive();
}
src/driveModi/Modi/Autopilot/autopilot.h
+19 -10
View File
@@ -17,6 +17,16 @@
#include "moveControl.h"
#include "driveModi/Modi/ManualControl/manualControl.h"
class DirectionChangeSignal : public DirectionChangeWrapper {
public:
DirectionChangeSignal(Navigation* navigation);
~DirectionChangeSignal();
void action() override;
private:
Navigation* navigation;
};
/**
* @brief This class use the navigate class to drive automaticaly
*
@@ -36,6 +46,7 @@ class Autopilot : public ManualControl {
GetToStartPoint,
SelfDrivingAvailable,
SelfDriving,
SelfDrivingRotate,
TargetReached
};
@@ -66,15 +77,6 @@ class Autopilot : public ManualControl {
*/
void loop();
/**
* @brief Manges the autopilot
*
* If the first Point is near to current location you can turn
* the autopilot on.
* Gets the course correction and decide what to do.
*/
void runAutopilot();
void restart();
/**
@@ -105,11 +107,15 @@ class Autopilot : public ManualControl {
* @return false
*/
bool shouldUpdate();
void testRotate(int16_t degree);
void endRotate();
private:
void init();
void drive();
void beginRotate();
void rotate();
void runAutopilot();
void checkButtonInput();
void askNavigationForOrder();
void selfDriving();
@@ -120,6 +126,7 @@ class Autopilot : public ManualControl {
State state = State::None;
State lastState = State::None;
Navigation::Status lastOrderStatus;
DirectionChangeSignal* directionChangeSignal;
bool updateDisplay = false;
@@ -131,8 +138,10 @@ class Autopilot : public ManualControl {
uint32_t loopLastMillis = 0;
uint32_t lastAutopilotChangeMillis = 0;
int16_t rotationAimAzimuth;
double drivingSpeed = 1;
double rotationSpeed = 3;
double rotationSpeed = 4.5;
double minRemainingDistance = 0.25;
};
src/driveModi/Modi/ManualControl/manualControl.cpp
+46 -3
View File
@@ -26,14 +26,30 @@ void ManualControl::loop() {
if (this->caliCompass)
this->caliCompass->loop();
if (millis() - this->lastMillis < delay) {
if (millis() - this->lastMillis < delay)
return;
switch (this->inputMode) {
case InputMode::Analog :
this->analogControl();
break;
case InputMode::Digital :
this->digitalControl();
break;
default:
break;
}
this->runManualControl();
this->lastMillis = millis();
}
void ManualControl::runManualControl() {
void ManualControl::switchInputMode() {
this->inputMode = (this->inputMode == InputMode::Analog) ? InputMode::Digital : InputMode::Analog;
}
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;
@@ -49,3 +65,30 @@ void ManualControl::runManualControl() {
value_per_step = this->max_rotation * 2 / UINT8_MAX;
this->moveControl->setRotationSpeed(x * value_per_step);
}
void ManualControl::digitalControl() {
int16_t y = this->input->x - 127;
int16_t x = this->input->y - 127;
if (y > 120)
this->moveControl->setSpeed(-this->max_speed);
else if (y < -120)
this->moveControl->setSpeed(this->max_speed);
else
this->moveControl->setSpeed(0);
if (x > 120)
this->moveControl->setRotationSpeed(this->max_rotation);
else if (x < -120)
this->moveControl->setRotationSpeed(-this->max_rotation);
else
this->moveControl->setRotationSpeed(0);
if (this->directionChangeWrapper && (x > 120 || x < -120))
this->lastLoopTurned = true;
else if (this->lastLoopTurned) {
if (this->directionChangeWrapper)
this->directionChangeWrapper->action();
this->lastLoopTurned = false;
}
}
src/driveModi/Modi/ManualControl/manualControl.h
+27 -8
View File
@@ -16,6 +16,11 @@
#include "controlPadInput.h"
#include "calibrateCompass.h"
class DirectionChangeWrapper {
public:
virtual void action() = 0;
};
/**
* @brief Drive the Rover with a Joystick
*
@@ -26,6 +31,11 @@
*/
class ManualControl : public DriveModi {
public:
enum class InputMode : uint8_t {
Analog,
Digital
};
ManualControl(MoveControl *moveControl, const ControlPadInput *input);
/**
@@ -35,7 +45,7 @@ class ManualControl : public DriveModi {
~ManualControl();
/**
* @brief Calls runManualControl() to update all values.
* @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.
@@ -44,12 +54,6 @@ class ManualControl : public DriveModi {
*/
void loop() override;
/**
* @brief Noramly called repeatedly by loop() to calcluate new values.
* Set new values for speed and rotation in moveControl
*/
void runManualControl();
/**
* @brief Set the min delay between each loop
*
@@ -79,6 +83,11 @@ class ManualControl : public DriveModi {
void setCalibrateCompass(CalibrateCompass* val = nullptr) { this->caliCompass = val; }
void switchInputMode();
void setInputMode(InputMode mode) { this->inputMode = mode; }
InputMode getInputMode() const { return this->inputMode; }
void setDirectionChangeCallback(DirectionChangeWrapper* callback) { this->directionChangeWrapper = callback; }
uint16_t getDutycycleLeft() const { return this->moveControl->getDutycycleLeft(); }
uint16_t getDutycycleRight() const { return this->moveControl->getDutycycleRight(); }
@@ -87,11 +96,21 @@ class ManualControl : public DriveModi {
const ControlPadInput* input;
private:
uint32_t lastMillis = 0;
/**
* @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;
double max_speed = 1;
double max_rotation = 7;
CalibrateCompass* caliCompass = nullptr;
DirectionChangeWrapper* directionChangeWrapper = nullptr;
InputMode inputMode = InputMode::Analog;
};
#endif // MANUAL_CONTROL_H