clean main.cpp, clean moveControl.cpp

added classdiagramm and ManualControl

doc/classdiagramm.uxf

@@ -0,0 +1,108 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<diagram program="umlet" version="14.3.0">
+  <zoom_level>10</zoom_level>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>120</x>
+      <y>670</y>
+      <w>210</w>
+      <h>510</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;hardware&gt;&gt;
+MotorControl
+--
+- name: String
+- debug: DebugMqtt*
+- target_power: int8_t = 0
+- power: int8_t = 0
+- direction: uint8_t = 0
+- last_millis: uint64_t = 0;
+- pwm_pin: uint8_t
+- pwm_channel: uint8_t
+- direction_pin_1: uint8_t
+- direction_pin_2: uint8_t
+--
++ MotorControl()
++ init(pwm_pin: uint8_t,
+	   pwm_channel: uint8_t,
+	  	 direction_1: uint8_t, 
+  		 direction_2: uint8_t, 
+	  	 name: String)
++ runMotorControl()
++ setTargetPower(power: int8_t)
++ stop()
++ emergencyStop()
++ toString()
++ getPower(): int8_t
++ getTargetPower(): int8_t
++ isTargetPowerReached(): bool
++ isAccelerationPositive(): bool
++ isAccelerationNegative(): bool
+- setRealPower(power: int8_t)
+- increasePower(power: int8_t)</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>380</x>
+      <y>670</y>
+      <w>210</w>
+      <h>330</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;hardware&gt;&gt;
+Speedometer
+--
+- encoder: ESP32Encoder
+- debug: DebugMqtt*
+- name: String
+- speed: double = 0
+- last_millis: uint64_t = 0;
+- bufPos: uint8_t
+- buf: int16_t[BUF_SIZE]
+--
++ Speedometer()
++ init(pinA: uint8_t,
+	   pinB: uint8_t, 
+	  	 name: String)
++ runSpeedometer()
++ setTargetPower(power: int8_t)
++ getSpeed(): double
++ getDirection(): uint8_t
+- addValToBuf(val: int16_t)
+- getAverage(): int16_t</panel_attributes>
+    <additional_attributes/>
+  </element>
+  <element>
+    <id>UMLClass</id>
+    <coordinates>
+      <x>900</x>
+      <y>660</y>
+      <w>210</w>
+      <h>330</h>
+    </coordinates>
+    <panel_attributes>&lt;&lt;hardware&gt;&gt;
+Speedometer
+--
+- encoder: ESP32Encoder
+- debug: DebugMqtt*
+- name: String
+- speed: double = 0
+- last_millis: uint64_t = 0;
+- bufPos: uint8_t
+- buf: int16_t[BUF_SIZE]
+--
++ Speedometer()
++ init(pinA: uint8_t,
+	   pinB: uint8_t, 
+	  	 name: String)
++ runSpeedometer()
++ setTargetPower(power: int8_t)
++ getSpeed(): double
++ getDirection(): uint8_t
+- addValToBuf(val: int16_t)
+- getAverage(): int16_t</panel_attributes>
+    <additional_attributes/>
+  </element>
+</diagram>

src/config.h

@@ -46,13 +46,14 @@
 #define RUN_MOVE_CONTROL_DELAY 150 // Normal 10 untested
 #define ACCELERATE_STEPS 1
 
+//ManualControl config
+#define RUN_MANUALCONTROL_DELAY 10
+#define MANUALCONTROL_MAX_SPEED 1.0
+#define MANUALCONTROL_MAX_ROTATION 1.0
+
 // PS3 Controller
 // ESP32 MAC BL 24:62:AB:F2:4B:3A
 
-//Max Values for speed and rotation
-#define MAX_SPEED 1.0
-#define MAX_ROTATION 1.0
-
 //Network config
 #define WLAN_SSID "Kleiax2"
 #define WLAN_PASSWORD "Punica-699"

src/driveModi/manualControl.cpp

