Merge branch 'development' of...

Merge branch 'development' of https://gitlab.cvh-server.de/sboettger1/lokalisierung-vrob into development

driving.py

@@ -14,17 +14,17 @@ L = 2 # Value for a left turn
 
 #%% Classes
 """
-# This class countaions some functions to analyse data from the lego robot EV3.
-# It detects the type of the section depending on the rotationspeed of the robot.
-# Also it calculate the odometrie for x-position, y-position an direction of the robot
-# and can calculate the driven distance on a section.
+# 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.
-    # Need threshold values of rotationspeed for left and right turn
-    # and also a block size represent the number of left and right turns which are sumed up to classifie the section type.
+    # 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
@@ -39,14 +39,14 @@ class Driving_Analyser:
         self.driven_distance_on_section = 0
     
     """
-    # This function detect the section type.
-    # For the detection is use the given threshold for left and right turns
-    # and the block size to sum up the values.
+    # 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 are lower than the block size it's return None
-    # If the detection of the section is completed, the function will return the section_type as an integer value.
+    # 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
@@ -74,18 +74,21 @@ class Driving_Analyser:
         return (section_type, self.counter_left, self.counter_right, self.counter_straight)
     
     """
-    # This function caluates the continues odometrie which add the new positon to the old one.
-    # The calculation of the position depending on the speed, rotationspeed and the time.span.
+    # 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:
+        if w != 0: #or v != 0:
             theta = self.theta_old + (w * time_delta)
-            if w < 0:
-                y = self.y_old + (v * (-np.cos(theta) / w - 1))
+            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:
-                y = self.y_old + (v * (-np.cos(theta) / w + 1))
-            x =  self.x_old + (v * (np.sin(theta) / w))
+            theta = self.theta_old
+            x = self.x_old + (v * np.cos(self.theta_old) * time_delta)
+            y = self.y_old + (v * np.sin(self.theta_old) * time_delta)
+
 
             self.theta_old = theta
             self.y_old = round(y, 5)
@@ -94,16 +97,20 @@ class Driving_Analyser:
         return (self.x_old, self.y_old, self.theta_old)
     
     """
-    # This function caluates the delta odometrie which return only the delta position of the given time span.
-    # The calculation of the position depending on the speed, rotationspeed and the time.span.
+    # 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)
-        if w < 0:
-            y_delta = (v * (-np.cos(theta_delta) / w - 1))
+            x_delta = (v * (np.sin(theta_delta) / w))
+            y_delta = (v * (-(np.cos(theta_delta) + cos(0)) / w))
+
         else:
-            y_delta = (v * (-np.cos(theta_delta) / w + 1))
-        x_delta = (v * (np.sin(w * time_delta) / w))
+            theta_delta = 0
+            x_delta = v * (np.cos(0))
+            y_delta = v * (np.sin(0))
         
         return (x_delta, y_delta, theta_delta)
     
@@ -115,14 +122,14 @@ class Driving_Analyser:
         return self.driven_distance_on_section
     
     """
-    # This function will reset the drivien distance.
+    # This function resets the drivien distance.
     """
     def reset_driven_distance_on_section (self):
         self.driven_distance_on_section = 0
     
     """
-    # This function is only for internal use and printing.
-    # Its convert the integer values for the section type to the correct discription of the section type.
+    # 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 = ""

grafic.py

@@ -13,7 +13,7 @@ from matplotlib.patches import Circle
 import matplotlib.image as img
 import tkinter as tk
 
-# not impoertend imports
+# not importend imports
 import time
 import random
 

localization.py

@@ -8,7 +8,7 @@ Created on Mon May  1 14:48:45 2023
 import numpy as np
 import pandas as pd
 
-#%% constans
+#%% 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
@@ -17,19 +17,19 @@ 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 creatin of the object and contains the following structure:
+# 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 an can be choosen individually.
-    The sectiontyp is an int an can be one of the states:
+    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
-        This three values are saved in the const-variables S, R and L.
+        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 stand for one of the three states S, R or L.
+        Each int stands for one of the three states S, R or L.
 """
 class Localizer:
     
     """
-    # The constructor get a set of sections and a boolean value for debuging.
+    # The constructor gets a set of sections and a boolean value for debugging.
     """
     def __init__(self, sections, debug = False):
         self.sections = sections
@@ -115,9 +115,9 @@ class Localizer:
         return track
     
     """
-    # This function updates the object values depending of the section type which is provided.
-    If the section type is the same like befor nothing change.
-    If the section type hava changed, all object variables are updated and the variable "enter_new_section" is set to true.
+    # 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:
@@ -129,8 +129,8 @@ class Localizer:
             self.enter_new_section = False
     
     """
-    # This function calculate the section with the highest probability  to the current section the robot are on.
-    # The calculation based on a given set of sections, the last five recogniced sections and the current recogniced section.
+    # 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):
@@ -216,8 +216,8 @@ class Localizer:
         return possible_sections
     
     """
-    # This is a intern function which translate the int values to a string.
-    It is only used for debuging or prints at the consol.
+    # 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 = ""

race_managment.py

@@ -14,10 +14,10 @@ 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 witch one message contains
+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 analyse the sektion type
+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