Jebediahgop
Reputation: 1
EV3 Micropython thread is stuck
I start the program in VSCode on the brick and it's stuck on the thread and does nothing and I'm do not know how. I use deamons to have background processes for monitoring and use the data parallel.
#!/usr/bin/env pybricks-micropython
from pybricks.hubs import EV3Brick as brick
from pybricks.ev3devices import (Motor, ColorSensor,
UltrasonicSensor)
from pybricks.parameters import (Port, Stop, Direction, Button, Color,
SoundFile, ImageFile, Align)
from pybricks.tools import (print, wait, StopWatch, DataLog)
from pybricks.media.ev3dev import Font
import threading
import time # Importiere die time Bibliothek
from modules import color_module # Importiere das color_module
# Initialisiere EV3-Stein und Sensoren
recorded_data = color_module.measure_color_values(1) # Messung für 1 Sekunde
COLOR_THRESHOLDS = color_module.process_color_data(recorded_data)
ev3 = brick()
left_motor = Motor(Port.B, Direction.CLOCKWISE)
right_motor = Motor(Port.C, Direction.CLOCKWISE)
medium_motor = Motor(Port.D)
color_sensor = ColorSensor(Port.S3)
ultrasonic_sensor = UltrasonicSensor(Port.S2)
# Initialisiere globale Variablen
round_number = 0
start_time = 0
all_rounds_completed = False
r, g, b = color_sensor.rgb()
# PID-Parameter
Kp = 0
Ki = 0
Kd = 0
# Stoppuhr initialisieren
stopwatch = StopWatch()
# Datenprotokoll initialisieren
data_log = DataLog('time', 'angle', 'color', 'correction')
# Debugging: Ausgabe der Farbschwellenwerte
print("COLOR_THRESHOLDS:", COLOR_THRESHOLDS)
# Debugging: Ausgabe der RGB-Werte des Farbsensors
print("RGB:", color_sensor.rgb())
# Funktion zum Starten des Countdowns und Abspielen von Sound
def start_countdown_thread():
global start_time
countdown = 3
ev3.light.on(Color.RED)
while countdown > 0:
ev3.screen.clear()
ev3.screen.draw_text(60, 50, str(countdown))
if countdown == 3:
ev3.speaker.say("drei")
elif countdown == 2:
ev3.speaker.say("zwei")
ev3.light.on(Color.ORANGE)
elif countdown == 1:
ev3.speaker.say("eins")
wait(1000)
countdown -= 1
ev3.screen.clear()
ev3.light.on(Color.GREEN)
ev3.speaker.say("Los")
# Automatische Einstellung der PID-Parameter mit Ziegler-Nichols-Methode
def auto_tune_pid():
# Set initial parameters
Kp = 1
Ki = 0
Kd = 0
Ts = 0.1 # Sampling time
# Set the step change for tuning
step_change = 20
# Initialize the system state and variables
state = 'start'
last_error = 0
integral = 0
derivative = 0 # Initialize derivative
tuning_complete = False
error = 0
while not tuning_complete:
if state == 'start':
control_signal = Kp * step_change
state = 'step_change'
elif state == 'step_change':
error = step_change - last_error
integral += error * Ts
derivative = (error - last_error) / Ts
control_signal += Kp * error + Ki * integral + Kd * derivative
if last_error > 0 and error < 0:
Kc = 4 * Kp / step_change
tuning_complete = True
last_error = error
# Berechne PID parameter mit der Ziegler-Nichols Methode
Kp = 0.6 * Kc
Ki = 2 * Kp / (0.5 * Ts)
Kd = Kp * (0.125 * Ts)
return Kp, Ki, Kd
# Funktion zum Ausführen des PID-Algorithmus
def PID():
global Kp, Ki, Kd
Ts = 150
error = 0
integral = 0 # Initialisiere integral
last_error = 0 # Initialisiere last_error
for _ in range(1000):
# Farbe basierend auf Schwellenwerten klassifizieren
if r < COLOR_THRESHOLDS['black'][0] and g < COLOR_THRESHOLDS['black'][1] and b < COLOR_THRESHOLDS['black'][2]:
color = 'black'
elif r > COLOR_THRESHOLDS['red'][0] and g < COLOR_THRESHOLDS['red'][1] and b < COLOR_THRESHOLDS['red'][2]:
color = 'rot'
elif r < COLOR_THRESHOLDS['green'][0] and g > COLOR_THRESHOLDS['green'][1] and b < COLOR_THRESHOLDS['green'][2]:
color = 'grün'
elif r < COLOR_THRESHOLDS['blue'][0] and g < COLOR_THRESHOLDS['blue'][1] and b > COLOR_THRESHOLDS['blue'][2]:
color = 'blau'
elif r > COLOR_THRESHOLDS['yellow'][0] and g > COLOR_THRESHOLDS['yellow'][1] and b < COLOR_THRESHOLDS['yellow'][2]:
color = 'gelb'
else:
color = 'unbekannt'
# Debugging: Ausgabe der Farbklassifizierung
print("Color Classification:", color)
# Fehler basierend auf Farbe berechnen
integral += error
derivative = error - last_error
correction = (Kp * error + Ki * integral + Kd * derivative) * -1
# Daten aus dem PID drucken
print("Error:", error)
print("Integral:", integral)
print("Derivative:", derivative)
print("Correction:", correction)
# Korrektur auf Motoren anwenden
left_motor.run(Ts + correction)
right_motor.run(Ts - correction)
last_error = error
wait(10)
# Optimale PID-Parameter berechnen
run_pid_with_calculated_parameters()
# Funktion zum Ausführen des PID-Algorithmus mit berechneten Parametern
def run_pid():
while True:
print("123")
PID() # Hier wird die PID-Funktion aufgerufen
print("123")
# Funktion zum Ausführen des PID-Algorithmus mit berechneten Parametern
def run_pid_with_calculated_parameters():
global Kp, Ki, Kd
Kp, Ki, Kd = auto_tune_pid()
print("PID-Parameter:", Kp, Ki, Kd) # Debugging-Ausgabe der PID-Parameter
# Starten Sie den PID-Thread
pid_thread = threading.Thread(target=run_pid)
pid_thread.daemon = True
pid_thread.start()
print("PID-Thread gestartet") # Debugging-Ausgabe, um zu überprüfen, ob der Thread gestartet wurde
# Rundenfunktionen
# Funktion zum Ausführen von Runde 1
def round_1():
global round_number
ev3.speaker.say("Runde 1")
while True:
# Debugging: Ausgabe des aktuellen Farbwerts
print("Color:", color_sensor.rgb())
if color_sensor.color() == Color.GREEN:
ev3.speaker.say("Eins")
break
# Funktion für Runde 2
def round_2():
global round_number
ev3.speaker.say("Runde 2")
while True:
if ultrasonic_sensor.distance() < 100:
pick_up_palette()
if color_sensor.color() == Color.BLUE:
break
round_number += 1
# Funktion zum Ausführen von Runde 3
def round_3():
global round_number
ev3.speaker.say("Runde 3")
while True:
if ultrasonic_sensor.distance() < 100:
ev3.speaker.say("Drei")
break
# Funktion zum Ausführen von Runde 4
def round_4():
global round_number
ev3.speaker.say("Runde 4")
while True:
if color_sensor.color() == Color.GREEN:
ev3.speaker.say("Good Job")
break
# Funktion zum Ausführen von Runde 5 (Abkühlungsrunde)
def round_5():
global round_number
ev3.speaker.say("Abkühlungsrunde")
while True:
if color_sensor.color() == Color.BLUE:
break
elif color_sensor.color() == Color.RED:
ev3.speaker.say("Good Job")
left_motor.stop()
right_motor.stop()
break
def pick_up_palette():
while True:
pass
# Funktion zum Starten der Zeitmessung
def start_time_measurement():
global start_time
start_time = stopwatch.time()
# Funktion zum Anzeigen der vergangenen Zeit
def show_elapsed_time():
global start_time
elapsed_time = stopwatch.time() - start_time
ev3.screen.load_image(ImageFile.SQUARE_SMALL)
ev3.screen.draw_text(60, 50, "Zeit: " + str(elapsed_time))
# Funktion für alle Runden
def all_rounds():
global round_number
start_countdown_thread()
start_time_measurement()
run_pid_with_calculated_parameters()
round_number = 1
while round_number <= 5:
if round_number == 1:
round_1()
elif round_number == 2:
round_2()
elif round_number == 3:
round_3()
elif round_number == 4:
round_4()
elif round_number == 5:
round_5()
show_elapsed_time()
round_number += 1
# Funktion für den Hintergrundprozess zur kontinuierlichen Farbüberwachung
def color_monitor():
while True:
recorded_data = color_module.measure_color_values(1)
color_thresholds = color_module.process_color_data(recorded_data)
time.sleep(1)
# Hintergrundprozess starten
color_monitor_thread = threading.Thread(target=color_monitor)
color_monitor_thread.daemon = True
color_monitor_thread.start()
# Hauptfunktion
def main():
all_rounds()
if __name__ == "__main__":
main()
# color_module.py
from pybricks.hubs import EV3Brick as brick
from pybricks.ev3devices import ColorSensor
from pybricks.parameters import Port
from pybricks.tools import wait, StopWatch
# Globale Variablen und Objekte initialisieren
ev3 = brick()
color_sensor = ColorSensor(Port.S3)
stopwatch = StopWatch()
def measure_color_values(duration_seconds):
"""
Misst die Farbwerte für eine bestimmte Dauer und gibt sie zurück.
"""
recorded_data = []
start_time = stopwatch.time()
while stopwatch.time() - start_time < duration_seconds * 1000:
r, g, b = color_sensor.rgb()
color = color_sensor.color()
# Daten protokollieren
recorded_data.append({'time': stopwatch.time(), 'red': r, 'green': g, 'blue': b, 'color': color})
# Kurze Pause vor der nächsten Messung
wait(10)
return recorded_data
def process_color_data(recorded_data):
"""
Verarbeitet die aufgezeichneten Farbwerte und passt die Schwellenwerte dynamisch an.
"""
# Berechne die Durchschnittswerte für rote, grüne und blaue Kanäle
num_samples = len(recorded_data)
sum_r = sum(sample['red'] for sample in recorded_data)
sum_g = sum(sample['green'] for sample in recorded_data)
sum_b = sum(sample['blue'] for sample in recorded_data)
avg_r = sum_r / num_samples
avg_g = sum_g / num_samples
avg_b = sum_b / num_samples
# Passe die Schwellenwerte dynamisch an
COLOR_THRESHOLDS = {
'black': (avg_r * 0.8, avg_g * 0.8, avg_b * 0.8),
'red': (avg_r * 1.2, avg_g * 0.8, avg_b * 0.8),
'green': (avg_r * 0.8, avg_g * 1.2, avg_b * 0.8),
'blue': (avg_r * 0.8, avg_g * 0.8, avg_b * 1.2),
'yellow': (avg_r * 1.2, avg_g * 1.2, avg_b * 0.8),
}
return COLOR_THRESHOLDS
This module just has a tempory table to dynamically assume the color when the light condition changes
I was debugging with a friend and I expected that using Self tuning PID for FLL robot kills the need to tune manually. It resulted in segerrors, but now I don't get any print statements from the console
Upvotes: 0
Views: 14