@@ -0,0 +1,83 @@
+#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;
+    }
+    this->last_millis = millis();
+
+    switch (this->drive_mode) {
+        case DriveMode::stop:
+            this->reset();
+            break;
+
+        case DriveMode::joystick:
+            this->driveWithControllerJoystick();
+            this->moveControl->runMoveControl();
+            break;
+
+        case DriveMode::trigger:
+            this->driveWithControllerShoulderTrigger();
+            break;
+
+        default:
+            break;
+    }
+}
+
+void ManualControl::driveWithControllerJoystick() {
+    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);
+
+    value_per_step = MANUALCONTROL_MAX_ROTATION * 2 / 256;
+    this->moveControl->setRotationspeed(x * value_per_step);
+}
+
+void ManualControl::changeDriveMode(DriveMode drive_mode) {
+    this->drive_mode = drive_mode;
+}
+
+void ManualControl::driveWithControllerShoulderTrigger() {
+    uint8_t nowL = Ps3.data.analog.button.l2;
+    uint8_t nowR = Ps3.data.analog.button.r2;
+
+    map(nowL, 0, 255, 0, 100);
+    map(nowR, 0, 255, 0, 100);
+
+    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.h

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

src/hardware/motorControl.cpp

@@ -8,9 +8,11 @@ MotorControl::MotorControl() {
     this->debug = new DebugMqtt("MotorControl");
 }
 
-void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2) {
+void MotorControl::init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_pin_1, uint8_t direction_pin_2, String name) {
     debug->sendMsg(Loglevel::info, "Init...");
 
+    this->name = name;
+
     this->pwm_pin = pwm_pin;
     this->pwm_channel = pwm_channel;
     this->direction_pin_1 = direction_pin_1;
@@ -83,7 +85,9 @@ void MotorControl::runMotorControl() {
 void MotorControl::setTargetPower(int8_t power) {
     if (power <= 100 && power >= -100) {
         this->target_power = power;
-        debug->writeToInflux("motorControl", "target_power_ASIDE", power);
+        char str[32];
+        sprintf(str, "target_power_%s", this->name.c_str());
+        debug->writeToInflux("motorControl", str, power);
     } else {
         debug->sendMsg(Loglevel::error, "Invalid Argument in setTargetPower!");
     }
@@ -91,7 +95,9 @@ void MotorControl::setTargetPower(int8_t power) {
 
 void MotorControl::stop() {
     this->target_power = 0;
-    debug->writeToInflux("motorControl", "target_power_ASIDE", this->target_power);
+    char str[32];
+    sprintf(str, "target_power_%s", this->name.c_str());
+    debug->writeToInflux("motorControl", str, this->target_power);
 }
 
 void MotorControl::emergencyStop() {
@@ -158,7 +164,9 @@ void MotorControl::setRealPower(int8_t power) {
     }
 
     ledcWrite(this->pwm_channel, pwm_val);
-    debug->writeToInflux("motorControl", "real_power_ASIDE", power);
+    char str[32];
+    sprintf(str, "real_power_%s", this->name.c_str());
+    debug->writeToInflux("motorControl", str, power);
     // Serial.printf("pwm_val: %d, ", pwm_val);
 }
 

src/hardware/motorControl.h

@@ -9,7 +9,7 @@
 class MotorControl {
     public:
         MotorControl();
-        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2);
+        void init(uint8_t pwm_pin, uint8_t pwm_channel, uint8_t direction_1, uint8_t direction_2, String name);
         void runMotorControl();
         void setTargetPower(int8_t power);
         void stop();
@@ -26,6 +26,8 @@ class MotorControl {
         void setRealPower(int8_t power);
         void increasePower(int8_t power);
 
+        String name;
+
         DebugMqtt *debug;
 
         int8_t target_power = 0;

src/hardware/speedometer.cpp

@@ -7,8 +7,9 @@ Speedometer::Speedometer() {
     this->debug = new DebugMqtt("Speedometer");
 }
 
-void Speedometer::init(uint8_t pinA, uint8_t pinB) {
+void Speedometer::init(uint8_t pinA, uint8_t pinB, String name) {
     debug->sendMsg(Loglevel::info, "Init...");
+    this->name = name;
     ESP32Encoder::useInternalWeakPullResistors=DOWN;
     this->encoder.attachFullQuad(pinA, pinB);
     this->encoder.setFilter(ENC_FILTER);
@@ -49,7 +50,9 @@ void Speedometer::runSpeedometer() {
     } else {
         this->speed = 0;
     }
-    debug->writeToInflux("speedometer", "speed_ASIDE", this->speed);
+    char str[32];
+    sprintf(str, "speed_%s", this->name.c_str());
+    debug->writeToInflux("speedometer", str, this->speed);
 
     // Serial.printf("time: %d, ", time);
     // Serial.printf("count: %d, ",count);

src/hardware/speedometer.h

@@ -11,7 +11,7 @@
 class Speedometer {
     public:
         Speedometer();
-        void init(uint8_t pinA, uint8_t pinB);
+        void init(uint8_t pinA, uint8_t pinB, String name);
         void runSpeedometer();
         double getSpeed();
         uint8_t getDirection();
@@ -24,6 +24,7 @@ class Speedometer {
         ESP32Encoder encoder;
         DebugMqtt *debug;
 
+        String name;
         double speed = 0;
         uint32_t last_millis = 0;
 

src/main.cpp

@@ -12,7 +12,6 @@
 
 void callbackControllerAction();
 void callbackControllerConnect();
-void callbackControllerDisconnect();
 void controllerPrintBattery();
 
 void reconnectMqtt();
@@ -32,16 +31,8 @@ IPAddress mqtt_server(MQTT_SERVER);
 WiFiClient wifi_client;
 PubSubClient mqtt_client(wifi_client);
 
-uint64_t last_millis = 0;
 int controller_battery = -1;
 
-// Temp code
-double speed = 0;
-uint8_t drive_mode = 0;
-void driveWithControllerJoystick(int8_t x, int8_t y);
-void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right);
-void changeDriveMode();
-
 void setup() {
     Serial.begin(115200);
 
@@ -79,34 +70,24 @@ void setup() {
     Serial.println("\nReady to connect a PS3 Controller... \n");
     Ps3.begin();
 
-  left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12);
+    left_motor.init(M_PWM_1, PWM_CHANNEL_M1, M_DIR_11, M_DIR_12, "left");
-  right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22);
+    right_motor.init(M_PWM_2, PWM_CHANNEL_M2, M_DIR_21, M_DIR_22, "right");
 
-  speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B);
+    speedometer_left.init(M_ENCODE_1A, M_ENCODE_1B, "left");
-  speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B);
+    speedometer_right.init(M_ENCODE_2A, M_ENCODE_2B, "right");
 
     moveController.init(&left_motor, &right_motor, &speedometer_left, &speedometer_right);
 }
 
 void loop() {
-  if (!mqtt_client.connected()) {
+    if (!mqtt_client.connected())
         reconnectMqtt();
-  }
     mqtt_client.loop();
 
-  if (millis() - last_millis > 1000) {
-    debugger.sendTime();
-
-    last_millis = millis();
-  }
-
     left_motor.runMotorControl();
     right_motor.runMotorControl();
     speedometer_left.runSpeedometer();
     speedometer_right.runSpeedometer();
-
-  if (drive_mode == 1)
-    moveController.runMoveControl();
 }
 
 void reconnectMqtt() {
@@ -132,29 +113,14 @@ void reconnectMqtt() {
 }
 
 void callbackControllerAction() {
-  if (Ps3.event.button_down.r3) { controllerPrintBattery(); }
+    if (Ps3.event.button_down.start) {
-  if (Ps3.event.button_down.l1) { moveController.setSpeed(speed -= 0.1); }
-  if (Ps3.event.button_down.r1) { moveController.setSpeed(speed += 0.1); }
-  if (Ps3.event.button_down.start) { changeDriveMode(); }
-  if (abs(Ps3.event.analog_changed.stick.lx) || abs(Ps3.event.analog_changed.stick.ly))
-    driveWithControllerJoystick(Ps3.data.analog.stick.lx, Ps3.data.analog.stick.ly);
         
-  if (abs(Ps3.event.analog_changed.button.l2) || abs(Ps3.event.analog_changed.button.r2)) {
-    driveWithControllerShoulderTrigger(Ps3.data.analog.button.l2, Ps3.data.analog.button.r2);
-    Serial.println(Ps3.data.analog.button.l2, DEC);
     }
 }
 
 void callbackControllerConnect() {
     Serial.println("Controller connected to ESP32");
-
+    debugger.sendMsg(Loglevel::info, "Controller connected to ESP32")
-  delay(400);
-  Serial.print("Setting LEDs to Status "); Serial.println(drive_mode, DEC);
-  Ps3.setPlayer(drive_mode);
-}
-
-void callbackControllerDisconnect() {
-  Serial.println("Controller disconnected from ESP32");
 }
 
 void controllerPrintBattery() {
@@ -171,43 +137,3 @@ void controllerPrintBattery() {
     else if( controller_battery == ps3_status_battery_shutdown ) Serial.println("SHUTDOWN");
     else Serial.println("UNDEFINED");
 }
-
-void driveWithControllerJoystick(int8_t x, int8_t y) {
-  if (drive_mode != 1) return;
-
-  double value_per_step = MAX_SPEED * 2 / 256;
-  moveController.setSpeed((y * -1) * value_per_step);
-
-  value_per_step = MAX_ROTATION * 2 / 256;
-  moveController.setRotationspeed(x * value_per_step);
-}
-
-void driveWithControllerShoulderTrigger(uint8_t left, uint8_t right) {
-  if (drive_mode != 2) return;
-
-  map(left, 0, 255, 0, 100);
-  map(right, 0, 255, 0, 100);
-
-  left_motor.setTargetPower(left);
-  right_motor.setTargetPower(right);
-}
-
-void changeDriveMode() {
-  drive_mode++;
-  if (drive_mode > 2) {
-    drive_mode = 1;
-  }
-  Ps3.setPlayer(drive_mode);
-
-  if (drive_mode == 2) {
-    moveController.setSpeed(0);
-  }
-}
-
-void getDis(double lat1, double lon1, double lat2, double lon2) {
-  double lat = (lat1 + lat2) / 2 * 0.01745;
-  double dx = 111.3 * cos(lat) * (lon1 - lon2);
-  double dy = 111.3 * (lat1 - lat2);
-  double erg = sqrt(dx * dx + dy * dy);
-}
-

src/moveControl.cpp

@@ -64,99 +64,7 @@ void MoveControl::regulateMotors() {
         this->right_motor->setTargetPower(0);
         break;
 
-    case DrivingStatus::straightForward :
+    case DrivingStatus::forward :
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-            && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-                    && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::straightBackward :
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() >= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() <= this->right_speedometer->getSpeed()) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-            && this->right_speedometer->getSpeed() >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-                    && this->right_speedometer->getSpeed() <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::arcForwardLeft : // Identical with arcForwardRight
-        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
-
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed()) * ratio) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed()) * ratio) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-            && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-                    && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::arcForwardRight : // Identical with arcForwardLeft
         ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
 
         // Left motor
@@ -188,39 +96,7 @@ void MoveControl::regulateMotors() {
         }
         break;
 
-    case DrivingStatus::arcBackwardLeft : // Identical with arcBackwardRight
+    case DrivingStatus::backward :
-        ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
-
-        // Left motor
-        // Too slow
-        if (this->left_speedometer->getSpeed() > this->wheelspeed_left_target
-            && this->left_speedometer->getSpeed() >= (this->right_speedometer->getSpeed() * ratio)) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->left_speedometer->getSpeed() < this->wheelspeed_left_target
-                    && this->left_speedometer->getSpeed() <= (this->right_speedometer->getSpeed() * ratio)) {
-
-            this->left_motor->setTargetPower(this->left_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-
-        // Right motor
-        // Too slow
-        if (this->right_speedometer->getSpeed() > this->wheelspeed_right_target
-            && (this->right_speedometer->getSpeed() * ratio) >= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() - ACCELERATE_STEPS);
-
-        // Too fast
-        } else if (this->right_speedometer->getSpeed() < this->wheelspeed_right_target
-                    && (this->right_speedometer->getSpeed() * ratio) <= this->left_speedometer->getSpeed()) {
-
-            this->right_motor->setTargetPower(this->right_motor->getTargetPower() + ACCELERATE_STEPS);
-        }
-        break;
-
-    case DrivingStatus::arcBackwardRight : // Identical with arcBackwardLeft
         ratio = this->wheelspeed_left_target / this->wheelspeed_right_target;
 
         // Left motor

src/route.cpp

@@ -39,3 +39,10 @@ Point Route::getNextPoint() {
 uint16_t Route::getNumberOfPoints() {
     return this->count_points;
 }
+
+double Route::getDis(Point point_1, Point point_2) {
+    double lat = (point_1.lat + point_2.lat) / 2 * 0.1745;
+    double dx = 111.3 * cos(lat) * (point_1.lon - point_2.lon);
+    double dy = 111.3 * (point_1.lat - point_2.lat);
+    return sqrt(dx * dx + dy * dy);
+}

src/route.h

@@ -9,8 +9,8 @@
 #include "config.h"
 
 struct Point{
-    float N = 0;
+    float lat = 0;
-    float E = 0;
+    float lon = 0;
 };
 
 class Route {
@@ -25,6 +25,8 @@ class Route {
         Point getNextPoint();
         uint16_t getNumberOfPoints();
 
+        static double getDis(Point point_1, Point point_2);
+
 
     private:
         std::list<Point> points;