a lot of bullshit

doc/Notes and TODOs/TODO allgemein.txt

@@ -3,4 +3,7 @@ Meine Libs von andren Abhängigkeiten lösen
     - Alle config.h und nicht allgemeinen Header entfernen
 
 Speedometer:
-    - Mitrechnen durchschnittliche aufrufzeit
+    - Mitrechnen durchschnittliche aufrufzeit
+
+Alle loops Nachgucken ob last_millis aktualisiert wird.
+Mqtt kram auslagern

include/config.h

@@ -1,136 +1,10 @@
-// Global config file
+    //GPS Serial Pins
-// The front is where the boards are
-
-
-//Pin config
-    //Left
-    #define M_DIR_11 27
-    #define M_DIR_12 12
-    #define M_PWM_1 13
-    #define M_ENCODE_1A 33
-    #define M_ENCODE_1B 32
-    //Right
-    #define M_DIR_21 23
-    #define M_DIR_22 14
-    #define M_PWM_2 22
-    #define M_ENCODE_2A 26
-    #define M_ENCODE_2B 25
-    //GPS
     #define GPS_RX 16
     #define GPS_TX 17
+    #define GPS_BAUD 9600
 
-//Motors
-#define LEFT_MOTOR 1
-#define RIGHT_MOTOR 2
-#define BOTH_MOTOR 3
 
-//Directions
-#define STOP 0
-#define FORWARD 1
-#define BACKWARD 2
 
-//MotorControl config
-#define RUN_MOTOR_CONTROL_DELAY 10 // Normal 10
-#define PWM_FREQ 16000
-#define PWM_RES 8
-#define PWM_CHANNEL_M1 0
-#define PWM_CHANNEL_M2 1
-#define POWER_STEPS 2       //  A total of 20 levels ( 100 / SPEED_STEPS ) * RUN_MOTOR_CONTROL_DELAY = 500ms
-#define PWM_MIN 120
-#define PWM_MAX 245         // Max 98% of 2^PWM_RES
-
-//Speedometer config
-#define RUN_SPEEDOMETER_DELAY 30 // Normal 30 untested
-#define ENC_FILTER 1023 // 1023 is max
-#define ENC_STEPS 1024
-#define WHEEL_DIAMETER 0.1263 //meter
-#define BUF_SIZE 10
-
-//MoveControl config
-#define RUN_MOVE_CONTROL_DELAY 10
-#define ACCELERATE_STEPS 1
-#define PID_LEFT_P 85
-#define PID_LEFT_I 0
-#define PID_LEFT_D 0
-#define PID_RIGHT_P 75
-#define PID_RIGHT_I 0
-#define PID_RIGHT_D 0
-#define PID_OUT_MIN -100
-#define PID_OUT_MAX 100
-#define PID_SAMPLETIME 30
-
-//ManualControl config
-#define RUN_MANUALCONTROL_DELAY 10 // normaly 10
-#define MANUALCONTROL_MAX_SPEED 1.0 // m/s
-#define MANUALCONTROL_MAX_ROTATION 7.0 // rad/s
-
-//MQTT global config
-#define MQTT_BUFFER_SITE 128
-#define MQTT_TIME_RECONNECT 2500
 
 // PS3 Controller
 // ESP32 MAC BL 24:62:AB:F2:4B:3A
-
-#define RHEDE
-
-//Network config RHEDE
-#ifdef RHEDE
-#define WLAN_SSID "LebennigHuus"
-#define WLAN_PASSWORD "Punica-699"
-#define WLAN_IP 0x040BA8C0          //192.168.11.4
-#define WLAN_SUBNETMASK 0x0080FFFF  //255.255.128.0
-#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
-#define MQTT_SERVER 0x0701A8C0      //192.168.1.7
-#define MQTT_PORT 1883
-#define MQTT_AUTH
-#define MQTT_USER "kleiax"
-#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
-#define NTP_SERVER 0x0100a8c0       //192.168.0.1
-#endif //RHEDE
-
-
-//Network config HW1
-#ifdef HW1
-#define WLAN_SSID "hw1_gast"
-#define WLAN_PASSWORD "KeineAhnung"
-#define WLAN_IP 0x3400a8c0          //192.168.0.67
-#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
-#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
-#define MQTT_SERVER 0xD84016AC      // 172.22.64.216
-#define MQTT_PORT 1883
-#define NTP_SERVER "2.de.pool.ntp.org"
-#endif //HW1 
-
-//Network config FRENZY
-#ifdef FRENZY
-#define WLAN_SSID "GNX7EDD84-2"
-#define WLAN_PASSWORD "2W2ZLPJ9NVQ3"
-#define WLAN_IP 0x0F01a8c0          //192.168.1.15
-#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
-#define WLAN_GATEWAY 0xfe01a8c0     //192.168.1.254
-#define MQTT_SERVER 0x0201a8c0      //192.168.1.2
-#define MQTT_PORT 1883
-#define NTP_SERVER 0x0201a8c0       //192.168.1.2
-#endif //FRENZY 
-
-//Network config TPMOBIL
-#ifdef TPMOBIL
-#define WLAN_SSID "TP-Mobil"
-#define WLAN_PASSWORD "Punica-699"
-#define WLAN_IP 0x0F01a8c0          //192.168.1.15
-#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
-#define WLAN_GATEWAY 0x0101a8c0     //192.168.1.1
-#define MQTT_SERVER 0x0201a8c0      //192.168.1.2
-#define MQTT_PORT 1883
-#define NTP_SERVER 0x0201a8c0       //192.168.1.2
-#endif //TPMOBIL 
-
-// GPS config
-#define GPS_BAUD 9600
-#define RUN_ROUTE_DELAY  330
-
-// CaptureRoute config
-#define RUN_CAPTuRE_ROUTE_DELAY 300
-
-// Route config
-#define DISTANCE_BETWEEN_POINTS 0.75 // Meter

