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


#print("Links: ", tank_drive.left_motor.speed, file=sys.stderr)
#print("Rechts: ", tank_drive.right_motor.speed, file=sys.stderr)
#right_speed = (right_motor / 100) * max_speed
#left_speed = (left_motor / 100) * max_speed
#print("links: ", left_speed, file=sys.stderr)
#print("rechts: ", right_speed, file=sys.stderr)
#print("Links: ", tank_drive.left_motor.speed / 360, file=sys.stderr)
#print("Rechts: ", tank_drive.right_motor.speed / 360, file=sys.stderr)