Compare revisions

# folder with images

/Bilder/*

# Python cache-files

*.pyc

 No newline at end of file

{

	// 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

{

	// 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

		}

	]

}

// 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"

}

#!/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

# 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.

    * 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

    * 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

* 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

* 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)

* 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

# -*- 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

# -*- 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])