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
include/moveControl.h
+3 -45
View File
@@ -20,6 +20,7 @@
#include "speedometer.h"
#include "moveControlConfig.h"
#include "debugTimes.h"
#include "component.h"
/**
@@ -30,7 +31,7 @@
* given target speed to calculate a new duty cycle for the motors.
*
*/
class MoveControl {
class MoveControl : public Component {
public:
/**
* @brief used to set driving status
@@ -58,22 +59,6 @@ class MoveControl {
*/
~MoveControl();
/**
* @brief Calls runMoveControl() to update all Values.
*
* Besides that this function calls the loop() functions for the motors and encodes.
* This function should be called every mainloop. If the delay is not reached, than the
* functions returns immediately.
* @see runMoveControl()
* @see setDelay()
*/
void loop();
/**
* @brief Normally called repeatedly by loop() to calculate new values.
*/
void runMoveControl();
/**
* @brief Set the Status
*
@@ -159,36 +144,11 @@ class MoveControl {
uint16_t getDutycycleLeft() const { return this->left_motor->getDutycycle(); }
uint16_t getDutycycleRight() const { return this->right_motor->getDutycycle(); }
/**
* @brief Set the min delay between each loop
*
* @param delay_ time in Milliseconds
*/
void setDelay(uint8_t delay) { this->delay = delay; }
private:
void run() override;
void setSpeedometerDirection(Speedometer *speedometer, double value);
/**
* @brief Converts values into wheel speeds
*
* Converts target speed and target rotationspeed into
* wheel speeds
*/
void calcTargetWheelSpeed();
/**
* @brief Set the target power to motors
*
* Checks if the driving_status is set to Drive.
* If yes, then the motors get the pid_out values as targetpower.
* If no, then the motors target power is set to zero.
*/
void regulateMotors();
/**
* @brief Updates the wheel speeds with speedometer
*/
void updateCurrentWheelSpeed();
MotorControl *left_motor;
@@ -210,11 +170,9 @@ class MoveControl {
double left_pid_out;
double right_pid_out;
uint8_t delay = 20;
uint8_t overTimeCounter = 0;
uint8_t overTimeMax = 100;
int8_t rawPowerLeft = 0;
int8_t rawPowerRight = 0;
uint32_t lastMillis = 0;
};
#endif // MOVE_CONTROL_H
lib/Battery/battery.cpp
+14 -7
View File
@@ -16,28 +16,27 @@ Battery::Battery(uint8_t pin, uint32_t r1, uint32_t r2) {
this->r1 = r1;
this->r2 = r2;
this->initBuffer();
this->loopDelay = 100;
}
Battery::Battery(uint8_t pin) {
this->pin = pin;
this->initBuffer();
this->loopDelay = 100;
}
bool Battery::loop() {
if (millis() - this->lastMillis < this->delay)
return false;
void Battery::run() {
this->readAdcToBuf();
this->loopCounter++;
this->lastMillis = millis();
if (this->loopCounter == this->calulationDelayMultiplier) {
this->calculateBatteryVoltage();
this->calculateBatteryPercent();
return true;
this->loopCounter = 0;
this->calculatetNewValues = true;
}
return false;
return;
}
double Battery::getBatteryVoltage() {
@@ -51,6 +50,14 @@ bool Battery::isBatteryLow(double voltage) {
return false;
}
bool Battery::isNewValue() {
if (!this->calculatetNewValues)
return false;
this->calculatetNewValues = false;
return true;
}
double Battery::calculateInputVoltage() {
// Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
double reading = this->getBufAvg();
lib/Battery/battery.h
+7 -24
View File
@@ -17,6 +17,8 @@
#include <math.h>
#include <Arduino.h>
#include "component.h"
/**
* @brief A class for battery monitoring
*
@@ -25,7 +27,7 @@
* to be after a voltage diveder, so that maximum voltage for the
* microcontroller is 3.3 Volt.
*/
class Battery {
class Battery : public Component {
public:
/**
* @brief Construct a new Battery object
@@ -41,27 +43,6 @@ class Battery {
Battery(uint8_t pin, uint32_t r1, uint32_t r2);
Battery(uint8_t pin);
/**
* @brief Read new values if the delay is reached.
*
* @return bool true if new values are calculated
*/
bool loop();
/**
* @brief Set the delay to wait befor new values are read
*
* @param delay in milliseconds
*/
void setDelay(uint16_t delay) {this->delay = delay; }
/**
* @brief Get the delay to wait befor new values are read
*
* @return uint16_t delay in milliseconds
*/
uint16_t getDelay() {return this->delay; }
/**
* @brief Get the battery voltage
*
@@ -89,7 +70,10 @@ class Battery {
*/
bool isBatteryLow(double voltage);
bool isNewValue();
private:
void run() override;
double calculateInputVoltage();
void calculateBatteryVoltage();
void calculateBatteryPercent();
@@ -104,13 +88,12 @@ class Battery {
uint8_t batteryPercent = 0;
uint8_t batteryLowPercent = 10;
uint8_t bufferPos = 0;
uint8_t delay = 100;
uint8_t calulationDelayMultiplier = 10;
uint8_t loopCounter = 0;
uint16_t adcBuffer[bufferSize];
uint32_t r1 = 0;
uint32_t r2 = 0;
uint64_t lastMillis = 0;
bool calculatetNewValues = false;
double batteryVoltage = 0;
const float capacityVoltages[21] = {9.82, 10.83, 11.06, 11.12, // 0 5 10 15
lib/Component/component.cpp
+40
View File
@@ -0,0 +1,40 @@
#include "component.h"
Component::Component(uint16_t loopDelay) {
this->loopDelay = loopDelay;
}
void Component::loop() {
if (!this->active)
return;
if (this->childComponents.size()){
std::list<Component*>::iterator it;
for (it = this->childComponents.begin(); it != this->childComponents.end(); it++)
(*it)->loop();
}
this->runAsChild();
if (this->onlyChilds)
return;
if (this->loopDelay && millis() - this->lastMillis < this->loopDelay)
return;
this->lastMillis = millis();
this->beforeRun();
this->run();
this->afterRun();
if (this->timeUpdateAfter)
this->lastMillis = millis();
}
void Component::addChildComponent(Component* child) {
this->childComponents.push_back(child);
}
void Component::removeChildComponent(Component* child) {
this->childComponents.remove(child);
}
lib/Component/component.h
+51
View File
@@ -0,0 +1,51 @@
/**
* @file component.h
* @author Alexander Klein (alex@kleiax.de)
* @brief
* @version 0.1
* @date 2023-08-16
*
* @copyright Copyright (c) 2023
*
*/
#pragma once
#include <Arduino.h>
#include <list>
class Component {
public:
Component() {}
Component(uint16_t loopDelay);
void loop();
void deactivate() { this->active = false; }
void activate() { this->active = false; }
protected:
virtual void runAsChild() {}
virtual void beforeRun() {}
virtual void run() = 0;
virtual void afterRun() {}
void addChildComponent(Component* child);
void removeChildComponent(Component* child);
void activateOnlyChilds() { this->onlyChilds = true; }
void deactivateOnlyChilds() { this->onlyChilds = false; }
void setTimerAfterTask() { this->timeUpdateAfter = true; }
uint16_t loopDelay = 0;
private:
std::list<Component*> childComponents;
bool active = true;
bool onlyChilds = false;
bool timeUpdateAfter = false;
uint32_t lastMillis = 0;
};
lib/ControllPad/controlPad.cpp
+6 -10
View File
@@ -12,23 +12,19 @@
#include "controlPad.h"
ControlPad::ControlPad() {
this->loopDelay = 5;
}
bool ControlPad::loop() {
if (millis() - this->lastLoopMillis < this->loopDelay)
return false;
this->lastLoopMillis = millis();
void ControlPad::run() {
if(!this->connected)
return false;
return;
if (millis() - this->lastMessageReceive > this->disconnectTime) {
this->connected = false;
this->controlInput.buttons = 0;
this->controlInput.x = 127;
this->controlInput.y = 127;
return false;
return;
}
if (this->lastButtons != controlInput.buttons)
@@ -39,7 +35,7 @@ bool ControlPad::loop() {
this->menuControl->printMenu();
this->firstButtonPress = false;
std::cout << "ControlPad::loop - First menu print" << std::endl;
return true;
return;
}
if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Left))
@@ -58,7 +54,7 @@ bool ControlPad::loop() {
this->updated = false;
this->lastButtons = controlInput.buttons;
}
return true;
return;
}
void ControlPad::insertData(const uint8_t *data) {
lib/ControllPad/controlPad.h
+5 -7
View File
@@ -13,14 +13,13 @@
#include <menuControl.h>
#include "controlPadInput.h"
#include <controlPadInput.h>
#include <component.h>
class ControlPad {
class ControlPad : public Component {
public:
ControlPad();
bool loop();
void insertData(const uint8_t *data);
void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; }
@@ -30,6 +29,8 @@ class ControlPad {
bool isControlPadConnected() const { return this->connected; }
private:
void run() override;
MenuControl* menuControl = nullptr;
ControlPadInput controlInput;
@@ -42,9 +43,6 @@ class ControlPad {
uint8_t lastButtons = 0;
uint16_t disconnectTime = 100;
uint16_t loopDelay = 5;
uint32_t lastMessageReceive = 0;
uint32_t lastLoopMillis = 0;
};
lib/LcdWrapper/LcdWrapper.cpp
+1 -1
View File
@@ -17,7 +17,7 @@ LcdWrapper::LcdWrapper(LiquidCrystal_I2C* lcd) {
this->changed = false;
}
void LcdWrapper::loop() {
void LcdWrapper::run() {
if (!this->changed)
return;
lib/LcdWrapper/LcdWrapper.h
+4 -7
View File
@@ -15,6 +15,7 @@
#include <iostream>
#include <LiquidCrystal_I2C.h>
#include <displayWrapper.h>
#include <component.h>
#define DISPLAY_WRAPPER_ROWS 16
#define DISPLAY_WRAPPER_LINES 2
@@ -28,7 +29,7 @@ typedef void (*LcdWrapperCallback) (const char data[][DISPLAY_WRAPPER_ROWS], uin
* The class takes the information to print from any thread. The
* loop function has to be called to print the data.
*/
class LcdWrapper : public DisplayWrapper {
class LcdWrapper : public DisplayWrapper, public Component {
public:
/**
* @brief Construct a new Lcd Wrapper object
@@ -37,12 +38,6 @@ class LcdWrapper : public DisplayWrapper {
*/
LcdWrapper(LiquidCrystal_I2C* lcd);
/**
* @brief Print the saved data
*
*/
void loop();
/**
* @brief Empty the buffer
*
@@ -67,6 +62,8 @@ class LcdWrapper : public DisplayWrapper {
private:
void run() override;
LiquidCrystal_I2C* lcd;
LcdWrapperCallback callback = nullptr;
char data[DISPLAY_WRAPPER_LINES][DISPLAY_WRAPPER_ROWS];
lib/MotorControl/motorControl.cpp
+3 -20
View File
@@ -15,6 +15,7 @@
MotorControl::MotorControl() {
this->setMinPwm(PWMMIN);
this->setMaxPwm(PWMMAX);
this->loopDelay = DELAY;
}
void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2) {
@@ -34,20 +35,7 @@ void MotorControl::init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8
ledcWrite(this->pwmChannel, 0);
}
uint16_t MotorControl::loop() {
uint32_t time = millis();
uint16_t elapsed_time = time - this->lastMillis;
//Cancel if delayLoop is not reached
if (elapsed_time < delayLoop)
return elapsed_time;
runMotorControl();
this->lastMillis = time;
return elapsed_time;
}
void MotorControl::runMotorControl() {
void MotorControl::run() {
// Absolute difference between targetPower and power
uint8_t abs_difference = abs(this->targetPower - this->power);
@@ -108,7 +96,7 @@ void MotorControl::setMaxPwm(uint8_t max) {
uint16_t MotorControl::setPowerSteps(uint8_t increment) {
this->powersteps = increment;
return (uint16_t) (delayLoop * ( 100 / powersteps ));
return (uint16_t) (this->loopDelay * ( 100 / powersteps ));
}
void MotorControl::setTargetPower(int8_t power) {
@@ -118,11 +106,6 @@ void MotorControl::setTargetPower(int8_t power) {
std::cout << " MotorControl::setTargetPower: Invalid Argument - Power: " << power << std::endl;
}
uint16_t MotorControl::setDelay(uint8_t delayLoop) {
this->delayLoop = delayLoop;
return (uint16_t) (delayLoop * ( 100 / powersteps ));
}
void MotorControl::stop() {
this->targetPower = 0;
}
lib/MotorControl/motorControl.h
+4 -37
View File
@@ -16,6 +16,8 @@
#include <iostream>
#include <Arduino.h>
#include <component.h>
#define DELAY 10
#define PWMFREQ 16000
#define PWMRES 8
@@ -28,7 +30,7 @@
* You can control the acceleration of the motor, for example to
* prevent a damage on your H-Bridge.
*/
class MotorControl {
class MotorControl : public Component {
public:
MotorControl();
@@ -42,26 +44,6 @@ class MotorControl {
*/
void init(uint8_t pwmPin, uint8_t pwmChannel, uint8_t dir_1, uint8_t dir_2);
/**
* @brief Calls runMotorControl() to update the pwm signal
*
* This function should be called every mainloop. If the delayLoop is not reached, than the
* functions returns immediately.
* @see runMotorControl()
* @see DELAY
* @return time since the last call in Milliseconds
*/
uint16_t loop();
/**
* @brief Normally called repeatedly by loop() to update the pwm signal.
*
* Checks the difference between target power and current power to
* increase or decrease the duty cycle. The amount of decrease or increase
* is set by setPowerSetps() (default = 2).
*/
void runMotorControl();
/**
* @brief Set the minimum duty cycle
*
@@ -104,19 +86,6 @@ class MotorControl {
*/
void setTargetPower(int8_t power);
/**
* @brief Set the min delayLoop between each loop
*
* Note the dependency between delayLoop and
* setPowerSteps().
*
* @see setPowerSteps()
*
* @param delayLoop time in Milliseconds
* @return time from 0% power to 100% power in Milliseconds
*/
uint16_t setDelay(uint8_t delayLoop);
/**
* @brief Stops the motor like setTargetPower() to 0
*
@@ -149,6 +118,7 @@ class MotorControl {
bool isAccelerationNegative();
private:
void run() override;
void setRealPower(int8_t power);
void increasePower(int8_t power);
@@ -164,11 +134,8 @@ class MotorControl {
uint8_t dutycycleMax;
uint8_t dir_1;
uint8_t dir_2;
uint8_t delayLoop = DELAY;
uint8_t powersteps = POWERSTEPS;
uint32_t lastMillis = 0;
};
#endif // MOTOR_CONTROL_H
lib/Navigation/navigation.cpp
+13 -20
View File
@@ -39,11 +39,6 @@ void Navigation::init(Route* route) {
uint8_t versionLow = this->gps->getProtocolVersionLow();
std::cout << "u-blox protocol version: " << unsigned(versionHigh) << "." << unsigned(versionLow) << std::endl;
// std::cout << "Set GNSS Module to factory settings... ";
// this->gps->factoryReset();
// delay(5000);
// std::cout << "Complete" << std::endl;
this->gps->setSPIOutput(COM_TYPE_UBX);
this->gps->enableNMEAMessage(UBX_NMEA_GGA, COM_PORT_SPI, 10);
this->gps->setUSBOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
@@ -67,14 +62,16 @@ void Navigation::init(Route* route) {
CalibrateCompass caliCompass(this->compass);
caliCompass.loadData();
caliCompass.useData();
// std::cout << "Navigation::init compass correction data: " << caliCompass << std::endl;
this->loopDelay =AZIMUTH_UPDATE_DELAY;
}
Navigation::~Navigation() {
delete this->gps;
delete this->compass;
delete this->ntripClient;
delete this->route;
if (this->ntripClient)
delete this->ntripClient;
}
void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String user, String password) {
@@ -83,9 +80,17 @@ void Navigation::initNtrip(String host, uint16_t port, String mountPoint, String
this->ntripClient->loop();
this->ntripClient->setActivated(false);
this->isNtripInit = true;
this->addChildComponent(this->ntripClient);
}
void Navigation::loop() {
void Navigation::run() {
this->compass->read();
this->realAzimuth = this->compass->getAzimuth();
this->updateMagneticDeclination();
}
void Navigation::runAsChild() {
this->gps->checkUblox();
this->gps->checkCallbacks();
@@ -93,18 +98,6 @@ void Navigation::loop() {
this->updateCurrentLocation();
Navigation::newData = false;
}
if (this->ntripClient)
this->ntripClient->loop();
if (millis() - this->lastMillis > AZIMUTH_UPDATE_DELAY) {
this->compass->read();
this->realAzimuth = this->compass->getAzimuth();
this->updateMagneticDeclination();
this->lastMillis = millis();
}
}
void Navigation::newRoute() {
lib/Navigation/navigation.h
+4 -8
View File
@@ -21,6 +21,7 @@
#include "route.h"
#include "NTRIPClient.h"
#include "calibrateCompass.h"
#include "component.h"
/**
* @brief The minimal distance between Points
@@ -54,7 +55,7 @@ struct CourseCorrection {
* be drive and the distance to the next checkpoint.
*
*/
class Navigation {
class Navigation : public Component {
public:
enum Status {
InsufficientAccuracy,
@@ -103,13 +104,6 @@ class Navigation {
*/
void initNtrip(String host, uint16_t port, String mountPoint, String user, String password);
/**
* @brief Decodes new GPS information
*
* Should be called ever main loop.
*/
void loop();
/**
* @brief creates a new empty route
*
@@ -218,6 +212,8 @@ class Navigation {
static int16_t fixDegree(int16_t degree);
private:
void run() override;
void runAsChild() override;
void updateCurrentLocation();
void updateMagneticDeclination();
void init(Route* route);
lib/NtripClient/ntripClient.cpp
+7 -7
View File
@@ -22,19 +22,15 @@ NTRIPClient::NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, c
this->ntripClient = new WiFiClient;
this->state = NTRIPClientStates::closeConnection;
this->loopDelay = 20;
}
NTRIPClient::~NTRIPClient() {
delete this->ntripClient;
}
void NTRIPClient::loop() {
this->pushGPGGA();
if (millis() - this->lastLoopTime < this->delayTime)
return;
this->lastLoopTime = millis();
void NTRIPClient::run() {
switch (this->state) {
case NTRIPClientStates::openConnection:
if (!this->activated) {
@@ -81,6 +77,10 @@ void NTRIPClient::loop() {
}
}
void NTRIPClient::runAsChild() {
this->pushGPGGA();
}
void NTRIPClient::gpsConfiguration() {
this->gps->setSPIOutput(COM_TYPE_UBX | COM_TYPE_NMEA);
this->gps->setPortInput(COM_PORT_SPI, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3);
lib/NtripClient/ntripClient.h
+4 -11
View File
@@ -19,6 +19,7 @@
#include <SparkFun_u-blox_GNSS_Arduino_Library.h>
#include "debugTimes.h"
#include "component.h"
/**
* @brief States for the state machine.
@@ -39,7 +40,7 @@ enum NTRIPClientStates {
* data and push it to a given gnss module. This module have to be compatible
* with the SparkFun u-blox GNSS Arduino Library.
*/
class NTRIPClient {
class NTRIPClient : public Component {
public:
/**
* @brief Construct a new NTRIPClient object
@@ -54,14 +55,6 @@ class NTRIPClient {
NTRIPClient(SFE_UBLOX_GNSS* gps, const char* host, uint16_t port, const char* mountPoint, const char* user, const char* password);
~NTRIPClient();
/**
* @brief Runs the state machine.
*
* See NTRIPClientStates for more information.
* Should be called ever main loop.
*/
void loop();
/**
* @brief Configure the Gnss module to accept correction data
*
@@ -100,6 +93,8 @@ class NTRIPClient {
NTRIPClientStates getClientState() { return this->state; }
private:
void run() override;
void runAsChild() override;
void pushGPGGA();
bool beginClient();
void closeConnection();
@@ -119,14 +114,12 @@ class NTRIPClient {
uint32_t lastReceivedRtcmTime = 0;
// uint32_t lastNtripConnectTime = 0; // can deleted?
uint32_t lastGPGGAPushTime = 0;
uint32_t lastLoopTime = 0;
uint32_t lastReconnectTime = 0;
char host[128];
char mountPoint[128];
char user[128];
char password[128];
const uint8_t delayTime = 20;
const uint8_t maxReconnectAttemps = 10;
const uint16_t reconnectDelayTime = 1000;
const uint16_t timeOut = 10000;
lib/Speedometer/speedometer.cpp
+3 -15
View File
@@ -14,6 +14,7 @@
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps, uint8_t numOfValForAvg) {
this->init(pin, diameter, steps);
this->bufSize = numOfValForAvg;
this->loopDelay = DELAY_SPEEDOMETER;
}
Speedometer::Speedometer(uint8_t pin, double diameter, uint16_t steps) {
@@ -25,24 +26,11 @@ Speedometer::~Speedometer() {
delete this->pulseCounter;
}
uint16_t Speedometer::loop() {
void Speedometer::run() {
if (this->calibrationRunning)
return -1;
return;
uint32_t time = millis();
uint16_t elapsedTime = time - this->lastMillisLoop;
//Cancel if delayLoop is not reached
if (elapsedTime < this->delayLoop)
return elapsedTime;
runSpeedometer();
this->lastMillisLoop = time;
return elapsedTime;
}
void Speedometer::runSpeedometer() {
uint32_t time = millis();
uint16_t elapsedTime = time - this->lastMillisCalc;
this->lastMillisCalc = time;
lib/Speedometer/speedometer.h
+3 -28
View File
@@ -17,6 +17,7 @@
#include <iostream>
#include "counter.h"
#include "component.h"
/**
* @brief The default size of numbers to be taken in account for the average.
@@ -38,7 +39,7 @@
* The calculated speed is the average of an amount of last measurements.
*
*/
class Speedometer {
class Speedometer : public Component {
public:
/**
* @brief Enum to control the direction.
@@ -66,24 +67,6 @@ class Speedometer {
~Speedometer();
/**
* @brief Calls runSpeedometer() to update all Values.
*
* This function should be called every mainloop. If the delayLoop is not reached, than the
* functions returns immediately.
* @see runSpeedometer()
* @see DELAY_SPEEDOMETER
* @return time since the last call in Milliseconds
*/
uint16_t loop();
/**
* @brief Normally called repeatedly by loop() to calculate new values.
*
* Add a new Value to the average and update the speed.
*/
void runSpeedometer();
/**
* @brief Set the direction
*
@@ -107,13 +90,6 @@ class Speedometer {
*/
void setEncFilter(uint16_t val);
/**
* @brief Set the min delayLoop between each loop
*
* @param delayLoop time in Milliseconds
*/
void setDelay(uint8_t delayLoop) { this->delayLoop = delayLoop; }
/**
* @brief Get the Direction
*
@@ -149,6 +125,7 @@ class Speedometer {
private:
void run() override;
void init(uint8_t pin, double diameter, uint16_t steps);
void initAvgBuf();
void clearAvgBuf();
@@ -166,13 +143,11 @@ class Speedometer {
uint8_t printCounter = 0;
uint8_t bufSize = BUFSIZE;
uint8_t delayLoop = DELAY_SPEEDOMETER;
uint8_t bufPos = 0;
uint16_t steps;
int16_t *buf = nullptr;
uint32_t lastMillisLoop = 0;
uint32_t lastMillisCalc = 0;
};
lib/calibrateCompass/calibrateCompass.cpp
+4 -1
View File
@@ -15,9 +15,10 @@ CalibrateCompass::CalibrateCompass(QMC5883LCompass* compass) {
this->compass = compass;
this->state = State::Ready;
this->clearData();
this->activateOnlyChilds();
}
void CalibrateCompass::loop() {
void CalibrateCompass::runAsChild() {
if (this->state != State::Calibrating)
return;
@@ -67,6 +68,8 @@ void CalibrateCompass::loop() {
}
}
void CalibrateCompass::run() {}
void CalibrateCompass::start() {
if (this->state != State::Ready)
return;
lib/calibrateCompass/calibrateCompass.h
+5 -2
View File
@@ -15,7 +15,9 @@
#include <Preferences.h>
#include <iostream>
class CalibrateCompass {
#include "component.h"
class CalibrateCompass : public Component {
public:
enum State {
Ready,
@@ -29,7 +31,6 @@ class CalibrateCompass {
CalibrateCompass(QMC5883LCompass* compass);
void loop();
void start();
void useData();
void removeCalibration();
@@ -43,6 +44,8 @@ class CalibrateCompass {
friend std::ostream& operator<<(std::ostream& os, const CalibrateCompass& caliComp);
private:
void runAsChild() override;
void run() override;
void checkDataValidity();
QMC5883LCompass* compass;
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
+11 -19
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,15 +43,13 @@ void DriveManager::nextModus() {
void DriveManager::changeModus(Modi modus) {
this->currentModus = modus;
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:
this->currentModusPtr = nullptr;
@@ -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
+9 -35
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);
@@ -37,6 +38,11 @@ MoveControl::MoveControl() {
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->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 :