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

Merge branch 'main' into schaufel-funktion

Browse source
This commit is contained in:
Jens Noack 2023-11-16 09:41:29 +00:00
commit b5314bcd5d
2 changed files with 68 additions and 95 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

148
iqrobot.py
View file

@ -6,9 +6,6 @@ from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
from spike.control import wait_for_seconds from spike.control import wait_for_seconds
HELLO = "HELLO IQ" HELLO = "HELLO IQ"
BRICKIES_BOT = "brickies"
BRICKIES_BOT_2 = "brickies_2"
BACKSTEIN_BOT = "backstein"
''' '''
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor` Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
@ -16,53 +13,30 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
''' '''
class IQRobot: class IQRobot:
def __init__(self, hub: PrimeHub, typ: str): def __init__(self, hub: PrimeHub):
self.hub: PrimeHub = hub self.hub: PrimeHub = hub
self.typ=typ
if self.typ==BACKSTEIN_BOT: # Radantrieb
# Radantrieb LEFT_MOTOR_PORT = 'E'
LEFT_MOTOR_PORT = 'F' RIGHT_MOTOR_PORT = 'F'
RIGHT_MOTOR_PORT = 'B'
# Motoren für Aufsätze
FRONT_MOTOR_RIGHT_PORT = "E"
self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
elif self.typ==BRICKIES_BOT: # Motoren für Aufsätze
# Radantrieb FRONT_MOTOR_RIGHT_PORT = "B"
LEFT_MOTOR_PORT = 'E' FRONT_MOTOR_LEFT_PORT = "A"
RIGHT_MOTOR_PORT = 'F'
# Motoren für Aufsätze self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
FRONT_MOTOR_RIGHT_PORT = "B"
FRONT_MOTOR_LEFT_PORT = "A" self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
self.frontMotorLeft: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
# Radius der Antriebsräder
self.rad_radius = 2.1
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
self.abstand_rad_front = 5.55
elif self.typ==BRICKIES_BOT_2:
# Radantrieb
LEFT_MOTOR_PORT = 'E'
RIGHT_MOTOR_PORT = 'F'
# Radius der Antriebsräder
self.rad_radius = 2.9
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
self.abstand_rad_front = 8.5
## Allgemein ##
self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
# Radumfang neu berechnen und Motor konfigurieren # Radumfang neu berechnen und Motor konfigurieren
# Radius der Antriebsräder
self.rad_radius = 2.1
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
self.abstand_rad_front = 5.55
rad_umfang = 2 * math.pi * self.rad_radius rad_umfang = 2 * math.pi * self.rad_radius
self.movementMotors.set_motor_rotation(rad_umfang) self.antrieb.set_motor_rotation(rad_umfang)
self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT) self.bewegungsSensor: MotionSensor = MotionSensor()
#self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
#self.frontMotorLeft: Motor = Motor("C")
self.motionSensor: MotionSensor = MotionSensor()
def show(self, image: str): def show(self, image: str):
@ -73,88 +47,84 @@ class IQRobot:
self.hub.light_matrix.show_image(image) self.hub.light_matrix.show_image(image)
def driveForward_for_sec(self, seconds: float):
# Fahre die übergebene Anzahl seconds gerade aus
self.movementMotors.start()
wait_for_seconds(seconds)
self.movementMotors.stop()
def getColorIntensity(self):
# Ermittele Farbintensität über den Farbsensor
(red, green, blue, colorIntensity) = self .colorSensor.get_rgb_intensity()
return colorIntensity
def drehe(self, grad=90, with_reset=True): def drehe(self, grad=90, with_reset=True):
"""
Funktion um den Roboter auf der Stelle zu drehen
:param int grad: Grad um die der Roboter gedreht werden soll
mittels Vorzeichen +/- kann links oder rechts herum gedreht werden
"""
if grad == 0 or grad == 360 : if grad == 0 or grad == 360 :
print("nichts zu tun") print("nichts zu tun")
return return
if with_reset: if with_reset:
self.motionSensor.reset_yaw_angle() self.bewegungsSensor.reset_yaw_angle()
#steering = 100 if grad > 0 else -100 #steering = 100 if grad > 0 else -100
toleranz = 0 toleranz = 0
aktuell = self.motionSensor.get_yaw_angle() aktuell = self.bewegungsSensor.get_yaw_angle()
ziel = grad ziel = grad
steering = 100 if ziel > aktuell else -100 steering = 100 if ziel > aktuell else -100
self.movementMotors.start(steering=steering, speed=10) self.antrieb.start(steering=steering, speed=10)
differenz = ziel - aktuell differenz = ziel - aktuell
print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell)) print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
while abs(differenz) > toleranz : while abs(differenz) > toleranz :
aktuell = self.motionSensor.get_yaw_angle() aktuell = self.bewegungsSensor.get_yaw_angle()
differenz = ziel - aktuell differenz = ziel - aktuell
pass pass
self.movementMotors.stop() self.antrieb.stop()
print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell)) print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
def fahre_gerade(self, cm):
if self.typ == "brickies":
cm = -cm
self.motionSensor.reset_yaw_angle()
self.movementMotors.start_tank(10, 10)
self.movementMotors.set_default_speed(10)
self.movementMotors.move(cm)
versatz = self.motionSensor.get_yaw_angle()
self.drehe(grad=-versatz)
def fahre_gerade_geregelt(self, cm): def fahre_gerade_geregelt(self, cm):
if self.typ == "brickies": """
cm = -cm WIP by Lars & Klaus
self.motionSensor.reset_yaw_angle() """
self.movementMotors.start_tank(10, 10) self.bewegungsSensor.reset_yaw_angle()
self.movementMotors.set_default_speed(10) self.antrieb.start_tank(10, 10)
self.antrieb.set_default_speed(10)
sum_cm = 0 sum_cm = 0
while sum_cm < cm: while sum_cm < cm:
self.movementMotors.move(1) self.antrieb.move(1)
versatz = self.motionSensor.get_yaw_angle() versatz = self.bewegungsSensor.get_yaw_angle()
self.drehe(grad=-versatz) self.drehe(grad=-versatz)
self.motionSensor.reset_yaw_angle() self.bewegungsSensor.reset_yaw_angle()
sum_cm = sum_cm + 1 sum_cm = sum_cm + 1
self.movementMotors.move(cm - sum_cm) self.antrieb.move(cm - sum_cm)
def fahre_mit_drehung(self, strecke1, grad, strecke2): def fahre_mit_drehung(self, strecke1, grad, strecke2):
"""
Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke
Vereinfacht die Logik, da der Roboter durch die Drehung einen Versatz hat gegenüber einer
Strecke die mit dem Lineal ausgemessen wurde
"""
self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front) self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front)
self.drehe(grad) self.drehe(grad)
self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front) self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
def fahre_gerade_aus(self, cm,speed):
def fahre_gerade_aus(self, cm: float, speed: int):
"""
Funktion zum gerade aus fahren mit Korrektur am Ende
:param int cm: Zentimeter die gerade aus gefahren werden soll
:param speed: Geschwindigkeit mit der gefahren wird
"""
self.motionSensor.reset_yaw_angle() self.bewegungsSensor.reset_yaw_angle()
self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed) self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed)
drehung = self.motionSensor.get_yaw_angle() drehung = self.bewegungsSensor.get_yaw_angle()
print(drehung) print(drehung)
if drehung > 0: if drehung > 0:
richtung = -1 richtung = -1
else: else:
richtung = 1 richtung = 1
while abs(drehung) > 2: while abs(drehung) > 2:
self.movementMotors.move(amount=richtung * 0.1, steering=100) self.antrieb.move(amount=richtung * 0.1, steering=100)
drehung = self.motionSensor.get_yaw_angle() drehung = self.bewegungsSensor.get_yaw_angle()
print(drehung) print(drehung)
def schaufel(self,prozent): def schaufel(self,prozent):

15
main.py
View file

@ -74,14 +74,17 @@ dh auch an die Funktionen im importierten Code übergeben werde
hub = PrimeHub() hub = PrimeHub()
# Initialisiere Robot Klasse mit unseren Funktionen # Initialisiere Robot Klasse mit unseren Funktionen
iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT) iqRobot: iq.IQRobot = iq.IQRobot(hub)
# Führe Funktionen aus unser Robot Klasse aus: # Führe Funktionen aus unser Robot Klasse aus:
iqRobot.show('HAPPY') iqRobot.show('HAPPY')
iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10)
iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10)
def hologram_aufgabe1():
iqRobot.fahre_gerade_aus(cm=75,speed=80)
iqRobot.drehe(45, False)
iqRobot.fahre_gerade_aus(cm=14,speed=70)
iqRobot.fahre_gerade_aus(cm=-13,speed=50)
iqRobot.drehe(-45, False)
iqRobot.fahre_gerade_aus(cm=-75,speed=50)
hologram_aufgabe1()