cleaning

refactore
all components now have a base class compnent for loop functions

include/moveControl.h

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

@@ -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() {
+void Battery::run() {
-    if (millis() - this->lastMillis < this->delay)
-        return false;
-
     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

@@ -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
          * 
@@ -74,7 +55,7 @@ class Battery {
          * 
          * @return uint8_t charge level in percent
          */
-        uint8_t getBatteryPercent()   {return this->batteryPercent;}
+        uint8_t getBatteryPercent() { return this->batteryPercent; }
 
         /**
          * @brief Checks if the battery is low.
@@ -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

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

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

@@ -12,23 +12,19 @@
 #include "controlPad.h"
 
 ControlPad::ControlPad() {
-
+    this->loopDelay = 5;
 }
 
-bool ControlPad::loop() {
+void ControlPad::run() {
-    if (millis() - this->lastLoopMillis < this->loopDelay)
-        return false;
-    this->lastLoopMillis = millis();
-
     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

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

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

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

@@ -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() {
+void MotorControl::run() {
-    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() {
     // 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

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

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

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

@@ -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() {
+void NTRIPClient::run() {
-    this->pushGPGGA();
-
-    if (millis() - this->lastLoopTime < this->delayTime)
-        return;
-    this->lastLoopTime = millis();
-
     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

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

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

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

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

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

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

@@ -77,9 +77,8 @@ void MenuTestMode::init() {
     this->driveManager->changeModus(Modi::TestMode);
     this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
 
-    testMode->setSpeed(50);
+    testMode->setMaxSpeed(1);
-    testMode->setRotationSpeed(100);
+    testMode->setMaxRotation(8);
-
 
     auto dummy = []() {
         std::cout << "Dummy in Action" <<std::endl;

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -40,24 +40,9 @@ Autopilot::~Autopilot() {
     // this->navigation->getNTRIPClient()->setActivated(false);
 }
 
-void Autopilot::loop() {
+void Autopilot::run() {
-    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;
-
     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->maxRotationSpeed = 7;
-        this->maxForwardSpeed = 1;
+        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

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

@@ -23,17 +23,8 @@ CaptureRoute::~CaptureRoute() {
     // this->navigation->getNTRIPClient()->setActivated(false);
 }
 
-void CaptureRoute::loop() {
+void CaptureRoute::run() {
-    ManualControl::loop();
+    ManualControl::run();
-
-    if (millis() - this->lastMillis < this->delay)
-        return;
-    
-    this->runCaptureRoute();
-    this->lastMillis = millis();
-}
-
-void CaptureRoute::runCaptureRoute() {
     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

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

@@ -10,25 +10,16 @@
  */
 #include "manualControl.h"
 
-ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) { 
+ManualControl::ManualControl(MoveControl* moveControl, const ControlPadInput *input)
-    this->moveControl = moveControl;
+                : 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() {
+void ManualControl::run() {    
-    if (this->caliCompass)
-        this->caliCompass->loop();
-
-    if (millis() - this->lastMillis < delay)
-        return;
-    
     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->x - 127;
-    int16_t x = this->input->y - 127;
+    int16_t y = this->input->y - 127;
 
-    // if (x || y) {
+    // static int counter = 0;
-    //     std::cout << "x: " << (int) this->input->x << " y: " << (int) this->input->y << std::endl;
+    // if (counter % 60 == 0) {
-    //     std::cout << "x: " << (int) x << " y: " << (int) y << std::endl;
+    //     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

@@ -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();
 
-        /**
+        void setCalibrateCompass(CalibrateCompass* caliCompass = nullptr);
-         * @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 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

@@ -10,8 +10,8 @@
  */
 #include "testMode.h"
 
-TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) {
+TestMode::TestMode(MoveControl *moveControl,  Navigation* navigation) 
-    this->moveControl = moveControl;
+        : DriveModi(moveControl) {
     this->navigation = navigation;
 }
 
@@ -19,10 +19,7 @@ TestMode::~TestMode() {
     this->moveControl->setDrivingStatus(MoveControl::Status::Stop);
 }
 
-void TestMode::loop() {
+void TestMode::run() {
-    if (millis() - this->lastMillis < this->delay)
-        return;
-
     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

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

@@ -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() {
+void DriveManager::run() {}
-    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::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

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

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

@@ -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(){}
+        DriveModi(MoveControl* moveControl);
-        virtual ~DriveModi(){}
+        virtual ~DriveModi();
 
         /**
-         * @brief This function is called by the DriveManager
+         * @brief Set the max speed
          * 
-         * All actions from a DriveMode have to called from
+         * @param maxSpeed in m/s 
-         * this function or the PS3-Controller
          */
-        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

@@ -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 (mainBattery->isNewValue()) {
-
-    if (newValues) {
         static uint8_t batteryLowCounter = 0;
         if (mainBattery->isBatteryLow(10))
             batteryLowCounter++;

src/moveControl.cpp

@@ -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() {
+void MoveControl::run() {
-    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() {
     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->left_speedometer, this->left_motor->getPower());
-            this->setSpeedometerDirection(this->right_speedometer, this->right_pid_out);
+            this->setSpeedometerDirection(this->right_speedometer, this->right_motor->getPower());
             break;
 
         case Status::Raw :