Merge branch 'development' into 'main'

Development

See merge request sboettger1/lokalisierung-vrob!1

.gitignore

+# folder with images
+/Bilder/*
+
+# Python cache-files
+*.pyc
\ No newline at end of file

Lokalisierung_LEGO/.vscode/extensions.json

+{
+	// See http://go.microsoft.com/fwlink/?LinkId=827846 to learn about workspace recommendations.
+	// Extension identifier format: ${publisher}.${name}. Example: vscode.csharp
+
+	// List of extensions which should be recommended for users of this workspace.
+	"recommendations": [
+		"lego-education.ev3-micropython"
+	],
+	// List of extensions recommended by VS Code that should not be recommended for users of this workspace.
+	"unwantedRecommendations": [
+		"ms-python.python"
+	]
+}
\ No newline at end of file

Lokalisierung_LEGO/.vscode/launch.json

+{
+	// Use IntelliSense to learn about possible attributes.
+	// Hover to view descriptions of existing attributes.
+	// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
+	"version": "0.2.0",
+	"configurations": [
+		{
+			"name": "Download and Run",
+			"type": "ev3devBrowser",
+			"request": "launch",
+			"program": "/home/robot/${workspaceRootFolderName}/main.py",
+			"interactiveTerminal": false
+		}
+	]
+}

Lokalisierung_LEGO/.vscode/settings.json

+// Place your settings in this file to overwrite default and user settings.
+{
+	"files.eol": "\n",
+	"debug.openDebug": "openOnSessionStart",
+	"python.linting.enabled": false,
+	"python.languageServer": "None"
+}

Lokalisierung_LEGO/main.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Created on Tue May  9 12:16:54 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+
+Hi I'm Sir Ludwig. I'm a self driving robot.
+My job is it to follow a white line.
+I love my job!
+"""
+
+#%% imports
+import socket
+from datetime import datetime
+from ev3dev2.port import LegoPort
+from ev3dev2.sensor import INPUT_4, INPUT_1
+from ev3dev2.sensor.lego import ColorSensor
+from ev3dev2.motor import OUTPUT_A, OUTPUT_B, MoveTank, SpeedPercent
+from ev3dev2.sensor.lego import GyroSensor
+#import sys
+
+STUD_MM = 8
+use_gyro = 0 # 0 = ohne Gyrosensor, 1 = mit Gyrosensor
+
+#HOST = "10.128.41.108"
+#HOST = "10.128.41.90"
+#HOST = "192.168.178.81"
+HOST = "192.168.0.100"
+PORT = 50007
+
+#%% main programm
+
+print("Hello! My name is Sir Ludwig and I love to follow the line <3")
+
+s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+s.connect((HOST,PORT))
+
+tank_drive = MoveTank(OUTPUT_A, OUTPUT_B)
+max_speed = 170 / 60 # rps
+d = 120 # in mm
+radumfang = (3.141592 * 54) # in mm
+white_value = 54
+black_value = 13
+desired_light = (white_value + black_value) / 2
+t = 4.5 # in ms
+
+cs = ColorSensor(INPUT_4)
+
+gs = GyroSensor(INPUT_1)
+gs.mode = 'GYRO-RATE' # Rotationsgeschwindigkeit
+
+fehler = 0
+fehler_alt = 0
+s_r = 0
+s_r_alt = 0
+s_l = 0
+s_l_alt = 0
+
+counter_links = 0
+counter_rechts = 0
+counter_geradeaus = 0
+counter = 0
+w = 0
+v = 0
+
+time_last_send = datetime.utcnow()
+
+while (1):
+    sensorwert = cs.value()
+    fehler_alt = fehler
+    fehler = desired_light - sensorwert
+
+    ableitung = (fehler - fehler_alt) / t
+
+    turning = (0.4 * fehler) + (0.08 * ableitung)
+
+    right_motor = 30 - turning
+    left_motor = 30 + turning
+
+    tank_drive.on(SpeedPercent(left_motor), SpeedPercent(right_motor))
+    
+    right_speed =  tank_drive.right_motor.speed / 360
+    left_speed =  tank_drive.left_motor.speed / 360
+
+    v_r = right_speed * radumfang # in mm / s       
+    v_l = left_speed * radumfang # in mm / s        
+
+    v += (v_r + v_l) / 2
+
+    if (use_gyro == 0):
+        w += (v_r - v_l) / (2*d)
+    else:
+        w += (gs.value() / 180) * -3.141592
+
+    counter += 1
+    if(counter == 6):
+        w /= counter
+        v /= counter
+        v_str = str(format((round(v, 5)), '.5f')).zfill(9)
+        w_str = format((round(w, 5)), '.5f')
+        if (w >= 0):
+            w_str = "+" + w_str
+        time_delta = datetime.utcnow() - time_last_send
+        t_str = format((round(time_delta.total_seconds(), 6)), '.6f')
+        stringSend = "w=" + w_str + "v=" + v_str +"t=" + t_str
+        s.sendall(stringSend.encode())
+        time_last_send = datetime.utcnow()
+        counter = 0
+        w = 0
+        v = 0
\ No newline at end of file

README.md

-# localization  VROB
+# Toolkit for locating a vehicle on the race track
 This project deals with the localization of a vehicle on a race track. 
 This is a student project from Bochum University of Applied Sciences.
 
-## Name
-localization of a vehicle on a race track
+The main approach of localization is based on locating the vehicle based on the current and already travelled section types.
+A section type can be a right turn, a left turn and straight.
 
 ## Robot
-The used Robot is the LEGO EV3 Brick.
+The used Robot is the LEGO EV3 Brick as a line follower to test the algorithm.
+
+The robot use MicroPython.
+How to install MicroPython and set up your Visual Studio Code to connect your pc to the robot is documented in this file:
+https://assets.education.lego.com/v3/assets/blt293eea581807678a/bltb470b9ea6e38f8d4/5f8802fc4376310c19e33714/getting-started-with-micropython-v2_enus.pdf?locale=de-de
 
 Important notes regarding the robot:
 * The robot has differential drive.
@@ -16,7 +20,7 @@ Important notes regarding the robot:
     * GyroSensor: INPUT 1
 
 ## Getting started
-How to run MicroPython programs on LEGO EV3: 
+### How to run MicroPython programs on LEGO EV3: 
 1. Download and flash the EV3 MicroPython image onto a micro SD card
 2. Insert the micro SD card into the SD card slot on the EV3 Brick
 3. Download, install, and launch Visual Studio Code on your computer
@@ -25,7 +29,7 @@ How to run MicroPython programs on LEGO EV3:
     * After installing the extension, a device can be connected in Visual Studio Code (at the bottom, left).
     * The LEGO EV3 Brick should be listed and can be selected in the list.
 
-The following Python-packages must be installed on the computer to run the algorithm:
+### The following Python-packages must be installed on the computer to run the algorithm:
 * numpy
 * pandas
 * matplotlib
@@ -33,6 +37,28 @@ The following Python-packages must be installed on the computer to run the algor
 * socket
 * datetime
 
+### Preparations before run
+Clone this git. Connect the EV3 robot to the same network as your executing device.
+
+The following adjustments might be necessary, before the algorithm is executable:
+* change ip-address in Lokalisierung_LEGO/main.py to the ip-address of the executing device
+* insert the sections of the race track in localization.py (get_set_of_section_*())
+	* structure of a section: [name, section_type, previous_sections_type]
+* if the track plotter is used, insert also a set of track parts in the file grafic (get_set_of_trackparts_*()).
+    * structure of a section: [URL, name, start position(x,y), start orientation)]
+    * Images of the race tracks are not included in this git. If interested, please contact the authors.
+
+### Run
+1. Connect the robot via Visual Studio Code.
+2. Start the python file race_management.py
+    * Now some windows should pop up.
+    * You can't move this windows until the UDP-connection with the robot is connected.
+    * __When you want to stop the script you have to restart the python kernel.__
+3. Start the main.py file on the robot.
+    * To do this, select the file in the Lego-EV window in the lower left corner of Visual Studio Code. Then right click and click on "Run"
+4. Now the robot should drive and send data to the pc which plots in the console the current detected section and plot stuff.
+5. To stop the robot/program press the left upper button on the robot, or close the dashboard window.
+
 ## Description
 The main branch contains the following files:
 * localization.py:      localization algorithm
@@ -41,10 +67,6 @@ The main branch contains the following files:
 * race_management.py:   main program
 * Lokalisierung_LEGO (folder), main.py: programming of the robot LEGO EV3
 
-The following adjustments might be necessary, before the algorithm is executable:
-* change ip-address in main.py
-* entering the route in localization.py (get_set_of_section_*())
-
 The following measured values are stored and must be adjusted if necessary (in main.py):
 * wheel circumference (radumfang)
 * wheel spacing (d)
@@ -59,12 +81,16 @@ Notes on constant values (in race_management.py):
 * S = 0, R = 1, L = 2: value for a straight part, a right turn, a left turn of the track
 **These values should not be changed in order to maintain the functionality of the algorithm.**
 
+## Code overview
+![Alt text](Klassendiagramm.png)
+
 ## Authors and acknowledgment
 Jessica Dreyer, Sebastian Böttger
 
 ## License
-Project is licensed with MIT.
-See at LICENSE
+This project is licensed under the MIT License - see the LICENSE file for details.
 
 ## Project status
-Work in progress.
+Project closed.
+
+Cordial invitation to further develop this project.
\ No newline at end of file

driving.py

+# -*- coding: utf-8 -*-
+"""
+Created on Tue May  9 12:16:54 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% Imports
+import numpy as np
+
+#%% Constans
+S = 0 # Value for a straight part of the track
+R = 1 # Value for a right turn
+L = 2 # Value for a left turn
+#%% Classes
+"""
+# This class contains some functions to analyze data from the LEGO robot EV3.
+# It detects the section type depending on the rotationspeed of the robot.
+# It calculates the odometrie for x-position, y-position and direction of the robot.
+# It calculates the driven distance on a section.
+"""
+class Driving_Analyser:
+    
+    """
+    # Constructor of the driving analyser.
+    # Needs threshold values of rotationspeed for left and right turn
+    # and a block size which represents the number of iterations to analyze the section type
+    """
+    def __init__(self, threshold_left, threshold_right, block_size):
+        self.threshold_left = threshold_left
+        self.threshold_right = threshold_right
+        self.block_size = block_size
+        self.counter_left = 0;
+        self.counter_right = 0;
+        self.counter_straight = 0;
+        self.x_old = 0
+        self.y_old = 0
+        self.theta_old = 0
+        self.driven_distance_on_section = 0
+    
+    """
+    # This function detects the section type.
+    # For the detection the given threshold for left and right turns 
+    # and the block size to sum up the values are used.
+    # The function decides on the basis of the threshold values
+    # whether the current rotation speed is a left or a right turn
+    # and sums up these types in the amount of the block size to make the decision for the section type.
+    # If the function is called and the sum of values is lower than the block size it returns None
+    # If the detection of the section is completed, the function will return an integer value.
+    # 0 # Value for a straight part of the track
+    # 1 # Value for a right turn
+    # 2 # Value for a left turn
+    # The entire return statemant is a tuple with the section type, the counte for left and right turns and a counter for straight
+    # return:
+    # section_type, counter_left, counter_right, counter_straight
+    """
+    def detect_section_type(self, w):
+        section_type = None
+        if (w >= self.threshold_left):      # left turn
+            self.counter_left += 1
+        elif (w <= self.threshold_right):   # right turn
+            self.counter_right += 1
+        else:                               # straight
+            self.counter_straight += 1
+        if ((self.counter_left + self.counter_right + self.counter_straight) == self.block_size):
+            if (self.counter_left > self.counter_right and self.counter_left > self.counter_straight):
+                section_type = L
+            elif (self.counter_right > self.counter_left and self.counter_right > self.counter_straight):
+                section_type = R
+            else:
+                section_type = S
+            self.counter_left = self.counter_right = self.counter_straight = 0
+            #print(self.__get_section_type(section_type))
+        return (section_type, self.counter_left, self.counter_right, self.counter_straight)
+    
+    """
+    # This function calcuates the continues odometrie which adds the new positon to the old one.
+    # The calculation of the position depends on the speed, rotationspeed and the time span.
+    """
+    def calculate_odometrie_continues (self, w, v, time_delta):
+        
+        if w != 0: #or v != 0:
+            theta = self.theta_old + (w * time_delta)
+            x = self.x_old + (v * ((np.sin(theta) - np.sin(self.theta_old)) / w))
+            y = self.y_old + (v * (-(np.cos(theta) - np.cos(self.theta_old)) / w))
+
+        else:
+            theta = self.theta_old
+            x = self.x_old + (v * np.cos(theta) * time_delta)
+            y = self.y_old + (v * np.sin(theta) * time_delta)
+
+
+        self.theta_old = theta
+        self.y_old = round(y, 5)
+        self.x_old = round(x, 5)
+        
+        return (self.x_old, self.y_old, self.theta_old)
+    
+    """
+    # This function calcuates the delta odometrie which returns only the delta position of the given time span.
+    # The calculation of the position depends on the speed, rotationspeed and the time span.
+    """
+    def calculate_odometrie_delta (self, w, v, time_delta):
+
+        if w != 0:
+            theta_delta = (w * time_delta)
+            x_delta = (v * (np.sin(theta_delta) / w))
+            y_delta = (v * (-(np.cos(theta_delta) - np.cos(0)) / w))
+
+        else:
+            theta_delta = 0
+            x_delta = v * (np.cos(0))
+            y_delta = v * (np.sin(0))
+        
+        return (x_delta, y_delta, theta_delta)
+    
+    """
+    # This function returns the drivien distance on the section.
+    """
+    def calculate_driven_distance_on_section (self, v,time_delta):
+        self.driven_distance_on_section += v*time_delta
+        return self.driven_distance_on_section
+    
+    """
+    # This function resets the drivien distance.
+    """
+    def reset_driven_distance_on_section (self):
+        self.driven_distance_on_section = 0
+    
+    """
+    # This function is for internal use and printing only.
+    # It converts the integer values for the section type to the correct discription of the section type.
+    """
+    def __get_section_type(self, section_type_int):
+        section_type_string = ""
+        if section_type_int == S:
+            section_type_string = "straight"
+        elif section_type_int == R:
+            section_type_string = "right"
+        elif section_type_int == L:
+            section_type_string = "left"
+        return section_type_string
\ No newline at end of file

grafic.py

+# -*- coding: utf-8 -*-
+"""
+Created on Sat May  6 13:26:28 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% imports
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+from matplotlib.patches import Circle
+import matplotlib.image as img
+import tkinter as tk
+
+# not importend imports
+import time
+import random
+
+
+#%% class
+"""
+# This class can plot values as a consistant graph.
+# With the method update you can insert a new value to the graph and the grafic will refresh automaticly.
+"""
+class Line_Plotter:
+    """
+    # The constructor needs a title, for the diagramm as well as a lable for the x and y axses.
+    # Optional you can set the number of values that are plottet on the x-axses.
+    """
+    def __init__(self, title, x_lable, y_lable ,x_scale = 100):
+        self.counter = 0
+        self.fig = plt.figure()
+        self.x = np.array(range(-x_scale + 1,1))
+        self.y = np.zeros(x_scale)
+        self.ax = self.fig.add_subplot(111)
+        self.line, = self.ax.plot(self.x, self.y)
+        self.ax.set_xlim(self.x[0],self.x[self.x.size -1])
+        self.ax.set_ylim(0, 10)
+        self.ax.set_title(title)
+        self.ax.set_xlabel(x_lable)
+        self.ax.set_ylabel(y_lable)
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+        
+    """
+    # The method update insert a new value to the graph an refresh the graph automaticly.
+    # It needs only the value as a parameter.
+    """
+    def update(self, value):
+        self.y = np.append(self.y, value)
+        self.y = self.y[1:]
+        self.x = np.append(self.x, self.counter)
+        self.x = self.x[1:]
+        
+        self.line._x = self.x
+        self.line.set_ydata(self.y)
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+        
+        self.ax.set_xlim(self.x[0],self.x[self.x.size -1])
+        self.ax.set_ylim(self.y.min(), self.y.max())
+        
+        self.counter = self.counter + 1
+    
+    """
+    # The method close ensures that the window is closed.
+    """
+    def close(self):
+        plt.close(self.fig)
+
+"""
+# This class can plot values as a continues graph.
+# It can plot the values with a given timedelta to the last known value.
+"""
+class Line_Plotter_Time:
+    """
+    # The constructor needs a title, for the diagramm as well as a lable for the x and y axses.
+    # Optional you can set the number of values that are plottet on the x-axses.
+    """
+    def __init__(self, title, x_lable, y_lable ,x_scale = 100):
+        self.x_scale = x_scale
+        self.fig = plt.figure()
+        self.x = np.array(range(0,1))
+        self.y = np.zeros(1)
+        self.ax = self.fig.add_subplot(111)
+        self.line, = self.ax.plot(self.x, self.y)
+        self.ax.set_xlim(0,10)
+        self.ax.set_ylim(0, 10)
+        self.ax.set_title(title)
+        self.ax.set_xlabel(x_lable)
+        self.ax.set_ylabel(y_lable)
+        
+    """
+    # The method update insert a new value to the graph on the timestamp delte which is given an refresh the graph automaticly.
+    # It needs the value and a given time delta to the last value as a parameter.
+    # The timedelta will be summed to the hight time of a datapoint in the digramm and will plot at this time the new value.
+    """
+    def update(self, value, time_delta):
+        self.y = np.append(self.y, value)
+        self.x = np.append(self.x, self.x[self.x.size -1] + time_delta)
+        if (self.x.size > self.x_scale):
+            self.y = self.y[1:]
+            self.x = self.x[1:]
+        
+        self.line._x = self.x
+        self.line.set_ydata(self.y)
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+        
+        self.ax.set_xlim(self.x[0],self.x[self.x.size -1])
+        self.ax.set_ylim(self.y.min(), self.y.max())
+    
+    """
+    # The method close ensures that the window is closed.
+    """
+    def close(self):
+        plt.close(self.fig)
+
+"""
+# This class can plot the racetrack and highlighte the current section.
+# It should also display a little robot an update the position, but unfortunately, it's not function properly.
+"""
+class Track_Plotter:
+    """
+    # This constructor needs a set of trackparts which contains a URL to the backgroundimage for the plot.
+    """
+    def __init__(self, background_url, track, ratio = 1):
+        self.background = img.imread(background_url)
+        self.track = track
+        self.ratio = ratio
+        self.fig = plt.figure()
+        self.ax = self.fig.add_subplot(111)
+        self.live_image = self.ax.imshow(self.background, extent=[0,1000,750,0])
+        self.robot_is_active = False
+        self.robot_position = self.track.iloc[0].start_position
+        self.robot_direction = self.track.iloc[0].start_direction
+        self.ax.set_xlim(0,1000)
+        self.ax.set_ylim(750,0)
+        plt.show(block=False)
+        plt.pause(0.1)
+    
+    """
+    # Wich this funtion you get a set of trackparts. The URLs are not includet in this git.
+    """
+    def get_set_of_trackparts_3():
+        # Track plotting
+        trackparts = []
+        trackparts.append(['./Bilder/Strecke_03_S1.PNG', 'S1', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S2.PNG', 'S2', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S3.PNG', 'S3', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S4.PNG', 'S4', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S5.PNG', 'S5', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S6.PNG', 'S6', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S7.PNG', 'S7', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S8.PNG', 'S8', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S9.PNG', 'S9', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S10.PNG', 'S10', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S11.PNG', 'S11', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S12.PNG', 'S12', (0,0), 0])
+        trackparts.append(['./Bilder/Strecke_03_S13.PNG', 'S13', (0,0), 0])
+        
+        # Create DataFrame
+        track = pd.DataFrame(trackparts,columns=['image_url', 'name', 'start_position', 'start_direction'])
+        background = './Bilder/Strecke_03.png'
+        
+        return (background, track)
+    
+    """
+    # This method is internal used to create the robot. 
+    """
+    def __create_triangle(self, position, direction):
+        # Definiere die Position der Ecke des Dreiecks
+        x = position[0]
+        y = position[1]
+        
+        # Definiere die Länge der Kante, die in Richtung des Winkels theta zeigt
+        length = 100
+        
+        # Definiere den Winkel theta in Grad
+        theta_deg = direction
+        
+        # Berechne den Winkel theta in Bogenmaß
+        theta = np.deg2rad(theta_deg)
+        
+        # Berechne die Koordinaten des Dreiecks
+        x1 = x
+        y1 = y
+        
+        x2 = x + length * np.cos(theta -np.deg2rad(135))
+        y2 = y + length * np.sin(theta -np.deg2rad(135))
+        x3 = x + length * np.cos(theta -np.deg2rad(135) + np.pi/2)
+        y3 = y + length * np.sin(theta -np.deg2rad(135) + np.pi/2)
+        
+        
+        # Erstelle das Dreieck
+        self.__triangle = patches.Polygon([(x1, y1), (x2, y2), (x3, y3)], closed=True, fill=True, facecolor='green', alpha=0.5)
+        return self.__triangle
+    
+    """
+    # This function is used to select a section and set up that backgorundimage
+    """
+    def select_trackpart(self, trackpart_name):
+        trackpart = self.track[self.track.name == trackpart_name]
+        image = img.imread(trackpart.iloc[0].image_url)
+        self.live_image.set_data(image)
+        self.fig.canvas.draw()
+        self.fig.canvas.flush_events()
+    
+    """
+    # This funktion is used to set the robot to the startposition of a trackpart.
+    # Currently the robot simulation is not usable!
+    """
+    def set_robot_to_trackpart(self, trackpart_name):
+        trackpart = self.track[self.track.name == trackpart_name]
+        self.robot_position = trackpart.iloc[0].start_position
+        self.robot_direction = trackpart.iloc[0].start_direction
+        self.update_robot()
+    
+    """
+    # This funktion is used to move the robot forward.
+    # Currently the robot simulation is not usable!
+    """
+    def move_robot_forward(self, distance):
+        x,y = self.robot_position
+        theta = np.deg2rad(self.robot_direction)
+        x_new = x + distance*self.ratio * np.sin(theta)
+        y_new = y - distance*self.ratio * np.cos(theta)
+        self.robot_position = (x_new, y_new)
+        self.update_robot()
+    
+    """
+    # This funktion is used to set the robot to a specific position on the plot.
+    # Currently the robot simulation is not usable!
+    """
+    def move_robot_to_position(self, position):
+        self.robot_position += position
+        self.update_robot()
+    
+    """
+    # This funktion is used to rotate the robot.
+    # Currently the robot simulation is not usable!
+    """
+    def rotate_robot(self, rotation):
+        self.robot_direction += rotation
+        self.update_robot()
+    
+    """
+    # This funktion enable the plotting of the robot.
+    # Currently the robot simulation is not usable!
+    """
+    def activate_robot(self):
+        print("Do not use this method!!! - activate_robot")
+        self.robot_is_active = True
+        self.__circle = Circle(self.robot_position, 10, facecolor='green', edgecolor='green')
+        self.__circle = self.ax.add_artist(self.__circle)
+        self.__triangle = self.__create_triangle(self.robot_position, self.robot_direction)
+        self.__triangle = self.ax.add_artist(self.__triangle)
+        self.fig.canvas.draw_idle()
+        self.fig.canvas.flush_events()
+    
+    """
+    # This funktion is used to update the position and orientation of the robot on the plot.
+    # Currently the robot simulation is not usable!
+    """
+    def update_robot(self):
+        print("Do not use this method!!! - update_robot")
+        if self.robot_is_active:
+            try:
+                self.__circle.remove()
+                self.__triangle.remove()
+            except NotImplementedError:
+                pass
+            self.__triangle = self.triangle = self.__create_triangle(self.robot_position, self.robot_direction)
+            self.__triangle = self.ax.add_artist(self.__triangle)
+            self.__circle = Circle(self.robot_position, 15, facecolor='green', edgecolor='green')
+            self.__circle = self.ax.add_artist(self.__circle)
+            self.fig.canvas.draw_idle()
+            self.fig.canvas.flush_events()
+            
+            # self.ax.draw_artist(self.__triangle)
+            # self.ax.draw_artist(self.__circle)
+            # self.fig.canvas.blit(self.fig.bbox)
+            # self.ax.figure.canvas.flush_events()
+            
+            # try:
+            #     self.ax.draw_artist(self.__triangle)
+            #     self.ax.draw_artist(self.__circle)
+                
+            # except AttributeError:
+            #     pass
+            # self.__circle.set_center(self.robot_position)
+            # self.ax.draw_artist(self.__circle)
+            # self.fig.canvas.blit(self.fig.bbox)
+            # self.fig.canvas.flush_events()
+            
+    """
+    # This funktion close the window.
+    """
+    def close(self):
+        plt.close(self.fig)
+
+"""
+# This class creates a little Dashborad with the most important information from the robot and the localization.
+"""
+class Dashboard:
+    """
+    # This constructor needs nothing and set up all values for the dashboard.
+    """
+    def __init__(self):
+        self.current_section_type = ""
+        self.last_sections_type = ""
+        self.enter_new_section = ""
+        self.last_known_section = ""
+        self.count_fail_loc = 0
+        self.x_position = 0
+        self.y_position = 0
+        self.driven_distance_on_section = 0
+        
+        self.window = tk.Tk()
+        self.window.title("Dashboard")
+        
+        self.label_current_section_type = tk.Label(self.window, text="current section type:")
+        self.label_last_sections_type = tk.Label(self.window, text="last sections type:")
+        self.label_enter_new_section = tk.Label(self.window, text="enter new section:")
+        self.label_last_known_section = tk.Label(self.window, text="last known section:")
+        self.label_count_fail_loc = tk.Label(self.window, text="count fail loc:")
+        self.label_x_position = tk.Label(self.window, text="x position:")
+        self.label_y_position = tk.Label(self.window, text="y position:")
+        self.label_driven_distance_on_section = tk.Label(self.window, text="driven distance on section:")
+        
+        self.label_current_section_type.pack()
+        self.label_last_sections_type.pack()
+        self.label_enter_new_section.pack()
+        self.label_last_known_section.pack()
+        self.label_count_fail_loc.pack()
+        self.label_x_position.pack()
+        self.label_y_position.pack()
+        self.label_driven_distance_on_section.pack()
+        
+        self.window.protocol("WM_DELETE_WINDOW", print(""))
+        self.window.update_idletasks()
+        self.window.update()
+
+    """
+    # This function sets the current section type to the dashboard
+    """
+    def set_current_section_type(self, section_type):
+        self.current_section_type = section_type
+        self.label_current_section_type.config(text="current section type: " + self.current_section_type)
+        self.update_window()
+    
+    """
+    # This function sets the last sections type to the dashboard
+    """
+    def set_last_sections_type(self, last_sections_type):
+        self.last_sections_type = last_sections_type
+        self.label_last_sections_type.config(text="last sections type: " + self.last_sections_type)
+        self.update_window()
+    
+    """
+    # This function sets the value enter new section to the dashboard
+    """
+    def set_enter_new_section(self, enter_new_section):
+        self.enter_new_section = enter_new_section
+        self.label_enter_new_section.config(text="enter new section: " + self.enter_new_section)
+        self.update_window()
+    
+    """
+    # This function sets the last known section to the dashboard
+    """
+    def set_last_known_section(self, last_known_section):
+        self.last_known_section = last_known_section
+        self.label_last_known_section.config(text="last known section: " + self.last_known_section)
+        self.update_window()
+    
+    """
+    # This function sets the counter fail localization to the dashboard
+    """
+    def set_count_fail_loc(self, count_fail_loc):
+        self.count_fail_loc = count_fail_loc
+        self.label_count_fail_loc.config(text="count fail loc: " + str(self.count_fail_loc))
+        self.update_window()
+    
+    """
+    # This function sets the x position of the robot to the dashboard
+    """
+    def set_x_position(self, x_position):
+        self.x_position = x_position/1000
+        self.label_x_position.config(text="x position: " + str(self.x_position) + " m")
+        self.update_window()
+    
+    """
+    # This function sets the y position of the robot to the dashboard
+    """  
+    def set_y_position(self, y_position):
+        self.y_position = y_position/1000
+        self.label_y_position.config(text="y position: " + str(self.y_position) + " m")
+        self.update_window()
+    
+    """
+    # This function sets the driven distance on the section to the dashboard
+    """
+    def set_driven_distance_on_section(self, driven_distance_on_section):
+        self.driven_distance_on_section = driven_distance_on_section/1000
+        self.label_driven_distance_on_section.config(text="driven distance on section: " + str(self.driven_distance_on_section) + " m")
+        self.update_window()
+    
+    """
+    # This function update the dashboard it's self. without this method, the dashboard will not refresh.
+    """
+    def update_window(self):
+        self.window.update_idletasks()
+        self.window.update()
+
+#%% Main Skript
+
+if __name__ == '__main__':
+    # Line plotter without timing
+    #ln_plotter_1 = Line_Plotter("Linieargeschwindigkeit", "Zeitschritte", "mm/s")
+    #ln_plotter_2 = Line_Plotter("Winkelgeschwindigkeit", "Zeitschritte", "rad/s")
+    #for i in range(200):
+        #ln_plotter_1.update(random.random() * 5)
+        #ln_plotter_2.update(random.random() * 5)
+        #time.sleep(0.05)
+    
+    # Line plotter with timing
+    # ln_plotter_1 = Line_Plotter_Time("Linieargeschwindigkeit", "s", "mm/s")
+    # ln_plotter_2 = Line_Plotter_Time("Winkelgeschwindigkeit", "s", "rad/s")
+    # for i in range(200):
+    #     ln_plotter_1.update(random.random() * 5, 0.05)
+    #     ln_plotter_2.update(random.random() * 5, 0.05)
+    #     #time.sleep(0.05)
+    
+    # Track plotting
+    background, track = Track_Plotter.get_set_of_trackparts_3()
+    
+    tr_plotter = Track_Plotter(background, track)
+    #tr_plotter.activate_robot()
+    #tr_plotter.update_robot()
+    #tr_plotter.set_robot_to_trackpart('S1')
+    for i in range(3):
+        if i == 0:
+            tr_plotter.select_trackpart('S1')
+        if i ==  1:
+            tr_plotter.select_trackpart('S5')
+        if i == 2:
+            tr_plotter.select_trackpart('S9')
+        time.sleep(3)
\ No newline at end of file

localization.py

+# -*- coding: utf-8 -*-
+"""
+Created on Mon May  1 14:48:45 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% imports
+import numpy as np
+import pandas as pd
+
+#%% constants
+S = 0 # Value for a straight part of the track
+R = 1 # Value for a right turn
+L = 2 # Value for a left turn
+
+
+#%% Classes
+"""
+# This class provides methods to localize a robot on a given race track.
+# The race track is given during the creation of the object and contains the following structure:
+    [name of the section, section type, array of the last section types]
+    The name of the section is a string and can be choosen individually.
+    The sectiontyp is an int and can be one of the following states:
+        0 = straight; 1 = right turn; 2 = left turn
+        These three values are saved in the const-variables S, R and L.
+    The array of the last section types is a simple array which contains a set of ints.
+        Each int stands for one of the three states S, R or L.
+"""
+class Localizer:
+    
+    """
+    # The constructor gets a set of sections and a boolean value for debugging.
+    """
+    def __init__(self, sections, debug = False):
+        self.sections = sections
+        self.current_section_type = None
+        self.last_sections_type = [None, None, None, None, None]
+        self.enter_new_section = False
+        self.last_known_section = None
+        self.count_fail_loc = 0
+        self.debug = debug
+    
+    """
+    # This function returns a set of sections
+    """
+    def get_set_of_section_1():
+        track = []
+        track.append(["S1", S, [L,S,L,S,L]])
+        track.append(["S2", L, [S,L,S,L,S]])
+        track.append(["S3", S, [L,S,L,S,L]])
+        track.append(["S4", L, [S,L,S,L,S]])
+        track.append(["S5", S, [L,S,L,S,L]])
+        track.append(["S6", R, [S,L,S,L,S]])
+        track.append(["S7", S, [R,S,L,S,L]])
+        track.append(["S8", L, [S,R,S,L,S]])
+        track.append(["S9", S, [L,S,R,S,L]])
+        track.append(["S10", R, [S,L,S,R,S]])
+        track.append(["S11", L, [R,S,L,S,R]])
+        track.append(["S12", S, [L,R,S,L,S]])
+        track.append(["S13", L, [S,L,R,S,L]])
+        track.append(["S14", S, [L,S,L,R,S]])
+        track.append(["S15", L, [S,L,S,L,R]])
+
+        # DataFrame erstellen
+        track = pd.DataFrame(track, columns=['s_name', 'type', 'previous'])
+        return track
+    
+    """
+    # This function returns a set of sections
+    """
+    def get_set_of_section_2():
+        track = []
+        track.append(["S1", S, [L,S,L,S,L]])
+        track.append(["S2", L, [S,L,S,L,S]])
+        track.append(["S3", S, [L,S,L,S,L]])
+        track.append(["S4", L, [S,L,S,L,S]])
+        track.append(["S5", S, [L,S,L,S,L]])
+        track.append(["S6", R, [S,L,S,L,S]])
+        track.append(["S7", S, [R,S,L,S,L]])
+        track.append(["S8", L, [S,R,S,L,S]])
+        track.append(["S9", S, [L,S,R,S,L]])
+        track.append(["S10", R, [S,L,S,R,S]])
+        track.append(["S11", S, [R,S,L,S,R]])
+        track.append(["S12", L, [S,R,S,L,S]])
+        track.append(["S13", S, [L,S,R,S,L]])
+        track.append(["S14", L, [S,L,S,R,S]])
+        track.append(["S15", S, [L,S,L,S,R]])
+        track.append(["S16", L, [S,L,S,L,S]])
+
+        # DataFrame erstellen
+        track = pd.DataFrame(track, columns=['s_name', 'type', 'previous'])
+        return track
+    
+    """
+    # This function returns a set of sections
+    """
+    def get_set_of_section_3():
+        track = []
+        track.append(["S1", S, [L,S,L,R,S]])
+        track.append(["S2", L, [S,L,S,L,R]])
+        track.append(["S3", S, [L,S,L,S,L]])
+        track.append(["S4", L, [S,L,S,L,S]])
+        track.append(["S5", S, [L,S,L,S,L]])
+        track.append(["S6", R, [S,L,S,L,S]])
+        track.append(["S7", S, [R,S,L,S,L]])
+        track.append(["S8", L, [S,R,S,L,S]])
+        track.append(["S9", S, [L,S,R,S,L]])
+        track.append(["S10", R, [S,L,S,R,S]])
+        track.append(["S11", L, [R,S,L,S,R]])
+        track.append(["S12", S, [L,R,S,L,S]])
+        track.append(["S13", L, [S,L,R,S,L]])
+
+        # DataFrame erstellen
+        track = pd.DataFrame(track, columns=['s_name', 'type', 'previous'])
+        return track
+    
+    """
+    # This function updates the object values depending on the section type which is provided.
+    If the section type is the same as before nothing changes.
+    If the section type has changed, all object variables are updated and the variable "enter_new_section" is set to true.
+    """
+    def update_position(self, section_type):
+        if self.current_section_type is not section_type:
+            self.enter_new_section = True
+            self.last_sections_type = self.last_sections_type[:4]
+            self.last_sections_type.insert(0, self.current_section_type)
+            self.current_section_type = section_type
+        else:
+            self.enter_new_section = False
+    
+    """
+    # This function calculates the section where the robot is located with the highest probability
+    # The calculation is based on a given set of sections, the last five recognized sections and the current recognized section.
+    """
+    def localize(self):
+        if(self.debug):
+            print("Current: ", self.__get_section_type(self.current_section_type), " Last: ", self.last_sections_type, " Last know section:" , self.last_known_section)
+        
+        # get all sections which have the same sectiontype as the current sectiontype
+        possible_sections = self.sections[self.sections.type == self.current_section_type]
+        previous_count = 0
+        
+        # iterate the this sections and compare the previous sectiontyps of the possible section with the last know sectiontypes
+        while(len(possible_sections) > 1 and previous_count < len(self.last_sections_type)):
+            # brock up if the last known sectiontype is not filled
+            if(self.last_sections_type[previous_count] == None):
+                previous_count = 5
+            else:
+                previous_types = pd.DataFrame(possible_sections.previous.array)
+                possible_sections = possible_sections[(previous_types[previous_count] == self.last_sections_type[previous_count]).array]
+                previous_count += 1
+        # check if more than one section is possible
+        if(len(possible_sections) > 1):
+            if(self.debug):
+                print("More than one part of the track is possible for localization.")
+            if (self.last_known_section is not None):
+                
+                # Calculate distance from last known_section to all possible sections
+                index_last_known_section = self.sections[self.sections.s_name == self.last_known_section.s_name].index[0]
+                if(self.debug):
+                    print("Index of Last Section: ", index_last_known_section)
+                
+                help_sections = self.sections.copy()
+                help_sections.index += self.sections.shape[0]
+                help_sections = self.sections.copy().append(help_sections)
+                
+                filtert_section = pd.DataFrame()
+                for possible_section_s_name in np.array(possible_sections.s_name):
+                    filtert_section = filtert_section.append(help_sections[help_sections.s_name == possible_section_s_name])
+                
+                
+                indexs = abs(filtert_section.index - index_last_known_section)
+                if(self.debug):
+                    print("Distance from indexs: ", indexs)
+                
+                # Get index of entry with the smallest distance
+                min_index = np.argmin(indexs)
+                
+                # Check if there is more than one entry with the same distance like min_index
+                all_min_indices = np.where(indexs == indexs[min_index])[0]
+                
+                # If there is more than on index, localization failed
+                if(len(all_min_indices) > 1):
+                    min_index = all_min_indices[1]
+                    if(self.debug):
+                        print("No localization possible")
+                if(self.debug):
+                    print(possible_sections)
+                s_name_possible_section = filtert_section.iloc[min_index].s_name
+                possible_sections = possible_sections[possible_sections.s_name == s_name_possible_section]
+                if(self.debug):
+                    print ("Am nahliegenste Section:", possible_sections)
+            else:
+                if(self.debug):
+                    print("No localization possible - More than one part of the track can fit")
+                    print(possible_sections)
+                
+        elif(len(possible_sections) < 1):
+            self.count_fail_loc += 1
+            if(self.last_known_section is not None and self.last_sections_type[4] is not None):
+                self.current_section_type = self.last_known_section.type
+                self.last_sections_type = self.last_known_section.previous
+            if(self.debug):
+                print("No possible section was found. No localization possible.")
+            if (self.count_fail_loc == 3):
+                self.last_sections_type = [None, None, None, None, None]
+                self.last_known_section = None
+                self.count_fail_loc = 0
+        
+        if(len(possible_sections) == 1):
+            if(self.debug):
+                print(possible_sections)
+            self.last_known_section = possible_sections.iloc[0]
+            self.count_fail_loc = 0
+        
+        return possible_sections
+    
+    """
+    # This is an intern function which translates the int values to a string.
+    It is only used for debugging or printing to the consol.
+    """
+    def __get_section_type(self, section_type_int):
+        section_type_string = ""
+        if section_type_int == S:
+            section_type_string = "straight"
+        elif section_type_int == R:
+            section_type_string = "right"
+        elif section_type_int == L:
+            section_type_string = "left"
+        return section_type_string
+#%% Main
+if __name__ == '__main__':
+    lk = Localizer(Localizer.get_set_of_section_1())
+    lk.update_position(L)
+    lk.update_position(R)
+    print(lk.localize())
\ No newline at end of file

race_management.py

+# -*- coding: utf-8 -*-
+"""
+Created on Tue May  2 15:01:44 2023
+
+@author: Jessica Dreyer, Sebastian Böttger
+"""
+#%% imports
+import socket
+from datetime import datetime
+from localization import Localizer
+from driving import Driving_Analyser
+from grafic import Line_Plotter, Line_Plotter_Time, Track_Plotter, Dashboard
+
+#%% constant values
+HOST = ''                   # Symbolic name meaning all available interfaces
+PORT = 50007                # Arbitrary non-privileged port
+CHAR_RECIVING = 31          # Number of chars one UDP-message contains
+THRESHOLD_LEFT = 0.05       # Value to detect a left turn of the robot
+THRESHOLD_RIGHT = -0.05     # Value to detect a right turn of the robot
+BLOCK_SIZE_DA = 10          # Number of iterations to analyze the section type
+S = 0 # Value for a straight part of the track
+R = 1 # Value for a right turn
+L = 2 # Value for a left turn
+
+#%% Functions
+def main_loop():
+    print("TO EXIT THE PROGRAMM, CLOSE THE DASHBOARDWINDOW!!!")
+    try:
+        # create objektes
+        driving_analyser = Driving_Analyser(THRESHOLD_LEFT, THRESHOLD_RIGHT, BLOCK_SIZE_DA)
+        localizer = Localizer(Localizer.get_set_of_section_3(), False)
+        
+        # line plotter with time
+        lp_v = Line_Plotter_Time("Linear speed", "s", "mm/s", 1000)
+        lp_w = Line_Plotter_Time("Rotation speed", "s", "rad/s", 1000)
+        #lp_section_type = Line_Plotter_Time("Filtered rotation speed", "s", "current section type", 1000)
+        
+        # trackplotter
+        background, track = Track_Plotter.get_set_of_trackparts_3()
+        tp = Track_Plotter(background, track)
+        
+        # dashboard
+        dashboard = Dashboard()
+        dashboard.update_window()
+        
+        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
+            s.bind((HOST, PORT))
+            s.listen(1)
+            conn, addr = s.accept()
+            with conn:
+                print('Connected by', addr)
+                while True:
+                    dashboard.update_window()
+                    # reciving data
+                    data = conn.recv(CHAR_RECIVING)
+                    
+                    # check if data countions something
+                    if len(data) > 0:
+                        
+                        # pars data to v w and t
+                        v,w,t = pars_data(data)
+                        
+                        # update odometrie
+                        driving_analyser.calculate_odometrie_continues(w, v, t)
+                        driving_analyser.calculate_driven_distance_on_section(v, t)
+                        
+                        # update line plotter
+                        lp_v.update(v,t)
+                        lp_w.update(w,t)
+                        #lp_section_type.update(localizer.current_section_type if localizer.current_section_type is not None else 0, t)
+                        
+                        # get section type
+                        section_type,_,_,_ = driving_analyser.detect_section_type(w)
+                        
+                        # if section type is None driving_analyser still count to decide which type it is
+                        if(section_type is not None):
+                            #print(get_section_type_formated(section_type))
+                            # update localizer position
+                            localizer.update_position(section_type)
+                            update_dashboard(dashboard, localizer, driving_analyser)
+                            
+                            if(localizer.enter_new_section):
+                                driving_analyser.reset_driven_distance_on_section()
+                                section = localizer.localize()
+                                #print(section)
+                                if len(section) == 1:
+                                    print(section.iloc[0].s_name)
+                                    tp.select_trackpart(section.iloc[0].s_name)
+
+    except Exception as e:
+        print("An error occured")
+        print(e)
+    finally:
+        try:
+            dashboard.window.destroy()
+        except Exception:
+            pass 
+        lp_w.close()
+        lp_v.close()
+        #lp_section_type.close();
+        tp.close()
+        
+
+def pars_data(data):
+    data = data.decode("utf-8")
+    w = float(data[3:10])
+    if data[2] == "-":
+        w *= -1
+    v = float(data[12:21])
+    t = float(data[23:])
+    return (v,w,t)
+
+def get_section_type_formated(section_type_int):
+    section_type_string = ""
+    if section_type_int == S:
+        section_type_string = "straight"
+    elif section_type_int == R:
+        section_type_string = "right"
+    elif section_type_int == L:
+        section_type_string = "left"
+    return section_type_string
+
+def get_last_sections_type_formated(last_sections_type_int):
+    last_sections_type = ""
+    for section_type in last_sections_type_int:
+        last_sections_type += get_section_type_formated(section_type) + ', '
+    return last_sections_type
+
+def update_dashboard(dashboard, localizer, driving_analyser):
+    dashboard.set_current_section_type(get_section_type_formated(localizer.current_section_type))
+    dashboard.set_last_sections_type(get_last_sections_type_formated(localizer.last_sections_type))
+    dashboard.set_enter_new_section("Yes" if localizer.enter_new_section else "No")
+    dashboard.set_last_known_section("None" if localizer.last_known_section is None else localizer.last_known_section.s_name)
+    dashboard.set_count_fail_loc(localizer.count_fail_loc)
+    dashboard.set_x_position(driving_analyser.x_old)
+    dashboard.set_y_position(driving_analyser.y_old)
+    dashboard.set_driven_distance_on_section(driving_analyser.driven_distance_on_section)
+    #dashboard.update_window()
+
+#%% main script for race controle
+if __name__ == '__main__':
+    main_loop()
\ No newline at end of file