edit manualControl for new ControlPad

.vscode/settings.json

@@ -82,11 +82,15 @@
     "cSpell.words": [
         "Ahnung",
         "akku",
+        "ardui",
         "blox",
         "bluedroid",
+        "CIPO",
+        "COPI",
         "gast",
         "GNSS",
         "GPGGA",
+        "HSPI",
         "Huus",
         "Keine",
         "kleiax",

include/config.h

@@ -33,3 +33,6 @@
 #define UBLOX_GNSS_SPI_CIPO 4
 #define UBLOX_GNSS_SPI_SCK 5
 ///@}
+
+#define RF24_CSN_PIN 18
+#define RF24_CE_PIN 2

include/moveControl.h

@@ -80,6 +80,12 @@ class MoveControl {
          */
         void setDrivingStatus(DrivingStatus status);
 
+        /**
+         * @brief Stops the engine immediately
+         * 
+         */
+        void emergencyStop();
+
         /**
          * @brief Set the target speed
          * 
@@ -156,6 +162,8 @@ class MoveControl {
         void setDelay(uint8_t delay) { this->delay = delay; }
 
     private:
+        void setSpeedometerDirection(Speedometer *speedometer, double value);
+
         /**
          * @brief Converts values into wheel speeds
          * 

include/moveControlConfig.h

@@ -18,7 +18,7 @@
     #define M_DIR_21 23
     #define M_DIR_22 14
     #define M_PWM_2 22
-    #define M_ENCODE_RIGHT 12
+    #define M_ENCODE_RIGHT 33
 
 // PID config
     #define PID_OUT_MIN -100
@@ -39,4 +39,4 @@
     
 // SPEEDOMETER
     #define WHEEL_DIAMETER 0.1263
-    #define ENC_STEPS 2048
+    #define ENC_STEPS 360

include/network.h

@@ -15,9 +15,13 @@
 #include <iostream>
 #include <WiFi.h>
 #include <PubSubClient.h>
+#include <esp_now.h>
 
 #include "networkConfig.h"
 
+typedef void (*recieveCallbackPtr) (const uint8_t * mac, const uint8_t *incomingData, int len); 
+typedef void (*sendCallbackPtr) (const uint8_t *mac_addr, esp_now_send_status_t status);
+
 /**
  * @brief This class handles all network stuff
  * 
@@ -39,6 +43,7 @@ class Network {
          * @return false Timeout
          */
         static bool connectWifi();
+        static bool connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send);
 
         /**
          * @brief Set all general settings to connect to a broker.
@@ -71,6 +76,7 @@ class Network {
          * @return PubSubClient* 
          */
         static PubSubClient* getMqttClient();
+        static const uint8_t * getBroadcastAddress() { return broadcastAddress; }
 
     private:
         /**
@@ -80,6 +86,10 @@ class Network {
          * @return false if the connect attemp was unsuccessful
          */
         static bool connectMQTT();
+        static int32_t getWiFiChannel(const char *ssid);
+
+        static esp_now_peer_info_t peerInfo;
+        static uint8_t broadcastAddress[6];
 
         static IPAddress local_IP;
         static IPAddress gateway;

include/networkConfig.h

@@ -30,6 +30,7 @@
  */
 #define WLAN_CONNECT_LOOP_TIME 500
 #define WLAN_DNS_SERVER     "8.8.8.8"
+#define ESP_NOW_CONTROLER_MAC {0xC8, 0xC9, 0xA3, 0xC8, 0x57, 0x10}
 
 #define NTRIP_RTK2GO
 

lib/Battery/battery.cpp

@@ -33,6 +33,12 @@ double Battery::getBatteryVoltage() {
     return (int)(res*100+0.5)/100.0;
 }
 
+bool Battery::isBatteryLow(uint8_t percent) {
+    if (this->getBatteryPercent() <= percent && this->batteryVoltage > this->absurdLowVoltage)
+        return true;
+    return false;
+}
+
 double Battery::readInputVoltage() {
     // Reference voltage is 3v3 so maximum reading is 3v3 = 4095 in range 0 to 4095
     double reading = analogRead(this->pin);

lib/Battery/battery.h

@@ -73,13 +73,28 @@ class Battery {
          */
         uint8_t getBatteryPercent()   {return this->batteryPercent;}
 
+        /**
+         * @brief Checks if the battery is low.
+         * 
+         * The function will also return false if the battery voltage is
+         * absurd low. This is for the case that the uController is powered
+         * by usb and no battery is connected.
+         * 
+         * @param percent the limit the battery have to
+         * @return true if the battery is low
+         * @return false if the battery is high
+         */
+        bool isBatteryLow(uint8_t percent);
+
     private:
         double readInputVoltage();
         void calculateBatteryVoltage();
         void calculateBatteryPercent();
 
+        uint8_t absurdLowVoltage = 5;
         uint8_t pin;
         uint8_t batteryPercent;
+        uint8_t batteryLowPercent = 10;
         uint16_t delay = 20000;
         uint32_t r1;
         uint32_t r2;

lib/ControllPad/controlPad.cpp

@@ -0,0 +1,80 @@
+/**
+ * @file controlPad.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-03-30
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#include "controlPad.h"
+
+ControlPad::ControlPad() {
+
+}
+
+bool ControlPad::loop() {
+    if (millis() - this->lastLoopMillis < this->loopDelay)
+        return false;
+    this->lastLoopMillis = millis();
+
+    if(!this->connected)
+        return false;
+
+    if (millis() - this->lastMessageReceive > this->disconnectTime) {
+        this->connected = false;
+        this->controlInput.buttons = 0;
+        this->controlInput.x = 127;
+        this->controlInput.y = 127;
+        return false;
+    }
+
+    if (this->lastButtons != controlInput.buttons)
+        this->updated = true;
+
+    if (this->menuControl && this->controlInput.buttons > 0 && this->updated) {
+        if (this->firstButtonPress) {
+            this->menuControl->printMenu();
+            this->firstButtonPress = false;
+            std::cout << "ControlPad::loop - First menu print" << std::endl;
+            return true;
+        }
+
+        if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Left))
+            this->menuControl->left();
+        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Right))
+            this->menuControl->right();
+        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Up))
+            this->menuControl->up();
+        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Down))
+            this->menuControl->down();
+        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::Yes))
+            this->menuControl->yes();
+        else if (ControlPadButton::isControlPadButtonPressed(&this->controlInput, ControlPadButton::PadButton::No))
+            this->menuControl->no();
+
+        this->updated = false;
+        this->lastButtons = controlInput.buttons;
+    }
+    return true;
+}
+
+void ControlPad::insertData(const uint8_t *data) {
+    this->connected = true;
+    this->lastMessageReceive = millis();
+
+    uint8_t lastCount = this->controlInput.counter + 1;
+    memcpy(&(this->controlInput), data, sizeof(this->controlInput));
+    if (lastCount != this->controlInput.counter)
+        std::cout << "ControlPad::insertData counter wrong value" << std::endl;
+
+    if (this->controlInput.x > 127 - this->deadZoneX 
+        && this->controlInput.x < 127 + this->deadZoneX)
+        this->controlInput.x = 127;
+
+    if (this->controlInput.y > 127 - this->deadZoneY 
+        && this->controlInput.y < 127 + this->deadZoneY)
+        this->controlInput.y = 127;
+}

lib/ControllPad/controlPad.h

@@ -0,0 +1,50 @@
+/**
+ * @file controlPad.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-03-30
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#pragma once
+
+#include <menuControl.h>
+
+#include "controlPadInput.h"
+
+class ControlPad {
+    public:
+        ControlPad();
+
+        bool loop();
+
+        void insertData(const uint8_t *data);
+
+        void setMenuControl(MenuControl* menuControl) { this->menuControl = menuControl; }
+
+        const ControlPadInput * getControlPadDataPtr() const { return &this->controlInput; }
+
+        bool isControlPadConnected() const { return this->connected; }
+
+    private:
+        MenuControl* menuControl = nullptr;
+        ControlPadInput controlInput;
+
+        bool connected = false;
+        bool updated = false;
+        bool firstButtonPress = true;
+
+        uint8_t deadZoneX = 20;
+        uint8_t deadZoneY = 20;
+        uint8_t lastButtons = 0;
+
+        uint16_t disconnectTime = 100;
+        uint16_t loopDelay = 5;
+
+        uint32_t lastMessageReceive = 0;
+        uint32_t lastLoopMillis = 0;
+
+};

lib/ControllPad/controlPadInput.h

@@ -0,0 +1,74 @@
+/**
+ * @file controlPadInput.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief 
+ * @version 0.1
+ * @date 2023-03-30
+ * 
+ * @copyright Copyright (c) 2023
+ * 
+ */
+
+#ifndef CONTROL_PAD_INPUT_H
+#define CONTROL_PAD_INPUT_H
+
+#include <stdint.h>
+
+struct ControlPadInput {
+    uint8_t buttons;
+    uint8_t x;
+    uint8_t y;
+    uint8_t counter;
+};
+
+
+class ControlPadButton {
+    public:
+        enum PadButton {
+            Left = 4,
+            Right = 8,
+            Up = 2,
+            Down = 1,
+            Yes = 32,
+            No = 16,
+            Action = 64
+        };
+
+        static bool isControlPadButtonPressed(const ControlPadInput *input, PadButton button) {
+            uint8_t buttonNum = input->buttons;
+            switch (button) {
+                case PadButton::Left :
+                    return buttonNum & (uint8_t) PadButton::Left;
+                    break;
+
+                case PadButton::Right :
+                    return buttonNum & (uint8_t) PadButton::Right;
+                    break;
+
+                case PadButton::Up :
+                    return buttonNum & (uint8_t) PadButton::Up;
+                    break;
+
+                case PadButton::Down :
+                    return buttonNum & (uint8_t) PadButton::Down;
+                    break;
+
+                case PadButton::Yes :
+                    return buttonNum & (uint8_t) PadButton::Yes;
+                    break;
+
+                case PadButton::No :
+                    return buttonNum & (uint8_t) PadButton::No;
+                    break;
+
+                case PadButton::Action :
+                    return buttonNum & (uint8_t) PadButton::Action;
+                    break;
+                
+                default:
+                    return false;;
+            }
+        }
+};
+
+#endif // CONTROL_PAD_INPUT_H

lib/Counter/counter.cpp

@@ -0,0 +1,56 @@
+#include "counter.h"
+#include <inttypes.h>
+
+uint8_t Counter::amountOfCounter = 0;
+
+Counter::Counter(uint8_t pin) {
+    this->pulsePin = pin;
+
+    this->unit = static_cast<pcnt_unit_t>(Counter::amountOfCounter);
+    if (Counter::amountOfCounter < 7)
+        Counter::amountOfCounter++;
+
+    pcnt_config_t config;
+    config.unit = this->unit;
+    config.channel = PCNT_CHANNEL_0;
+    config.counter_h_lim = COUNTER_HIGH_LIMIT;
+    config.counter_l_lim = COUNTER_LOW_LIMIT;
+    config.ctrl_gpio_num = PCNT_PIN_NOT_USED;
+    config.hctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP;
+    config.lctrl_mode = PCNT_CHANNEL_LEVEL_ACTION_KEEP;
+    config.pulse_gpio_num = this->pulsePin;
+    config.pos_mode = PCNT_CHANNEL_EDGE_ACTION_INCREASE;
+    config.neg_mode = PCNT_CHANNEL_EDGE_ACTION_HOLD;
+    pcnt_unit_config(&config);
+}
+
+void Counter::pause() {
+    pcnt_counter_pause(this->unit);
+}
+
+void Counter::resume() {
+    pcnt_counter_resume(this->unit);
+}
+
+void Counter::clear() {
+    pcnt_counter_clear(this->unit);
+}
+
+int16_t Counter::getValue() {
+    int16_t res;
+    pcnt_get_counter_value(this->unit, &res);
+    return res;
+}
+
+void Counter::setFilterValue(uint16_t value) {
+    pcnt_set_filter_value(this->unit, value);
+    this->filterEnable();
+}
+
+void Counter::filterEnable() {
+    pcnt_filter_enable(this->unit);
+}
+
+void Counter::filterDisable() {
+    pcnt_filter_disable(this->unit);
+}

lib/Counter/counter.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include <Arduino.h>
+#include <driver/pcnt.h>
+
+#define COUNTER_HIGH_LIMIT INT16_MAX
+#define COUNTER_LOW_LIMIT 0
+
+class Counter {
+    public:
+        Counter(uint8_t pin);
+
+        void pause();
+        void resume();
+        void clear();
+
+        int16_t getValue();
+
+        void setFilterValue(uint16_t value);
+        void filterEnable();
+        void filterDisable();
+
+    private:
+        static uint8_t amountOfCounter;
+
+        bool initalised = false;
+
+        uint8_t pulsePin;
+        pcnt_unit_t unit;
+};

lib/LcdWrapper/LcdWrapper.cpp

@@ -18,14 +18,19 @@ LcdWrapper::LcdWrapper(LiquidCrystal_I2C* lcd) {
 }
 
 void LcdWrapper::loop() {
-    if (this->changed) {
-        this->lcd->clear();
-        for (uint8_t i = 0; i < DISPLAY_WRAPPER_LINES; i++) {
-            this->lcd->setCursor(0, i);
-            this->lcd->print(this->data[i]);
-        }
-        this->changed = false;
+    if (!this->changed)
+        return;
+
+    this->lcd->clear();
+    for (uint8_t i = 0; i < DISPLAY_WRAPPER_LINES; i++) {
+        this->lcd->setCursor(0, i);
+        this->lcd->print(this->data[i]);
     }
+    
+    if (this->callback)
+        this->callback(this->data, DISPLAY_WRAPPER_LINES, DISPLAY_WRAPPER_ROWS);
+
+    this->changed = false;
 }
 
 void LcdWrapper::clear() {

lib/LcdWrapper/LcdWrapper.h

@@ -19,6 +19,8 @@
 #define DISPLAY_WRAPPER_ROWS 16
 #define DISPLAY_WRAPPER_LINES 2
 
+typedef void (*LcdWrapperCallback) (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows);
+
 /**
  * @brief A class for the Menu class to print information 
  * 
@@ -54,6 +56,7 @@ class LcdWrapper : public DisplayWrapper {
          * @param line 
          */
         void setCursor(uint8_t row, uint8_t line) override;
+        void setCallback(LcdWrapperCallback callback) { this->callback = callback; }
 
         /**
          * @brief Save the data to be printed
@@ -65,6 +68,7 @@ class LcdWrapper : public DisplayWrapper {
 
     private:
         LiquidCrystal_I2C* lcd;
+        LcdWrapperCallback callback = nullptr;
         char data[DISPLAY_WRAPPER_LINES][DISPLAY_WRAPPER_ROWS];
 
         uint8_t cursorRow = 0;

lib/Speedometer/speedometer.cpp

@@ -30,59 +30,59 @@ uint16_t Speedometer::loop() {
         return -1;
 
     uint32_t time = millis();
-    uint16_t elapsed_time = time - this->last_millis_loop;
+    uint16_t elapsedTime = time - this->lastMillisLoop;
 
     //Cancel if delayLoop is not reached
-    if (elapsed_time < this->delayLoop)
-        return elapsed_time;
+    if (elapsedTime < this->delayLoop)
+        return elapsedTime;
     
     runSpeedometer();
-    this->last_millis_loop = time;
-    return elapsed_time;
+    this->lastMillisLoop = time;
+    return elapsedTime;
 }
 
 void Speedometer::runSpeedometer() {
     uint32_t time = millis();
 
-    uint16_t elapsed_time = time - last_millis_calc;
-    last_millis_calc = time;
-
-    int16_t count = this->pulseCounter->get_value();
-    switch (this->currentDirection) {
-        case Direction::Forward :
-            this->addValToBuf(count);
-            break;
-
-        case Direction::Backward :
-            this->addValToBuf(-count);
-            break;
-
-        case Direction::None :
-            this->addValToBuf(0);
-            break;
-        
-        default:
-            std::cout << "Wrong value in Speedometer::runSpeedometer" << std::endl;
-            break;
-    }
+    uint16_t elapsedTime = time - this->lastMillisCalc;
+    this->lastMillisCalc = time;
 
+    this->addValToBuf(this->pulseCounter->getValue());
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 
-    uint16_t count_abs = abs(this->calcAverage());          // Absolute time in milliseconds
-    double n = (double)count_abs / steps;                   // Wheel revolutions in absolute time
-    double u = (double)n / ((double)elapsed_time / 1000);   // Wheel revolutions per second
-    double ms = u * (diameter * PI);                        // Speed in m/s
+    
+    double n = (double)this->calcAverage() / this->steps;   // Wheel revolutions in absolute time
+    double u = n / ((double)elapsedTime / 1000);           // Wheel revolutions per second
+    double ms = u * (diameter * PI);                      // Speed in m/s
 
-    if (count > 0) {
-        this->speed = ms;
-    } else if (count < 0) {
-        this->speed = ms * (-1);
-    } else {
-        this->speed = 0;
+    switch (this->currentDirection) {
+        case Direction::Forward :
+            this->speed = ms;
+            break;
+        
+        case Direction::Backward :
+            this->speed = -ms;
+            break;
+
+        case Direction::None :
+            this->speed = 0;
+            break;
     }
 
-    // std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
+    // if (this->speed)
+    //     this->printCounter++;
+    // if (this->printCounter > 10) {
+    //     this->printCounter = 0;
+    //     std::cout << "Speedometer::runSpeedometer speed: " << (int) this->speed << std::endl;
+    // }
+}
+
+void Speedometer::setDirection(Direction dir) {
+    if (this->currentDirection == dir)
+        return;
+    this->currentDirection = dir;
+    this->clearAvgBuf();
 }
 
 void Speedometer::setNumOfValForAvg(uint8_t val) {
@@ -93,20 +93,23 @@ void Speedometer::setNumOfValForAvg(uint8_t val) {
 void Speedometer::setEncFilter(uint16_t val) {
     if (val > 1023) 
         val = 1023;
-    this->pulseCounter->set_filter_value(val);
+    this->pulseCounter->setFilterValue(val);
 }
 
 void Speedometer::calibrationMeasurementStart() {
+    std::cout << "Start" << std::endl;
     this->calibrationRunning = true;
     this->pulseCounter->clear();
     this->pulseCounter->resume();
 }
 
 uint16_t Speedometer::calibrationMeasurementStop() {
+    std::cout << "Ende" << std::endl;
     this->calibrationRunning = false;
-    uint16_t res = abs(this->pulseCounter->get_value());
+    uint16_t res = abs(this->pulseCounter->getValue());
     this->pulseCounter->clear();
     this->pulseCounter->resume();
+    std::cout << "Result: " << res << std::endl;
     return res;
 }
 
@@ -114,10 +117,8 @@ void Speedometer::init(uint8_t pin, double diameter, uint16_t steps) {
     this->diameter = diameter;
     this->steps = steps;
 
-    this->pulseCounter = new PulseCounter();
-    this->pulseCounter->initialise(pin, PCNT_PIN_NOT_USED);
-    this->pulseCounter->set_mode(PCNT_COUNT_INC, PCNT_COUNT_DIS, PCNT_MODE_KEEP, PCNT_MODE_KEEP);
-    this->pulseCounter->set_filter_value(1000); // ignore pulses less than 1000 x 2.5ns
+    this->pulseCounter = new Counter(pin);
+    this->pulseCounter->setFilterValue(1000); // ignore pulses less than 1000 x 2.5ns
 
     this->pulseCounter->clear();
     this->pulseCounter->resume();
@@ -131,12 +132,17 @@ void Speedometer::initAvgBuf() {
         this->buf[i] = 0;
 }
 
+void Speedometer::clearAvgBuf() {
+    for (uint8_t i = 0; i < bufSize; i++) 
+        this->buf[i] = 0;
+}
+
 void Speedometer::addValToBuf(int16_t val) {
     this->buf[this->bufPos] = val;
     this->bufPos++;
 
     if (bufPos == bufSize)
-        bufPos = 0;
+       bufPos = 0;
 }
 
 void Speedometer::updateAvgBufSize() {

lib/Speedometer/speedometer.h

@@ -15,7 +15,8 @@
 #include <Arduino.h>
 #include <cstdint>
 #include <iostream>
-#include <esp32_pcnt.h>
+
+#include "counter.h"
 
 /**
  * @brief The default size of numbers to be taken in account for the average.
@@ -88,7 +89,7 @@ class Speedometer {
          * 
          * @param dir Direction
          */
-        void setDirection(Direction dir) { this->currentDirection = dir; }
+        void setDirection(Direction dir);
 
         /**
          * @brief Set the number of last values ​​to be taken into account for the average.
@@ -149,11 +150,12 @@ class Speedometer {
     private:
         void init(uint8_t pin, double diameter, uint16_t steps);
         void initAvgBuf();
+        void clearAvgBuf();
         void addValToBuf(int16_t val);
         void updateAvgBufSize();
         int16_t calcAverage();
 
-        PulseCounter* pulseCounter;
+        Counter* pulseCounter;
         Direction currentDirection = Direction::None;
 
         bool calibrationRunning = false;
@@ -161,6 +163,7 @@ class Speedometer {
         double speed = 0;
         double diameter;
 
+        uint8_t printCounter = 0;
         uint8_t bufSize = BUFSIZE;
         uint8_t delayLoop = DELAY_SPEEDOMETER;
         uint8_t bufPos = 0;
@@ -168,8 +171,8 @@ class Speedometer {
 
         int16_t *buf = nullptr;
 
-        uint32_t last_millis_loop = 0;
-        uint32_t last_millis_calc = 0;
+        uint32_t lastMillisLoop = 0;
+        uint32_t lastMillisCalc = 0;
 };
 
 #endif // SPEEDOMETER_H

platformio.ini

@@ -17,25 +17,22 @@ monitor_speed = 115200
 upload_speed = 921600
 monitor_port = COM3
 lib_deps = 
-	jvpernis/PS3 Controller Host@^1.1.0
 	sparkfun/SparkFun u-blox GNSS Arduino Library@^2.2.7
 	knolleary/PubSubClient@^2.8
 	br3ttb/PID@^1.2.1
 	marcoschwartz/LiquidCrystal_I2C@^1.1.4
-	marian-craciunescu/ESP32Ping@^1.7
 	bblanchon/ArduinoJson@^6.20.0
 	mprograms/QMC5883LCompass@^1.1.1
-	mike-gofton/ESP32PulseCounter@^0.2.0
 	https://git.kleiax.de/PlatformIO-Libs/Menu.git#v1.0
+	nrf24/RF24@^1.4.5
 upload_port = COM3
 
 [env:release]
 build_type = release
-lib_deps = mike-gofton/ESP32PulseCounter@^0.2.0
 
 [env:debug]
-monitor_filters = esp32_exception_decoder
 build_type = debug
+monitor_filters = esp32_exception_decoder
 check_tool = clangtidy
 
 [platformio]

src/SpecialMenus/Systeminformation/menuSysteminformatio.cpp

@@ -11,10 +11,9 @@
 
 #include "menuSysteminformation.h"
 
-MenuSysteminformation::MenuSysteminformation(Battery* mainBattery, Ps3Controller* gamepad) 
+MenuSysteminformation::MenuSysteminformation(Battery* mainBattery) 
             : MenuInformationSites(5) {
     this->mainBattery = mainBattery;
-    this->gamepad = gamepad;
 }
 
 void MenuSysteminformation::printPage() const {
@@ -44,11 +43,11 @@ void MenuSysteminformation::printPage() const {
             lineTwo.concat(this->mainBattery->getBatteryVoltage());
             break;
 
-        case 4:
-            lineOne = "PS3 battery";
-            lineTwo = "is ";
-            lineTwo.concat(MenuSysteminformation::ps3ToString(this->gamepad->data.status.battery));
-            break;
+        // case 4:
+        //     lineOne = "PS3 battery";
+        //     lineTwo = "is ";
+        //     lineTwo.concat(MenuSysteminformation::ps3ToString(this->gamepad->data.status.battery));
+        //     break;
 
         default:
             this->printDefault();
@@ -58,12 +57,3 @@ void MenuSysteminformation::printPage() const {
     this->print(lineOne, lineTwo);
 }
 
-String MenuSysteminformation::ps3ToString(uint8_t battery) {
-    if( battery == ps3_status_battery_charging )      return "LOAD";
-    else if( battery == ps3_status_battery_full )     return "FULL";
-    else if( battery == ps3_status_battery_high )     return "HIGH";
-    else if( battery == ps3_status_battery_low)       return "LOW";
-    else if( battery == ps3_status_battery_dying )    return "BAD";
-    else if( battery == ps3_status_battery_shutdown ) return "OFF";
-    else                                              return "UNDEF";
-}

src/SpecialMenus/Systeminformation/menuSysteminformation.h

@@ -13,8 +13,8 @@
 #define MENU_SYSTEMINFORMATION_H
 
 #include <Arduino.h>
-#include <Ps3Controller.h>
 
+#include "controlPadInput.h"
 #include "menuInformationSites.h"
 #include "battery.h"
 
@@ -31,14 +31,12 @@ class MenuSysteminformation : public MenuInformationSites {
          * @param mainBattery 
          * @param gamepad 
          */
-        MenuSysteminformation(Battery* mainBattery, Ps3Controller* gamepad);
+        MenuSysteminformation(Battery* mainBattery);
 
     private:
         void printPage() const override;
-        static String ps3ToString(uint8_t battery);
 
         Battery* mainBattery;
-        Ps3Controller* gamepad;
 };
 
 #endif // MENU_SYSTEMINFORMATION_H

src/SpecialMenus/driveModi/TestMode/menuTestMode.cpp

@@ -78,7 +78,7 @@ void MenuTestMode::init() {
     this->driveManager->changeModus(Modi::TestMode);
     this->testMode = (TestMode*) this->driveManager->getDriveModiPtr();
 
-    testMode->setSpeed(15);
+    testMode->setSpeed(50);
     testMode->setRotationSpeed(100);
 
 
@@ -93,7 +93,6 @@ void MenuTestMode::init() {
     Menu* encoderMenu = new Menu;
     // Sound
     // Ping google
-    // WheelEncoder
 
     MenuIntInput* drivingMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::drive));
     MenuIntInput* engineLeftMenu = new MenuIntInput(2, new MenuTestModeWrapper(this, &TestMode::leftEngine));

src/SpecialMenus/driveModi/TestMode/speedometerTest.cpp

@@ -13,6 +13,7 @@
 
 SpeedometerTest::SpeedometerTest(Speedometer* speedometer) {
     this->speedometer = speedometer;
+    this->speedometer->setDirection(Speedometer::Forward);
 }
 
 

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -11,8 +11,8 @@
 
 #include "driveModi/Modi/Autopilot/autopilot.h"
 
-Autopilot::Autopilot(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation)
-    : ManualControl(moveControl, gamepad) {
+Autopilot::Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation)
+    : ManualControl(moveControl, input) {
         this->navigation = navigation;
         this->navigationStarted = this->navigation->startNavigation();
         this->routeInfo = this->navigation->getRouteInfo();
@@ -45,7 +45,7 @@ void Autopilot::runAutopilot() {
         this->selfDrivingAvailable = true;
 
     if (!this->selfDriving && this->selfDrivingAvailable)
-        this->setSelfDriving(this->gamepad->data.button.triangle);
+        this->setSelfDriving(ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action));
 
     if (this->routeInfo.currentPoint == this->routeInfo.totalPoints) {
         this->navigationEnded = true;

src/driveModi/Modi/Autopilot/autopilot.h

@@ -35,7 +35,7 @@ class Autopilot : public ManualControl {
          * @param moveControl for ManualControl
          * @param navigation for route instructions
          */
-        Autopilot(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation);
+        Autopilot(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
 
         /**
          * @brief Calls runAutopilot or ManualControl loop
@@ -50,7 +50,7 @@ class Autopilot : public ManualControl {
         void loop();
 
         /**
-         * @brief Manges the autoipilot
+         * @brief Manges the autopilot
          * 
          * If the first Point is near to current location you can turn
          * the autopilot on.

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -11,8 +11,8 @@
 
 #include "driveModi/Modi/CaptureRoute/captureRoute.h"
 
-CaptureRoute::CaptureRoute(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation) 
-    : ManualControl(moveControl, gamepad) {
+CaptureRoute::CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation) 
+    : ManualControl(moveControl, input) {
         this->navigation = navigation;
         this->routeInfo = navigation->getRouteInfo();
 }
@@ -28,7 +28,7 @@ void CaptureRoute::loop() {
 }
 
 void CaptureRoute::runCaptureRoute() {
-    if (this->gamepad->data.button.triangle) {
+    if (ControlPadButton::isControlPadButtonPressed(this->input, ControlPadButton::PadButton::Action)) {
         if (this->navigation->addCurrentPosToRoute()) {
             this->routeInfo = navigation->getRouteInfo();
             this->updateDisplay = true;

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -33,7 +33,7 @@ class CaptureRoute : public ManualControl {
          * @param moveControl for ManualControl
          * @param navigation to add Points
          */
-        CaptureRoute(MoveControl* moveControl, Ps3Controller *gamepad, Navigation* navigation);
+        CaptureRoute(MoveControl* moveControl, const ControlPadInput *input, Navigation* navigation);
 
         /**
          * @brief Calls runCaptureRoute and ManualControl::loop

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -10,9 +10,9 @@
  */
 #include "manualControl.h"
 
-ManualControl::ManualControl(MoveControl *moveControl, Ps3Controller *gamepad) { 
+ManualControl::ManualControl(MoveControl *moveControl, const ControlPadInput *input) { 
     this->moveControl = moveControl;
-    this->gamepad = gamepad;
+    this->input = input;
     this->moveControl->setDrivingStatus(DrivingStatus::drive);
 }
 
@@ -31,12 +31,18 @@ void ManualControl::loop() {
 }
 
 void ManualControl::runManualControl() {
-    int8_t x = this->gamepad->data.analog.stick.lx;
-    int8_t y = this->gamepad->data.analog.stick.ly;
+    // 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;
+
+    // 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;
+    // }
 
     double value_per_step = this->max_speed * 2 / 256;
-    this->moveControl->setSpeed((y * -1) * value_per_step);
+    this->moveControl->setSpeed(-y * value_per_step);
 
     value_per_step = this->max_rotation * 2 / 256;
-    this->moveControl->setRotationSpeed(-x * value_per_step);
+    this->moveControl->setRotationSpeed(x * value_per_step);
 }

src/driveModi/Modi/ManualControl/manualControl.h

@@ -13,6 +13,7 @@
 
 #include "moveControl.h"
 #include "driveModi/driveModi.h"
+#include "controlPadInput.h"
 
 /**
  * @brief Drive the Rover with a Joystick
@@ -24,7 +25,7 @@
  */
 class ManualControl : public DriveModi {
     public:
-        ManualControl(MoveControl *moveControl, Ps3Controller *gamepad);
+        ManualControl(MoveControl *moveControl, const ControlPadInput *input);
 
         /**
          * @brief Destroy the Manual Control object
@@ -71,7 +72,7 @@ class ManualControl : public DriveModi {
 
     protected:
         MoveControl *moveControl;
-        Ps3Controller *gamepad;
+        const ControlPadInput* input;
 
     private:
         uint32_t lastMillis = 0;

src/driveModi/Modi/TestMode/testMode.cpp

@@ -79,7 +79,7 @@ bool TestMode::drive(int16_t cm, int16_t degree) {
         else if (cm > 0)
             this->moveControl->setSpeed(this->speed);
         
-        this->maneuverTime = (uint32_t) (((double) cm / 100.0) / this->speed) * 1000;
+        this->maneuverTime = (uint32_t) (((double) abs(cm) / 100.0) / this->speed) * 1000;
         this->busy = true;
         this->maneuver = Maneuver::Drive;
         return true;

src/driveModi/driveManager.cpp

@@ -15,12 +15,11 @@ Modi& operator++(Modi& m, int) {
     return m = (m == Modi::TestMode) ? Modi::Off : static_cast<Modi>(static_cast<int>(m)+1);
 }
 
-DriveManager::DriveManager(MoveControl *moveControl, Ps3Controller *gamepad, bool wifi) {
+DriveManager::DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi) {
     this->moveControl = moveControl;
-    this->gamepad = gamepad;
+    this->input = input;
 
-    this->spiPort = new SPIClass(HSPI);
-    spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS);
+    this->spiPort = spiPort;
     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);
@@ -66,17 +65,17 @@ void DriveManager::changeModus(Modi modus) {
             break;
 
         case Modi::ManualControl: {
-            this->currentModusPtr = new ManualControl(this->moveControl, this->gamepad);
+            this->currentModusPtr = new ManualControl(this->moveControl, this->input);
             }
             break;
 
         case Modi::CaptureRoute: {
-            this->currentModusPtr = new CaptureRoute(this->moveControl, this->gamepad,  this->navigation);
+            this->currentModusPtr = new CaptureRoute(this->moveControl, this->input,  this->navigation);
             }
             break;
 
         case Modi::Autopilot: {
-            this->currentModusPtr = new Autopilot(this->moveControl, this->gamepad, this->navigation);
+            this->currentModusPtr = new Autopilot(this->moveControl, this->input, this->navigation);
             }
             break;
 

src/driveModi/driveManager.h

@@ -21,6 +21,7 @@
 #include "config.h"
 #include "networkConfig.h"
 #include "debugTimes.h"
+#include "controlPadInput.h"
 
 // All Drive Modi
 #include "driveModi/Modi/ManualControl/manualControl.h"
@@ -51,7 +52,7 @@ class DriveManager {
          * 
          * @param moveControl 
          */
-        DriveManager(MoveControl *moveControl, Ps3Controller *gamepad, bool wifi = false);
+        DriveManager(MoveControl *moveControl, SPIClass *spiPort, const ControlPadInput *input, bool wifi = false);
 
         /**
          * @brief Destroy the Drive Manager object
@@ -117,7 +118,7 @@ class DriveManager {
     private:
         Modi currentModus = Modi::Off;
         MoveControl *moveControl;
-        Ps3Controller *gamepad;
+        const ControlPadInput* input;
         DriveModi *currentModusPtr = nullptr;
         Navigation* navigation;
         SPIClass* spiPort;

src/driveModi/driveModi.h

@@ -12,8 +12,6 @@
 #ifndef DRIVEMODI_H
 #define DRIVEMODI_H
 
-#include <Ps3Controller.h>
-
 /**
  * @brief Baseclass to build DriveModi
  * 

src/main.cpp

@@ -17,13 +17,10 @@
 
 #include "config.h"
 
-#include <Ps3Controller.h>
 #include <iostream>
 #include <Wire.h>
 #include <LiquidCrystal_I2C.h>
-#include <ESP32Ping.h>
-#include <BluetoothSerial.h>
-#include <esp_bt_main.h>
+#include <SPI.h>
 
 #include "driveModi/driveManager.h"
 #include "OutputBuf/outputBuf.h"
@@ -33,6 +30,7 @@
 #include "debugMqtt.h"
 #include "battery.h"
 #include "debugTimes.h"
+#include "controlPad.h"
 
 #include "menu.h"
 #include "menuAction.h"
@@ -53,21 +51,17 @@ LcdWrapper* lcdWrapper;
 OutputBuf* outputBuf;
 DebugMqtt* debugMqtt = nullptr;
 Battery* mainBattery;
-Ps3Controller* gamepad;
-BluetoothSerial SerialBT;
+SPIClass* spiPort;
+ControlPad* controlPad;
 
 bool wifiIsActive;
-bool disconnectPs3 = false;
 
-
-void configureController(void);
-void callbackControllerAction(void);
-void callbackControllerConnect(void);
-void callbackControllerDisconnect(void);
 void i2cScanner(void);
 void makeMenu(void);
 void restart(void);
-void disconnectController(void);
+void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len); 
+void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status);
+void lcdWrapperCallback (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows);
 
 
 void setup() {
@@ -76,7 +70,15 @@ void setup() {
     DebugTimes setupTime;
     char wifiIndicator = 'X';
 
+    WiFi.mode(WIFI_MODE_STA);
+    Serial.print("MAC Address: ");
+    Serial.println(WiFi.macAddress());
+
+    spiPort = new SPIClass(HSPI);
+    spiPort->begin(UBLOX_GNSS_SPI_SCK, UBLOX_GNSS_SPI_CIPO, UBLOX_GNSS_SPI_COPI, UBLOX_GNSS_SPI_CS);
+
     mainBattery = new Battery(35, 692000, 218500);
+    controlPad = new ControlPad();
 
     Wire.begin(21, 19);
     Wire.setClock(400000);
@@ -90,14 +92,6 @@ void setup() {
     Network::setupMQTT();
     wifiIsActive = Network::connectWifi();
     if (wifiIsActive) {
-        
-        // const char* remoteHost = "www.google.com";
-        // std::cout << "Pinging host: " << remoteHost << std::endl;
-        // if (Ping.ping(remoteHost))
-        //     std::cout << "Ping successful..." << std::endl;
-        // else
-        //     std::cout << "Ping failed. :/" << std::endl;
-
         if (Network::checkMQTT()) {
             DebugMqtt::init(Network::getMqttClient(), Loglevel::debug);
             std::cout << "Activate additional output via MQTT..." << std::endl;
@@ -106,17 +100,16 @@ void setup() {
         } else
             outputBuf = new OutputBuf();
         wifiIndicator = '-';
-    } else {
+    } else
         outputBuf = new OutputBuf();
-    }
     std::cout.rdbuf(outputBuf);
 
+    Network::connectEspNow(receiveCallback, sendCallback);
+
     std::cout << "Welcome to Kleiax-Rover" << std::endl;
     std::cout << "All actions from the main program run on Core -> " << xPortGetCoreID() << std::endl;
 
-    gamepad = new Ps3Controller;
-    driveManager = new DriveManager(&moveController, gamepad, wifiIsActive);
-    configureController();
+    driveManager = new DriveManager(&moveController, spiPort, controlPad->getControlPadDataPtr(), wifiIsActive);
 
     outputBuf->activateMqtt(false);
 
@@ -125,6 +118,7 @@ void setup() {
     lcd->setCursor(0, 1);
     lcd->print("Press PS-Button");
     lcdWrapper = new LcdWrapper(lcd);
+    lcdWrapper->setCallback(lcdWrapperCallback);
 
     makeMenu();
 
@@ -139,90 +133,23 @@ void loop() {
         wifiTime.stopConsol("WiFi-Time", 10);
     }
     driveManager->loop();
+    controlPad->loop();
+    main_m->update();
     lcdWrapper->loop();
 
     mainBattery->loop();
-    if (mainBattery->getBatteryPercent() <= 10) {
-        moveController.setDrivingStatus(DrivingStatus::stop);
-        moveController.setSpeed(0);
-        moveController.setRotationSpeed(0);
+    if (mainBattery->isBatteryLow(10)) {
+        moveController.emergencyStop();
+
+        uint16_t voltage = (uint16_t) (mainBattery->getBatteryVoltage() * 100);
 
         lcd->setCursor(0, 0);
-        lcd->printf("Low Battery...");
+        lcd->printf("Low Battery: %u", voltage);
         lcd->setCursor(0, 1);
         lcd->print("Please turn off.");
         
-        while (1);        
+        while (true);
     }
-
-    if (disconnectPs3) {
-        DebugTimes disconnectPS3Time;
-        std::cout << "Disconnect PS3 Controller." << std::endl;
-        gamepad->end();
-        esp_bluedroid_disable();
-        esp_bluedroid_deinit();
-        configureController();
-        disconnectPs3 = false;
-        disconnectPS3Time.stopConsol("Ps3 Disconnect");
-    }
-}
-
-void configureController() {
-    
-    gamepad->attach(callbackControllerAction);
-    gamepad->attachOnConnect(callbackControllerConnect);
-    gamepad->attachOnDisconnect(callbackControllerDisconnect);
-    std::cout << "\nReady to connect a PS3 Controller... \n";
-
-    gamepad->begin(CONTROLLER_MAC);
-}
-
-void callbackControllerAction() {
-    auto isDelayReached = [](bool reset = false) -> bool {
-        static uint32_t lastMillis = 0;
-        if (reset)
-            lastMillis = millis();
-        static const uint16_t delay = 100;
-        bool res = false;
-
-        if (millis() - lastMillis > delay)
-            res = true;    
-        return res;
-    };
-
-    // Menu controlling
-    if (isDelayReached()) {
-        if (gamepad->data.button.up)
-            main_m->up();
-        else if (gamepad->data.button.down)
-            main_m->down();
-        else if (gamepad->data.button.left)
-            main_m->left();
-        else if (gamepad->data.button.right)
-            main_m->right();
-        else if (gamepad->data.button.circle)
-            main_m->yes();
-        else if (gamepad->data.button.cross)
-            main_m->no();
-        else if (gamepad->data.button.select)
-            std::cout << "main.cpp callbackControllerAction - Here we are!" << std::endl;
-
-        isDelayReached(true);        
-    }
-    // This have to be stand here because it have to be run on the other Core
-    main_m->update();
-}
-
-void callbackControllerConnect() {
-    std::cout << "Controller connected to ESP32" << std::endl;
-    std::cout << "All actions from the Controller run on Core -> " << xPortGetCoreID() << std::endl;
-
-    main_m->printMenu();
-}
-
-void callbackControllerDisconnect() {
-    std::cout << "Controller disconnected..." << std::endl;
-    // gamepad->end();
 }
 
 void i2cScanner() {
@@ -272,7 +199,7 @@ void makeMenu() {
     MenuAutopilot* auto_m = new MenuAutopilot(driveManager);
     MenuTestMode* testM_m = new MenuTestMode(driveManager);
     MenuGPS* gps_m = new MenuGPS(driveManager);
-    MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery, gamepad);
+    MenuSysteminformation* sys_m = new MenuSysteminformation(mainBattery);
     MenuRoute* rout_m = new MenuRoute(driveManager->getNavigation()->getRoute());
 
     // Set Menus on LCD
@@ -295,7 +222,6 @@ void makeMenu() {
     MenuAction* gps_e = new MenuAction("GPS", gps_m);
     MenuAction* pid_e = new MenuAction("PID", pid_m);
     MenuAction* restart_e = new MenuAction("Restart", restart);
-    MenuAction* contr_e = new MenuAction("Remove PS3", disconnectController);
     MenuAction* sys_e = new MenuAction("Systeminfo", sys_m);
     MenuAction* rout_e = new MenuAction("Route", rout_m);
     main_m->addEntry(mode_e);
@@ -303,7 +229,6 @@ void makeMenu() {
     main_m->addEntry(rout_e);
     main_m->addEntry(pid_e);
     main_m->addEntry(sys_e);
-    main_m->addEntry(contr_e);
     main_m->addEntry(restart_e);
 
     // Entry for the mode Menu
@@ -334,6 +259,8 @@ void makeMenu() {
     pidr_m->setEntry(0, "P-Part", (uint8_t) moveController.getPID(1)->GetKp());
     pidr_m->setEntry(1, "I-Part", (uint8_t) moveController.getPID(1)->GetKi());
     pidr_m->setEntry(2, "D-Part", (uint8_t) moveController.getPID(1)->GetKd());
+
+    controlPad->setMenuControl(main_m);
 }
 
 void restart(void) {
@@ -343,11 +270,23 @@ void restart(void) {
     ESP.restart();
 }
 
-void disconnectController(void) {
-    disconnectPs3 = true;
-    lcdWrapper->clear();
-    lcdWrapper->setCursor(0, 0);
-    lcdWrapper->print("Controller is");
-    lcdWrapper->setCursor(0, 1);
-    lcdWrapper->print("disconnected.");
+void receiveCallback (const uint8_t * mac, const uint8_t *incomingData, int len) {
+    if (len != sizeof(ControlPadInput))
+        return;
+    controlPad->insertData(incomingData);
+}
+
+void sendCallback (const uint8_t *mac_addr, esp_now_send_status_t status) {
+    if (status != ESP_NOW_SEND_SUCCESS)
+        std::cout << "sendCallback - Delivery Fail" << std::endl;
+}
+
+void lcdWrapperCallback (const char data[][DISPLAY_WRAPPER_ROWS], uint8_t lines, uint8_t rows) {
+    if (!controlPad->isControlPadConnected())
+        return;
+
+    esp_err_t result = esp_now_send(Network::getBroadcastAddress(), (uint8_t *) data, lines * rows);
+   
+    if (result != ESP_OK)
+        Serial.println("Error sending the data");
 }

src/moveControl.cpp

@@ -60,6 +60,10 @@ void MoveControl::loop() {
         ||  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];
@@ -108,18 +112,32 @@ void MoveControl::setDrivingStatus(DrivingStatus status) {
     // }
 }
 
+void MoveControl::emergencyStop() {
+    this->left_motor->emergencyStop();
+    this->right_motor->emergencyStop();
+    this->setSpeed(0);
+    this->setRotationSpeed(0);
+    this->setRawPowerLeft(0);
+    this->setRawPowerRight(0);
+    this->driving_status = DrivingStatus::stop;
+}
+
 void MoveControl::setSpeed(double speed) {
-    if (speed < 0.1 && speed > -0.1)
+    if (speed < 0.1 && speed > -0.1) {
         this->x_speed = 0;
-    else
-        this->x_speed = speed;
+        return;
+    }
+
+    this->x_speed = speed;
 }
 
 void MoveControl::setRotationSpeed(double speed) {
-    if (speed < 0.1 && speed > -0.1)
+    if (speed < 0.1 && speed > -0.1){
         this->rotation_speed = 0;
-    else
-        this->rotation_speed = speed;
+        return;
+    }
+
+    this->rotation_speed = speed;
 }
 
 void MoveControl::setRawPowerLeft(int16_t power) {
@@ -151,6 +169,15 @@ PID* MoveControl::getPID(uint8_t side) {
         return nullptr;
 }
 
+void MoveControl::setSpeedometerDirection(Speedometer *speedometer, double value) {
+    if (value > 0)
+        speedometer->setDirection(Speedometer::Direction::Forward);
+    else if (value < 0)
+        speedometer->setDirection(Speedometer::Direction::Backward);
+    else
+        speedometer->setDirection(Speedometer::Direction::None);
+}
+
 void MoveControl::calcTargetWheelSpeed() {
     /* original formula:
        (1 / r) / 1  b \ / x \ = / Xl \
@@ -170,16 +197,22 @@ void MoveControl::regulateMotors() {
         case DrivingStatus::stop :
             this->left_motor->setTargetPower(0);
             this->right_motor->setTargetPower(0);
+            this->setSpeedometerDirection(this->left_speedometer, 0);
+            this->setSpeedometerDirection(this->right_speedometer, 0);
             break;
 
         case DrivingStatus::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);
             break;
 
         case DrivingStatus::raw :
             this->left_motor->setTargetPower(this->rawPowerLeft);
             this->right_motor->setTargetPower(this->rawPowerRight);
+            this->setSpeedometerDirection(this->left_speedometer, this->rawPowerLeft);
+            this->setSpeedometerDirection(this->right_speedometer, this->rawPowerRight);
             break;
 
         default:

src/network.cpp

@@ -20,6 +20,9 @@ IPAddress Network::dnsServer;
 WiFiClient Network::wifi_client;
 PubSubClient* Network::mqtt_client;
 
+esp_now_peer_info_t Network::peerInfo;
+uint8_t Network::broadcastAddress[6] = ESP_NOW_CONTROLER_MAC;
+
 void Network::setIps() {
     local_IP.fromString(WLAN_IP);
     gateway.fromString(WLAN_GATEWAY);
@@ -58,6 +61,10 @@ bool Network::connectMQTT() {
 }
 bool Network::connectWifi() {
    // Configures static IP address
+   
+    if (!WiFi.mode(WIFI_AP_STA))
+        std::cout << "Network::connectWiFi failed WiFi.mode" << std::endl;
+
     if (!WiFi.config(local_IP, gateway, subnet, dnsServer))
         std::cout << "STA Failed to configure" << std::endl;
 
@@ -81,6 +88,28 @@ bool Network::connectWifi() {
     std::cout << "WiFi connected." << std::endl;
     std::cout << "IP address: " << std::endl;
     std::cout << WLAN_IP << std::endl;
+    // std::cout << "WiFi MAC Address: " << WiFi.macAddress() << std::endl << std::endl;
+    return true;
+}
+
+bool Network::connectEspNow(recieveCallbackPtr reci, sendCallbackPtr send) {
+    if (esp_now_init() != ESP_OK) {
+        std::cout << "Network::connectEspNow - Error initializing ESP-NOW" << std::endl;
+        return false;
+    }
+
+    esp_now_register_send_cb(send);
+
+    memcpy(peerInfo.peer_addr, broadcastAddress, 6);
+    peerInfo.channel = 0;  
+    peerInfo.encrypt = false;
+
+    if (esp_now_add_peer(&peerInfo) != ESP_OK) {
+        std::cout << "Network::connectEspNow - Failed to add peer" << std::endl;
+        return false;
+    }
+
+    esp_now_register_recv_cb(reci);
     return true;
 }
 
@@ -100,6 +129,14 @@ bool Network::checkMQTT() {
     return true;
 }
 
+int32_t Network::getWiFiChannel(const char *ssid) {
+    if (int32_t n = WiFi.scanNetworks())
+        for (uint8_t i=0; i<n; i++)
+            if (!strcmp(ssid, WiFi.SSID(i).c_str()))
+                return WiFi.channel(i);
+    return 0;
+}
+
 void Network::checkWiFi() {
     static uint64_t previousMillis = 0;
     static uint16_t delay = 5000;