diff --git a/driving.py b/driving.py
index 944a6e791032d488daac602a59b3162b4f733713..a06e85401c7abc2a328c5c906dc97663330d9507 100644
--- a/driving.py
+++ b/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,19 +74,22 @@ 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):
+ 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))
- else:
- y = self.y_old + (v * (-np.cos(theta) / w + 1))
- x = self.x_old + (v * (np.sin(theta) / w))
-
+ 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(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)
self.x_old = round(x, 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):
- theta_delta = (w * time_delta)
- if w < 0:
- y_delta = (v * (-np.cos(theta_delta) / w - 1))
+ 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) + 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 = ""
diff --git a/grafic.py b/grafic.py
index c8f875911d0ecf6eed77315693529c679369feb1..6ab2c7849d7d3f2b1287acfb3fe1503907325191 100644
--- a/grafic.py
+++ b/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
diff --git a/localization.py b/localization.py
index b9249bc0f0e8ff764932470eec1ba1f3f9e7dfe0..c02cce718a24b8ffa0e94a30383c5efa3c2c6e24 100644
--- a/localization.py
+++ b/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:
- [name of the section, sectiontype, array of the last sectiontypes]
- 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 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
- This three values are saved in the const-variables S, R and L.
- The array of the last sectiontypes is a simple array which contains a set of ints.
- Each int stand for one of the three states S, R or 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 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 = ""
diff --git a/race_managment.py b/race_managment.py
index dfd0fb933ef7ff254c5465ff70a3c251a8066677..ae80b8b20ad556e8da6c405759879caec93b50d1 100644
--- a/race_managment.py
+++ b/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