include/moveControl.h

@@ -1,47 +1,151 @@
+/**
+ * @file moveControl.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contrains the MoveControl class
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
 #ifndef MOVE_CONTROL_H
 #define MOVE_CONTROL_H
 
 #include <cstdint>
 #include <PID_v1.h>
+#include <Arduino.h>
 
 #include "motorControl.h"
 #include "speedometer.h"
-#include "config.h"
+#include "moveControlConfig.h"
-#include "debugMqtt.h"
-#include "debugTimes.h"
 
+/**
+ * @brief used to set driving status
+ * 
+ * When set to stop all motors are set to halt
+ * 
+ */
 enum DrivingStatus {stop,
                     drive};
 
-
+/**
+ * @brief This class manages the motors and the encoders
+ * 
+ * The controler uses two PIDs to control the motors. The PIDs
+ * use the Speedometer class to get the current speed and the
+ * given target speed to calculate a new duty cycle for the motors.
+ * 
+ */
 class MoveControl {
     public:
+        /**
+         * @brief Construct a new Move Control object
+         * 
+         * Initalize the motors, encoders and PIDs with given
+         * values in moveControlConfig.h
+         */
         MoveControl();
-        void init(MotorControl *left_motor, MotorControl *right_motor,
+
-                    Speedometer *left_encoder, Speedometer *right_encoder);
+        /**
+         * @brief Destroy the Move Control object
+         * 
+         * Stops the motors with emergencyStop()
+         */
+        ~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 Noramly called repeatedly by loop() to calcluate new values.
+         */
         void runMoveControl();
+
+        /**
+         * @brief Set the DrivingStatus
+         * 
+         * @see DrDrivingStatus
+         * @param status 
+         */
         void setDrivingStatus(DrivingStatus status);
+
+        /**
+         * @brief Set the target speed
+         * 
+         * If the given speed is close to zero, then the
+         * target speed is set to zero.
+         * @param speed in m/s
+         */
         void setSpeed(double speed);
+
+        /**
+         * @brief Set the rotationspeed
+         * 
+         * If the given rotationspeed is close to zero, then the
+         * target rotationspeed is set to zero.
+         * @param speed rad/s
+         */
         void setRotationspeed(double speed);
+
+        /**
+         * @brief Set the pid tunings
+         * 
+         * TODO: check if the function is working or indicates seg fault
+         * 
+         * @param side 0 -> left, 1 -> right
+         * @param p 
+         * @param i 
+         * @param d 
+         */
         void setPidTunings(uint8_t side, double p, double i, double d);
 
-    private:
+        /**
-        void calcWheelSpeed();
+         * @brief Set the min delay between each loop
-        void regulateMotors();
+         * 
-        void updateWheelSpeed();
+         * @param delay time in Milliseconds
+         */
+        void setDelay(uint8_t delay_) { delay = delay_; }
 
-        DebugMqtt *debug;
+    private:
+        /**
+         * @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;
         MotorControl *right_motor;
         Speedometer *left_speedometer;
         Speedometer *right_speedometer;
-
-        DrivingStatus driving_status = DrivingStatus::stop;
-
         PID *left_pid;
         PID *right_pid;
 
+        DrivingStatus driving_status = DrivingStatus::stop;
+
         double x_speed = 0;
         double rotation_speed = 0;
 
@@ -52,6 +156,6 @@ class MoveControl {
         double left_pid_out;
         double right_pid_out;
 
+        uint8_t delay = 30;
 };
-
 #endif // MOVE_CONTROL_H

include/moveControlConfig.h

@@ -0,0 +1,44 @@
+/**
+ * @file moveControlConfig.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief This file inherits some configuartion for moveControl.h 
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+//Pin config
+    //Left Motor
+    #define M_DIR_11 27
+    #define M_DIR_12 12
+    #define M_PWM_1 13
+    #define M_ENCODE_1A 33
+    #define M_ENCODE_1B 32
+    //Right Motor
+    #define M_DIR_21 23
+    #define M_DIR_22 14
+    #define M_PWM_2 22
+    #define M_ENCODE_2A 26
+    #define M_ENCODE_2B 25
+
+// PID config
+    #define PID_OUT_MIN -100
+    #define PID_OUT_MAX 100
+    #define PID_SAMPLETIME 30
+    // Left PID
+    #define PID_LEFT_P 85
+    #define PID_LEFT_I 0
+    #define PID_LEFT_D 0
+    // Right PID
+    #define PID_RIGHT_P 75
+    #define PID_RIGHT_I 0
+    #define PID_RIGHT_D 0
+
+// PWM
+    #define PWM_CHANNEL_M1 0
+    #define PWM_CHANNEL_M2 1
+    
+// SPEEDOMETER
+    #define WHEEL_DIAMETER 0.1263
+    #define ENC_STEPS 1024

include/network.h

@@ -0,0 +1,36 @@
+/**
+ * @file network.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Some network and MQTT stuff
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#ifndef NETWORK_H
+#define NETWORK_H
+
+#include <WiFi.h>
+#include <PubSubClient.h>
+
+#include "networkConfig.h"
+
+#define MQTT_TIME_RECONNECT 2500
+
+IPAddress local_IP;
+extern IPAddress gateway;
+extern IPAddress subnet;
+extern IPAddress mqtt_server;
+
+extern WiFiClient wifi_client;
+PubSubClient mqtt_client(wifi_client);
+
+void setIps();
+void connectWifi();
+void setupMQTT();
+bool connectMQTT();
+void checkMQTT();
+
+#endif // NETWORK_H

include/networkConfig.h

@@ -0,0 +1,64 @@
+#define HW1_2
+
+//Network config RHEDE
+#ifdef RHEDE
+#define WLAN_SSID "LebennigHuus"
+#define WLAN_PASSWORD "Punica-699"
+#define WLAN_IP 0x040BA8C0          //192.168.11.4
+#define WLAN_SUBNETMASK 0x0080FFFF  //255.255.128.0
+#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
+#define MQTT_SERVER 0x0701A8C0      //192.168.1.7
+#define MQTT_PORT 1883
+#define MQTT_AUTH
+#define MQTT_USER "kleiax"
+#define MQTT_PASSWORD "p?{$_~5%hBM7wrcFkr55KWr#"
+#define NTP_SERVER 0x0100a8c0       //192.168.0.1
+#endif //RHEDE
+
+//Network config HW1
+#ifdef HW1
+#define WLAN_SSID "hw1_gast"
+#define WLAN_PASSWORD "KeineAhnung"
+#define WLAN_IP 0x3400a8c0          //192.168.0.67
+#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
+#define WLAN_GATEWAY 0x0100a8c0     //192.168.0.1
+#define MQTT_SERVER 0xD84016AC      // 172.22.64.216
+#define MQTT_PORT 1883
+#define NTP_SERVER "2.de.pool.ntp.org"
+#endif //HW1 
+
+//Network config HW1
+#ifdef HW1_2
+#define WLAN_SSID "hw1_gast"
+#define WLAN_PASSWORD "KeineAhnung"
+#define WLAN_IP "192.168.0.67"
+#define WLAN_SUBNETMASK "255.255.255.0"
+#define WLAN_GATEWAY "192.168.0.1"
+#define MQTT_SERVER "172.22.64.216"
+#define MQTT_PORT 1883
+#define NTP_SERVER "2.de.pool.ntp.org"
+#endif //HW1 
+
+//Network config FRENZY
+#ifdef FRENZY
+#define WLAN_SSID "GNX7EDD84-2"
+#define WLAN_PASSWORD "2W2ZLPJ9NVQ3"
+#define WLAN_IP 0x0F01a8c0          //192.168.1.15
+#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
+#define WLAN_GATEWAY 0xfe01a8c0     //192.168.1.254
+#define MQTT_SERVER 0x0201a8c0      //192.168.1.2
+#define MQTT_PORT 1883
+#define NTP_SERVER 0x0201a8c0       //192.168.1.2
+#endif //FRENZY 
+
+//Network config TPMOBIL
+#ifdef TPMOBIL
+#define WLAN_SSID "TP-Mobil"
+#define WLAN_PASSWORD "Punica-699"
+#define WLAN_IP 0x0F01a8c0          //192.168.1.15
+#define WLAN_SUBNETMASK 0x00FFFFFF  //255.255.255.0
+#define WLAN_GATEWAY 0x0101a8c0     //192.168.1.1
+#define MQTT_SERVER 0x0201a8c0      //192.168.1.2
+#define MQTT_PORT 1883
+#define NTP_SERVER 0x0201a8c0       //192.168.1.2
+#endif //TPMOBIL 

lib/MotorControl/motorControl.h

@@ -95,6 +95,9 @@ class MotorControl {
         /**
          * @brief Set the Target Power
          * 
+         * If the given power is greater than 100 or smaller than -100, then
+         * this function only print an error to consol.
+         * 
          * @param power power in percent 
          */
         void setTargetPower(int8_t power);

src/driveModi/CaptureRoute/captureRoute.h

@@ -1,22 +0,0 @@
-#ifndef CAPTURE_ROUTE_H
-#define CAPTURE_ROUTE_H
-
-#include "driveModi/manualControl.h"
-#include "debugMqtt.h"
-#include "route.h"
-
-#include "config.h"
-
-class CaptureRoute : ManualControl {
-    public:
-        CaptureRoute();
-
-        void init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
-        void runCaptureRoute();
-
-    private:
-        Route *route;
-        DebugMqtt *debug;
-};
-
-#endif // CAPTURE_ROUTE_H

src/driveModi/ManualControl/manualControl.cpp

@@ -1,125 +0,0 @@
-#include "manualControl.h"
-
-ManualControl::ManualControl() {
-    this->debug = new DebugMqtt("ManualControl");
-}
-
-void ManualControl::init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
-    debug->sendMsg(Loglevel::info, "Init...");
-
-    this->moveControl = moveControl;
-    this->motorControlLeft = motorControlLeft;
-    this->motorControlRight = motorControlRight;
-
-    debug->sendMsg(Loglevel::info, "Init finished!");
-}
-
-void ManualControl::runManualControl() {
-    //Cancel if delay is not reached
-    if (millis() - this->last_millis < RUN_MANUALCONTROL_DELAY) {
-        return;
-    }
-    //Reset if this function was called a long time ago
-    if (millis() - this->last_millis > 1000) {
-        this->reset();
-        controlMode = ControMode::halt;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to halt by Timereset");
-    }
-    this->last_millis = millis();
-
-    //Change controlMode
-    if (this->controlMode == ControMode::halt && Ps3.data.button.l3) {
-        this->reset();
-        controlMode = ControMode::joystick;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
-    } else if (this->controlMode == ControMode::halt && Ps3.data.button.r3) {
-        this->reset();
-        controlMode = ControMode::trigger;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
-    } else if (this->controlMode == ControMode::joystick && Ps3.data.button.r3) {
-        this->reset();
-        controlMode = ControMode::trigger;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to ShoulderTrigger by Gamepad");
-    } else if (this->controlMode == ControMode::trigger && Ps3.data.button.l3) {
-        this->reset();
-        controlMode = ControMode::joystick;
-        debug->sendMsg(Loglevel::info, "Changed ControlMode to Joystick by Gamepad");
-    }
-
-    switch (this->controlMode) {
-        case ControMode::halt:
-            this->moveControl->setDrivingStatus(DrivingStatus::stop);
-            break;
-
-        case ControMode::joystick: {
-            // DebugTimes driveWithJoyDebug;
-            this->driveWithControllerJoystick();
-            // driveWithJoyDebug.stop("driveWithControllerJoystick", 100);
-
-            // DebugTimes moveControllDebug;
-            this->moveControl->runMoveControl();
-            // moveControllDebug.stop("moveControl", 100);
-            }
-            break;
-
-        case ControMode::trigger:
-            this->driveWithControllerShoulderTrigger();
-            break;
-
-        default:
-            break;
-    }
-}
-
-void ManualControl::driveWithControllerJoystick() {
-    this->moveControl->setDrivingStatus(DrivingStatus::drive);
-    int8_t x = Ps3.data.analog.stick.lx;
-    int8_t y = Ps3.data.analog.stick.ly;
-
-    double value_per_step = MANUALCONTROL_MAX_SPEED * 2 / 256;
-    this->moveControl->setSpeed((y * -1) * value_per_step);
-    this->moveControl->setDrivingStatus(DrivingStatus::drive);
-    // char str[64];
-    // sprintf(str, "driveWithJoy: %f", (y * -1) * value_per_step);
-    // debug->sendMsg(Loglevel::debug, str); // Kommt 1 raus bei vollausschlag
-
-    value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
-    this->moveControl->setRotationspeed(-x * value_per_step);
-}
-
-void ManualControl::changeDriveMode(ControMode drive_mode) {
-    this->reset();
-    controlMode = drive_mode;
-    debug->sendMsg(Loglevel::info, "Changed ControlMode by function");
-}
-
-void ManualControl::driveWithControllerShoulderTrigger() {
-    uint8_t nowL = Ps3.data.analog.button.l2;
-    uint8_t nowR = Ps3.data.analog.button.r2;;
-
-    nowL = map(nowL, 0, 255, 0, 100);
-    nowR = map(nowR, 0, 255, 0, 100);
-
-    // char str[64];
-    // sprintf(str, "nachher: nowL: %u, nowR: %u", nowL, nowR);
-    // debug->sendMsg(Loglevel::debug, str);
-
-    if (nowL != this->oldL) {
-        this->motorControlLeft->setTargetPower(nowL);
-        this->oldL = nowL;
-    }
-
-    if (nowR != this->oldR) {
-        this->motorControlRight->setTargetPower(nowR);
-        this->oldR = nowR;
-    }
-}
-
-void ManualControl::reset() {
-    this->oldL = 0;
-    this->oldR = 0;
-    motorControlLeft->setTargetPower(0);
-    motorControlRight->setTargetPower(0);
-    moveControl->setRotationspeed(0);
-    moveControl->setSpeed(0);
-}

