Projekte
/
LegoLeague
3
0
Fork 0
Code Issues 2 Pull Requests Packages Projects Releases Wiki Activity

Merge pull request 'Funktion für neue Schaufel und Doku' (#19) from laptop02 into main 

Reviewed-on: #19
main
Lars Haferkamp 2024-01-03 16:26:50 +00:00
parent f48802abf7 88c5e7a9ba
commit 2157ec4101
2 changed files with 61 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
iqrobot.py
View File

@ -5,7 +5,8 @@ import math
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
from spike.control import wait_for_seconds
HELLO = "HELLO IQ"
print("Lade IQ-Bibliothek")
'''
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
@ -26,6 +27,10 @@ class IQRobot:
FRONT_MOTOR_LEFT_PORT = "A"
self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
self.linker_motor_vorne: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
self.linker_motor_vorne.set_stall_detection(stop_when_stalled=True)
self.rechter_motor_vorne: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
self.rechter_motor_vorne.set_stall_detection(stop_when_stalled=True)
self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT)
@ -50,9 +55,13 @@ class IQRobot:
self.hub.light_matrix.show_image(image)
def strecke_gefahren(self):
'''
Gibt die gefahrene Strecke basierend auf den Radumdrehungen zurück
'''
return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
def drehe(self, grad=90, with_reset=True):
def drehe(self, grad=90, with_reset=True, speed=10):
"""
Funktion um den Roboter auf der Stelle zu drehen
@ -68,14 +77,14 @@ class IQRobot:
# soll der Gierwinkel zurückgesetzt werden?
if with_reset:
self.bewegungsSensor.reset_yaw_angle()
self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
#steering = 100 if grad > 0 else -100
toleranz = 0
aktuell = self.bewegungsSensor.get_yaw_angle()
toleranz = 0 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht.
aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position
ziel = grad
steering = 100 if ziel > aktuell else -100
self.antrieb.start(steering=steering, speed=10)
self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten
differenz = ziel - aktuell
print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
@ -106,11 +115,13 @@ class IQRobot:
richtung = 1
if cm < 0:
richtung = -1
speed = speed * richtung
speed = speed * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren
self.linker_motor.set_degrees_counted(0)
# Alles zurücksetzen
self.linker_motor.set_degrees_counted(0)
self.bewegungsSensor.reset_yaw_angle()
# Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant
self.antrieb.start_tank(10, 10)
self.antrieb.set_default_speed(10)
@ -124,18 +135,18 @@ class IQRobot:
# wiederhole solange die gefahrene Strecke noch nicht erreicht ist
while sum_cm < cm * richtung:
wait_for_seconds(0.05)
sum_cm = self.strecke_gefahren() * richtung
versatz = self.bewegungsSensor.get_yaw_angle()
wait_for_seconds(0.05) # Sonst wird das zu oft ausgeführt
sum_cm = self.strecke_gefahren() * richtung # Gefahrene Strecke, ggf. eben negativ machen
versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch?
sum_versatz = sum_versatz + versatz
abweichung = (kp * versatz + ki * sum_versatz) / 100
abweichung = (kp * versatz + ki * sum_versatz) / 100 # Abweichung berechnen, um zu korrigieren
linker_speed = speed * (1 - abweichung * richtung)
rechter_speed = speed * (1 + abweichung * richtung)
self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed))
self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) # Mit neuer Geschwindigkeit starten
#print("Versatz: " + str(versatz) + " , linker Speed: " + str(linker_speed) + ", rechter Speed: " + str(rechter_speed) + ", strecke: " + str(sum_cm))
self.antrieb.stop()
self.drehe(-versatz)
self.antrieb.stop() # Stoppen
self.drehe(-versatz) # Da Versatz immer != 0, korrigieren
def fahre_mit_drehung(self, strecke1, grad, strecke2):
"""
@ -147,9 +158,13 @@ class IQRobot:
self.drehe(grad)
self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
#deprecated
def fahre_gerade_aus_alt(self, cm: float, speed: int):
"""
Funktion zum gerade aus fahren mit Korrektur am Ende
Wird nicht mehr aktiv genutzt, da wir jetzt fahre_gerade_aus haben,
welche geregelt ist, und der Roboter daher nicht schief wird.
:param int cm: Zentimeter die gerade aus gefahren werden soll
:param speed: Geschwindigkeit mit der gefahren wird
@ -170,14 +185,34 @@ class IQRobot:
print(drehung)
def heber(self, cm,speed):
self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed)
"""
Lässt den Heber fahren
def schaufel(self,prozent):
:param speed: Geschwindigkeit, mit der der Heber bewegt wird
:param cm: Um wie viel soll der Heber bewegt werden?
"""
self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) # Heber bewegen
def schaufel(self,prozent, speed=20):
"""
Lässt die Schaufel fahren
:param prozent: Auf wie viel Prozent soll die Schaufel bewegt werden?
"""
volle_umdrehung=0.29
rotations=volle_umdrehung*prozent/100
self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
#self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
self.bothFrontMotors.move_tank(rotations, 'rotations', speed, -speed)
# TODO: Geregeltes Fahren ist noch nicht eingebaut
def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True):
"""
Nutzt den Abstandssensor, um zu fahren, bis ein Abstand erreicht ist
:param abstand: Abstand zum Objekt
:param speed: Geschwindigkeit, mit der gefahren wird
:param geregelt: Soll mit Regler gefahren werden?
"""
self.antrieb.start_at_power(speed)
abstand_gerade = self.abstandsSensor.get_distance_cm()
while abstand_gerade > abstand:
@ -186,7 +221,7 @@ class IQRobot:
self.antrieb.stop()
print("successfully loaded the IQ Lego teams code :)")
print("Fertig geladen.")

20
main.py
View File

@ -27,11 +27,11 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
with open("/projects/.slots","rt") as f:
slots = eval(str(f.read()))
print(slots)
print(os.listdir("/projects/"+str(slots[slotid]["id"])))
#print(os.listdir("/projects/"+str(slots[slotid]["id"])))
with open("/projects/"+str(slots[slotid]["id"])+"/__init__"+suffix,"rb") as f:
print("trying to read import program")
program = f.read()
print(program)
#print(program)
try:
os.remove("/"+module_name+suffix)
except:
@ -52,14 +52,6 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
importFile(slotid=6, precompiled=True, module_name="iqrobot")
import iqrobot as iq
print(iq.HELLO)
# Importiere Go Robot Code
#importFile(slotid=3, precompiled=True, module_name="gorobot")
#import gorobot as gr
#gr.exampleFour()
#gr.db.gyroRotation(90, 25, 35, 25)
################### Hauptcode ####################################
'''
@ -120,6 +112,8 @@ def augmented_reality():
iqRobot.drehe(-90)
iqRobot.fahre_gerade_aus(5,20)
druckmaschine()
hologram()
augmented_reality()
#iqRobot.fahre_gerade_aus(16, 20)
#iqRobot.drehe(38)
#iqRobot.fahre_gerade_aus(33,25)
iqRobot.schaufel(1600, speed=100 )
iqRobot.schaufel(-1600, speed=100 )