src/driveModi/ManualControl/manualControl.h

@@ -1,37 +0,0 @@
-#ifndef MANUAL_CONTROL_H
-#define MANUAL_CONTROL_H
-
-#include <Ps3Controller.h>
-
-#include "moveControl.h"
-#include "motorControl.h"
-#include "debugMqtt.h"
-#include "debugTimes.h"
-
-enum ControMode {halt, joystick, trigger};
-class ManualControl {
-    public:
-        ManualControl();
-        void init(MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight);
-        void runManualControl();
-        void changeDriveMode(ControMode drive_mode);
-
-    private:
-        void driveWithControllerJoystick();
-        void driveWithControllerShoulderTrigger();
-        void reset();
-
-        MoveControl *moveControl;
-        MotorControl *motorControlLeft;
-        MotorControl *motorControlRight;
-        DebugMqtt *debug;
-
-        ControMode controlMode = ControMode::halt;
-        uint32_t last_millis = 0;
-
-        // Is needed for driveWithControllerShoulderTrigger()
-        uint8_t oldL = 0;
-        uint8_t oldR = 0;
-};
-
-#endif // MANUAL_CONTROL_H

src/driveModi/Modi/Autopilot/autopilot.cpp

@@ -1,4 +1,4 @@
-#include "driveModi/autopilot.h"
+#include "driveModi/Modi/Autopilot/autopilot.h"
 
 Autopilot::Autopilot() {
     this->debug = new DebugMqtt("Autopilot");

src/driveModi/Modi/Autopilot/autopilot.h

@@ -6,12 +6,14 @@
 #include "route.h"
 #include "moveControl.h"
 #include "debugMqtt.h"
+#include "driveModi/driveModi.h"
 
-class Autopilot {
+class Autopilot : DriveModi{
     public:
         Autopilot();
 
         void init(Route *route, MoveControl *moveControl);
+        void loop();
         void runAutopilot();
 
 

src/driveModi/Modi/CaptureRoute/captureRoute.cpp

@@ -1,12 +1,10 @@
-#include "driveModi/captureRoute.h"
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
 
 CaptureRoute::CaptureRoute() {
-    this->debug = new DebugMqtt("CaptureRoute");
 }
 
-void CaptureRoute::init(Route *route, MoveControl *moveControl, MotorControl *motorControlLeft, MotorControl *motorControlRight) {
+void CaptureRoute::init() {
-    this->route = route;
+    
-    ManualControl::init(moveControl, motorControlLeft, motorControlRight);
     
 }
 

src/driveModi/Modi/CaptureRoute/captureRoute.h

@@ -0,0 +1,21 @@
+#ifndef CAPTURE_ROUTE_H
+#define CAPTURE_ROUTE_H
+
+#include "driveModi/Modi/ManualControl/manualControl.h"
+// #include "route.h"
+
+#include "config.h"
+
+class CaptureRoute : ManualControl {
+    public:
+        CaptureRoute();
+
+        void init();
+        void runCaptureRoute();
+
+    private:
+        // Route *route;
+        // DebugMqtt *debug;
+};
+
+#endif // CAPTURE_ROUTE_H

src/driveModi/Modi/ConsolControl/consolControl.h

@@ -4,9 +4,10 @@
 #include <string>
 
 #include "moveControl.h"
+#include "driveModi/driveModi.h"
 #include "debugMqtt.h"
 
-class ConsolControl {
+class ConsolControl : DriveModi{
     public:
         ConsolControl();
         void init(MoveControl *moveControl);

src/driveModi/Modi/ManualControl/manualControl.cpp

@@ -0,0 +1,42 @@
+/**
+ * @file manualControl.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Implementation of the class manualControl.h
+ * @version 0.1
+ * @date 2021-12-13
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#include "manualControl.h"
+
+ManualControl::ManualControl() { }
+
+ManualControl::~ManualControl() {
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationspeed(0);
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+}
+
+void ManualControl::init(MoveControl *moveControl) {
+    this->moveControl = moveControl;
+    this->moveControl->setDrivingStatus(DrivingStatus::drive);
+}
+
+void ManualControl::loop() {
+    static uint32_t last_millis = 0;
+    if (millis() - last_millis < delay) {
+        return;
+    }
+}
+
+void ManualControl::runManualControl() {
+    int8_t x = Ps3.data.analog.stick.lx;
+    int8_t y = Ps3.data.analog.stick.ly;
+
+    double value_per_step = this->max_speed * 2 / 256;
+    this->moveControl->setSpeed((y * -1) * value_per_step);
+
+    value_per_step = this->max_rotation * 2 / 256;
+    this->moveControl->setRotationspeed(-x * value_per_step);
+}

src/driveModi/Modi/ManualControl/manualControl.h

@@ -0,0 +1,91 @@
+/**
+ * @file manualControl.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief A small class to drive the Rover by the controller joystick.
+ * @version 0.1
+ * @date 2021-12-13
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+#ifndef MANUAL_CONTROL_H
+#define MANUAL_CONTROL_H
+
+#include <Ps3Controller.h>
+
+#include "moveControl.h"
+#include "driveModi/driveModi.h"
+
+/**
+ * @brief Drive the Rover with a Joystick
+ * 
+ * This class gets the x and y value from the PS3 controller
+ * and map the values to moveControl
+ * 
+ * @see MoveControl
+ */
+class ManualControl : public DriveModi{
+    public:
+        ManualControl();
+
+        /**
+         * @brief Destroy the Manual Control object
+         * Set in moveControl speed and rotation to 0 and set DrivingStatus::stop
+         */
+        ~ManualControl();
+
+        /**
+         * @brief Initalize ManualControl
+         * 
+         * set DrivingStatus::drive
+         * 
+         * @param moveControl 
+         */
+        void init(MoveControl *moveControl);
+
+        /**
+         * @brief Calls runManualControl() 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();
+
+        /**
+         * @brief Noramly called repeatedly by loop() to calcluate new values.
+         * Set new values for speed and rotation in moveControl
+         */
+        void runManualControl();
+
+        /**
+         * @brief Set the min delay between each loop
+         * 
+         * @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 maxSpeed) { max_speed = maxSpeed; }
+        
+        /**
+         * @brief Set the max rotation
+         * 
+         * @param maxRotation in rad/s (maybe) 
+         */
+        void setMaxRotation(double maxRotation) { max_rotation = maxRotation; }
+
+    private:
+        MoveControl *moveControl;
+
+        uint8_t delay = 10;
+        double max_speed = 1;
+        double max_rotation = 7;
+};
+
+#endif // MANUAL_CONTROL_H

src/driveModi/driveManager.cpp

@@ -0,0 +1,72 @@
+/**
+ * @file driveManager.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Implemention of the class driveManager.h.
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#include "driveModi/driveManager.h"
+
+DriveManager::DriveManager(MoveControl *moveControl) {
+    this->moveControl = moveControl;
+}
+
+void DriveManager::loop() {
+    this->currentModus->loop();
+    this->moveControl->loop();
+}
+
+void DriveManager::runDriveManager() {
+    
+}
+
+void DriveManager::nextModus() {
+    
+}
+
+void DriveManager::changeModus(Modi modus) {
+
+    //Set moveControl to a safe state
+    this->moveControl->setSpeed(0);
+    this->moveControl->setRotationspeed(0);
+    this->moveControl->setDrivingStatus(DrivingStatus::stop);
+    delete this->currentModus;
+
+    switch (modus) {
+        case Modi::Off:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::ManualControl: {
+            ManualControl *ptr = new ManualControl;
+            ptr->init(this->moveControl);
+            this->currentModus = ptr;
+            }
+            break;
+
+        case Modi::CaptureRoute:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::Autopilot:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::ConsolControl:
+            this->currentModus = nullptr;
+            break;
+
+        case Modi::TestMode:
+            this->currentModus = nullptr;
+            break;
+        
+        default:
+            this->currentModus = nullptr;
+            break;
+    }
+}
+

src/driveModi/driveManager.h

@@ -0,0 +1,51 @@
+/**
+ * @file driveManager.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a class to switch the DriveModi
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#ifndef DRIVE_MANAGER_H
+#define DRIVE_MANAGER_H
+
+#include "moveControl.h"
+#include "driveModi/driveModi.h"
+
+// All Drive Modi
+#include "driveModi/Modi/ManualControl/manualControl.h"
+#include "driveModi/Modi/CaptureRoute/captureRoute.h"
+#include "driveModi/Modi/Autopilot/autopilot.h"
+#include "driveModi/Modi/ConsolControl/consolControl.h"
+#include "driveModi/Modi/TestMode/testMode.h"
+
+enum class Modi {   
+                    Off,
+                    ManualControl,
+                    CaptureRoute,
+                    Autopilot,
+                    ConsolControl,
+                    TestMode
+                };
+
+class DriveManager {
+    public:
+        DriveManager(MoveControl *moveControl);
+
+        void loop();
+        void runDriveManager();
+
+        void nextModus();
+        void changeModus(Modi modus);
+
+    private:
+        MoveControl *moveControl;
+        DriveModi *currentModus;
+
+};
+
+
+#endif // DRIVE_MANAGER_H

src/driveModi/driveModi.h

@@ -0,0 +1,22 @@
+/**
+ * @file driveModi.h
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains a virtual class for all Modi
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#ifndef DRIVEMODI_H
+#define DRIVEMODI_H
+
+class DriveModi {
+    public:
+        DriveModi(){}
+        virtual ~DriveModi(){}
+        virtual void loop() = 0;
+        
+};
+#endif // DRIVEMODI_H

src/main.cpp

@@ -1,208 +1,49 @@
 #include <Arduino.h>
 #include <Ps3Controller.h>
-#include <WiFi.h>
-#include <PubSubClient.h>
-#include <time.h>
 
 #include "config.h"
 
-#include "motorControl.h"
+
-#include "speedometer.h"
+#include "driveModi/Modi/ManualControl/manualControl.h"
-#include "driveModi/autopilot.h"
+#include "driveModi/driveManager.h"
-#include "driveModi/captureRoute.h"
-#include "driveModi/manualControl.h"
-#include "driveModi/consolControl.h"
 #include "moveControl.h"
-#include "route.h"
+#include "network.h"
-#include "debugMqtt.h"
-#include "debugTimes.h"
 
 void callbackControllerAction();
 void callbackControllerConnect();
 void controllerPrintBattery();
 
-bool reconnectMqtt();
-
-MotorControl left_motor;
-MotorControl right_motor;
-Speedometer speedometer_left;
-Speedometer speedometer_right;
 MoveControl moveController;
-Route route;
+DriveManager driveManager(&moveController);
-DebugMqtt debugger("main");
-
-ManualControl manualControl;
-CaptureRoute captureRoute;
-Autopilot autopilot;
-ConsolControl consolControl;
-
-IPAddress local_IP(WLAN_IP);
-IPAddress gateway(WLAN_GATEWAY);
-IPAddress subnet(WLAN_SUBNETMASK);
-IPAddress mqtt_server(MQTT_SERVER);
-
-WiFiClient wifi_client;
-PubSubClient mqtt_client(wifi_client);
-
-uint64_t lastReconnectAttempt = 0;
-int controller_battery = -1;
-enum DriveMode {autopilot_e, captureRoute_e, manualControl_e, consolControl_e};
-DriveMode driveMode = manualControl_e;
-
 
 void setup() {
     Serial.begin(115200);
 
-    // Configures static IP address
+    setIps();
-    if (!WiFi.config(local_IP, gateway, subnet)) {
+    connectWifi();
-        Serial.println("STA Failed to configure");
+    setupMQTT();
-    }
-
-    // Connect to Wi-Fi network with SSID and password
-    Serial.print("Connecting to ");
-    Serial.println(WLAN_SSID);
-    WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
-    while (WiFi.status() != WL_CONNECTED) {
-        delay(500);
-        Serial.print(".");
-    }
-
-    // Print local IP address and start web server
-    Serial.println("");
-    Serial.println("WiFi connected.");
-    Serial.println("IP address: ");
-    Serial.println(WiFi.localIP());
-
-    mqtt_client.setServer(mqtt_server, MQTT_PORT);
-    // mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
-    mqtt_client.setSocketTimeout(1);
-    reconnectMqtt();
-
-    //FIXME: make it from config.h
-    configTime(3600, 3600, "2.de.pool.ntp.org");
-
-    DebugMqtt::init(&mqtt_client, Loglevel::debug);
 
     Ps3.attach(callbackControllerAction);
     Ps3.attachOnConnect(callbackControllerConnect);
-    // Ps3.attachOnDisconnect(callbackControllerDisconnect);
     Serial.println("\nReady to connect a PS3 Controller... \n");
     Ps3.begin();
-
-    // Init hardware
-    left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
-    right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
-
-    speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
-    speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);
-
-    moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
-
-    // Init driveModi
-    manualControl.init(&moveController, &left_motor, &right_motor);
-    captureRoute.init(&route, &moveController, &left_motor, &right_motor);
-    autopilot.init(&route, &moveController);
-    consolControl.init(&moveController);
 }
 
 void loop() {
-
+    checkMQTT();
-
+    driveManager.loop();
-    if (!mqtt_client.connected()) {
-    long now = millis();
-    if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
-      lastReconnectAttempt = now;
-      // Attempt to reconnect
-      if (reconnectMqtt()) {
-        lastReconnectAttempt = 0;
-      }
-    }
-  } else {
-    // Client connected
-    mqtt_client.loop();
-  }   
-  
-    left_motor.runMotorControl();
-    right_motor.runMotorControl();
-    
-    speedometer_left.runSpeedometer();
-    speedometer_right.runSpeedometer();
-    // TODO: Auskommentiert weil macht vielleicht komische Sachen
-    // route.runRoute();
-
-    switch (driveMode) {
-        case manualControl_e:
-            manualControl.runManualControl();
-            break;
-
-        case captureRoute_e:
-            captureRoute.runCaptureRoute();
-            break;
-
-        case autopilot_e:
-            autopilot.runAutopilot();
-            break;
-
-        case consolControl_e:
-            consolControl.run();
-            break;
-
-        default:
-            break;
-    }
-}
-
-bool reconnectMqtt() {
-    // Create a random client ID
-    String clientId = "ESP32Rover-";
-    clientId += String(random(0xffff), HEX);
-    #ifdef MQTT_AUTH
-    if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
-    #endif //MQTT_AUTH
-    #ifndef MQTT_AUTH
-    if (mqtt_client.connect(clientId.c_str())) {
-    #endif //MQTT_AUTH
-        // Once connected, publish an announcement...
-        mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
-    }
-    return mqtt_client.connected();
 }
 
 void callbackControllerAction() {
-    if (Ps3.event.button_down.ps) {
-        switch (driveMode) {
-        case manualControl_e:
-            driveMode = DriveMode::captureRoute_e;
-            debugger.sendMsg(Loglevel::info, "New driveMode = CaptureRoute");
-            break;
 
-        case captureRoute_e:
-            driveMode = DriveMode::autopilot_e;
-            debugger.sendMsg(Loglevel::info, "New driveMode = Autopilot");
-            break;
-
-        case autopilot_e:
-            driveMode = DriveMode::consolControl_e;
-            debugger.sendMsg(Loglevel::info, "New driveMode = ConsolControl");
-            break;
-
-        case consolControl_e:
-            driveMode = DriveMode::manualControl_e;
-            debugger.sendMsg(Loglevel::info, "New driveMode = ManualControl");
-            break;
-
-        default:
-            break;
-        }
-    }
 }
 
 void callbackControllerConnect() {
     Serial.println("Controller connected to ESP32");
-    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32");
 }
 
 void controllerPrintBattery() {
+    static uint8_t controller_battery = -1;
     if( controller_battery != Ps3.data.status.battery ){
         controller_battery = Ps3.data.status.battery;
     }

src/moveControl.cpp

@@ -1,9 +1,11 @@
 #include "moveControl.h"
 
-#include <Arduino.h>
-
 MoveControl::MoveControl() {
-    this->debug = new DebugMqtt("MoveControl");
+    this->left_motor = new MotorControl;
+    this->right_motor = new MotorControl;
+    this->left_speedometer = new Speedometer;
+    this->right_speedometer = new Speedometer;
+
     this->left_pid = new PID(   &this->wheelspeed_left,
                                 &this->left_pid_out,
                                 &this->wheelspeed_left_target,
@@ -21,49 +23,42 @@ MoveControl::MoveControl() {
 
     this->right_pid->SetOutputLimits(PID_OUT_MIN, PID_OUT_MAX);
     this->right_pid->SetSampleTime(PID_SAMPLETIME);
-    this->right_pid->SetMode(AUTOMATIC);                                
+    this->right_pid->SetMode(AUTOMATIC); 
+
+    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->left_speedometer->init(M_ENCODE_1A, M_ENCODE_1B, WHEEL_DIAMETER, ENC_STEPS);
+    this->right_speedometer->init(M_ENCODE_2A, M_ENCODE_2B, WHEEL_DIAMETER, ENC_STEPS);                               
 }
 
-void MoveControl::init(MotorControl *left_motor, MotorControl *right_motor,
+MoveControl::~MoveControl() {
-                        Speedometer *left_speedometer, Speedometer *right_speedometer) {
+    this->left_motor->emergencyStop();
-    debug->sendMsg(Loglevel::info, "Init...");
+    this->right_motor->emergencyStop();
-    this->left_motor = left_motor;
+}
-    this->right_motor = right_motor;
+
-    this->left_speedometer = left_speedometer;
+void MoveControl::loop() {
-    this->right_speedometer = right_speedometer;
+    this->left_motor->loop();
-    debug->sendMsg(Loglevel::info, "Init finished!");
+    this->right_motor->loop();
+    this->left_speedometer->loop();
+    this->right_speedometer->loop();
+
+    static uint64_t last_millis = 0;
+    if (millis() - last_millis < delay) 
+        return;
+
+    this->runMoveControl();
+    last_millis = millis();
 }
 
 void MoveControl::runMoveControl() {
-        static uint64_t last_millis = 0;
+    this->updateCurrentWheelSpeed();
-    if (millis() - last_millis < RUN_MOVE_CONTROL_DELAY) {
+    this->calcTargetWheelSpeed();
-        return;
-    }
-    last_millis = millis();
 
-    this->updateWheelSpeed();
-    this->calcWheelSpeed();
-
-    DebugTimes pidTimes;
     this->right_pid->Compute();
     this->left_pid->Compute();
-    pidTimes.stopConsol("PID Calulate", 100);
 
-    DebugTimes regMotorTime;
     this->regulateMotors();
-    regMotorTime.stopConsol("regulateMotors", 100);
-
-    static uint32_t functioncalls = 0;
-    functioncalls++;
-    if (functioncalls % 50 == 0) {
-        char str[128];
-        sprintf(str, "x_speed: %3.2f, rotation_speed: %3.2f, left: %3.2f, right: %3.2f",
-            this->x_speed, this->rotation_speed, this->wheelspeed_left,
-            this->wheelspeed_right);
-        DebugTimes debugMsgTime;
-        debug->sendMsg(Loglevel::debug, str);
-        debugMsgTime.stopConsol("debug Msg", 100);
-    }
 }
 
 void MoveControl::setDrivingStatus(DrivingStatus status) {
@@ -75,7 +70,6 @@ void MoveControl::setSpeed(double speed) {
         this->x_speed = 0;
     else
         this->x_speed = speed;
-    // Serial.printf("New speed: %f in moveControll.cpp \n", speed);
 }
 
 void MoveControl::setRotationspeed(double speed) {
@@ -83,7 +77,6 @@ void MoveControl::setRotationspeed(double speed) {
         this->rotation_speed = 0;
     else
         this->rotation_speed = speed;
-    
 }
 
 void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
@@ -94,10 +87,9 @@ void MoveControl::setPidTunings(uint8_t side, double p, double i, double d) {
         selectedPID = this->right_pid;
 
     selectedPID->SetTunings(p, i, d);
-    
 }
 
-void MoveControl::calcWheelSpeed() {
+void MoveControl::calcTargetWheelSpeed() {
     /* original formula:
        (1 / r) / 1  b \ / x \ = / Xl \
                \ 1 -b / \ T /   \ Xr /  */
@@ -109,8 +101,6 @@ void MoveControl::calcWheelSpeed() {
 
     this->wheelspeed_right_target = (A * this->x_speed + B * this->rotation_speed) * (WHEEL_DIAMETER / 2);
     this->wheelspeed_left_target  = (A * this->x_speed + (-B) * this->rotation_speed) * (WHEEL_DIAMETER / 2);
-    //debug->writeToInflux("moveControl", "wheelspeed_right_target", this->wheelspeed_right_target);
-    //debug->writeToInflux("moveControl", "wheelspeed_left_target", this->wheelspeed_left_target);
 }
 
 void MoveControl::regulateMotors() {
@@ -132,7 +122,7 @@ void MoveControl::regulateMotors() {
     }
 }
 
-void MoveControl::updateWheelSpeed() {
+void MoveControl::updateCurrentWheelSpeed() {
     this->wheelspeed_left = this->left_speedometer->getSpeed();
     this->wheelspeed_right = this->right_speedometer->getSpeed();
 }

src/network.cpp

@@ -0,0 +1,76 @@
+/**
+ * @file network.cpp
+ * @author Alexander Klein (alex@kleiax.de)
+ * @brief Contains function implementation of network.h
+ * @version 0.1
+ * @date 2021-12-14
+ * 
+ * @copyright Copyright (c) 2021
+ * 
+ */
+
+#include "network.h"
+void setIps() {
+    local_IP.fromString(WLAN_IP);
+    gateway.fromString(WLAN_GATEWAY);
+    subnet.fromString(WLAN_SUBNETMASK);
+    mqtt_server.fromString(MQTT_SERVER);
+}
+
+void setupMQTT() {
+    mqtt_client.setServer(mqtt_server, MQTT_PORT);
+    // mqtt_client.setServer(MQTT_SERVER, MQTT_PORT);
+    mqtt_client.setSocketTimeout(1);
+    connectMQTT();
+}
+
+bool connectMQTT() {
+    // Create a random client ID
+    String clientId = "ESP32Rover-";
+    clientId += String(random(0xffff), HEX);
+    #ifdef MQTT_AUTH
+    if (mqtt_client.connect(clientId.c_str(), MQTT_USER, MQTT_PASSWORD)) {
+    #endif //MQTT_AUTH
+    #ifndef MQTT_AUTH
+    if (mqtt_client.connect(clientId.c_str())) {
+    #endif //MQTT_AUTH
+        // Once connected, publish an announcement...
+        mqtt_client.publish("Rover/Info", "Connected to Mqtt-Broker");
+    }
+    return mqtt_client.connected();
+}
+void connectWifi() {
+   // Configures static IP address
+    if (!WiFi.config(local_IP, gateway, subnet)) {
+        Serial.println("STA Failed to configure");
+    }
+
+    // Connect to Wi-Fi network with SSID and password
+    Serial.print("Connecting to ");
+    Serial.println(WLAN_SSID);
+    WiFi.begin(WLAN_SSID, WLAN_PASSWORD);
+    while (WiFi.status() != WL_CONNECTED) {
+        delay(500);
+        Serial.print(".");
+    }
+
+    // Print local IP address and start web server
+    Serial.println("");
+    Serial.println("WiFi connected.");
+    Serial.println("IP address: ");
+    Serial.println(WiFi.localIP()); 
+}
+
+void checkMQTT() {
+    static uint64_t lastReconnectAttempt = 0;
+    if (!mqtt_client.connected()) {
+        long now = millis();
+        if (now - lastReconnectAttempt > MQTT_TIME_RECONNECT) {
+            lastReconnectAttempt = now;
+            // Attempt to reconnect
+            if (connectMQTT()) 
+                lastReconnectAttempt = 0;
+        }
+    } else
+    mqtt_client.loop();
+}

src/route.cpp

@@ -1,4 +1,5 @@
 #include "route.h"
+#include "config.h"
 
 Route::Route() {
     Serial1.begin(GPS_BAUD, SERIAL_8N1, GPS_RX, GPS_TX);
@@ -7,7 +8,7 @@ Route::Route() {
 
 void Route::runRoute() {
     static uint64_t last_millis = 0;
-    if (millis() - last_millis < RUN_ROUTE_DELAY) {
+    if (millis() - last_millis < 123456789) {
         return;
     }
     last_millis = millis();
@@ -73,7 +74,7 @@ double Route::getDis(Point point_1, Point point_2) {
 }
 
 bool Route::nearlySameLocation(Point p1, Point p2) {
-    if (this->getDis(p1, p2) < DISTANCE_BETWEEN_POINTS)
+    if (this->getDis(p1, p2) < 0.5)
         return true;
     return false;
 }