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

Merge branch 'main' into laptop01

Browse source 
* main:
  .
  Gerade aus Fahr Funktion mit PI Regler
  Docs
  Debug entfernt
  Port gefixt
  Abstandssensor
  Ungenutzten Code/Robot Config entfernt und etwas dokumentiert
  Heber Funktion
  Schaufel Funktion
  Aufgabe 07: Hologramm

# Conflicts:
#	main.py
This commit is contained in:
Lars Haferkamp 2023-11-22 20:14:36 +01:00
commit a64f0c639a
2 changed files with 104 additions and 98 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

164
iqrobot.py
View file

@ -2,13 +2,10 @@
import math import math
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
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,34 @@ 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
LEFT_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:
# Radantrieb # Radantrieb
LEFT_MOTOR_PORT = 'E' LEFT_MOTOR_PORT = 'E'
RIGHT_MOTOR_PORT = 'F' RIGHT_MOTOR_PORT = 'F'
# Motoren für Aufsätze # Motoren für Aufsätze
FRONT_MOTOR_RIGHT_PORT = "B" FRONT_MOTOR_RIGHT_PORT = "B"
FRONT_MOTOR_LEFT_PORT = "A" FRONT_MOTOR_LEFT_PORT = "A"
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) self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT)
self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
# Radumfang neu berechnen und Motor konfigurieren
# Radius der Antriebsräder # Radius der Antriebsräder
self.rad_radius = 2.1 self.rad_radius = 2.1
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
self.abstand_rad_front = 5.55 self.abstand_rad_front = 5.55
self.rad_umfang = 2 * math.pi * self.rad_radius
self.antrieb.set_motor_rotation(self.rad_umfang)
elif self.typ==BRICKIES_BOT_2: self.bewegungsSensor: MotionSensor = MotionSensor()
# 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.abstandsSensor: DistanceSensor = DistanceSensor("D")
self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
# Radumfang neu berechnen und Motor konfigurieren
rad_umfang = 2 * math.pi * self.rad_radius
self.movementMotors.set_motor_rotation(rad_umfang)
self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT)
#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,90 +51,115 @@ class IQRobot:
self.hub.light_matrix.show_image(image) self.hub.light_matrix.show_image(image)
def driveForward_for_sec(self, seconds: float): def strecke_gefahren(self):
# Fahre die übergebene Anzahl seconds gerade aus return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
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): def fahre_gerade_aus(self, cm, speed=20):
if self.typ == "brickies": self.linker_motor.set_degrees_counted(0)
cm = -cm self.bewegungsSensor.reset_yaw_angle()
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)
self.antrieb.start_tank(10, 10)
self.antrieb.set_default_speed(10)
def fahre_gerade_geregelt(self, cm): linker_speed=speed
if self.typ == "brickies": rechter_speed=speed
cm = -cm kp = 1.5
self.motionSensor.reset_yaw_angle() ki = 1.0
self.movementMotors.start_tank(10, 10)
self.movementMotors.set_default_speed(10)
sum_cm = 0 sum_cm = 0
sum_versatz = 0
while sum_cm < cm: while sum_cm < cm:
self.movementMotors.move(1) wait_for_seconds(0.05)
versatz = self.motionSensor.get_yaw_angle() sum_cm = self.strecke_gefahren()
self.drehe(grad=-versatz) versatz = self.bewegungsSensor.get_yaw_angle()
self.motionSensor.reset_yaw_angle() sum_versatz = sum_versatz + versatz
sum_cm = sum_cm + 1 abweichung = (kp * versatz + ki * sum_versatz) / 100
linker_speed = speed * (1 - abweichung)
rechter_speed = speed * (1 + abweichung)
self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed))
#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.movementMotors.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):
self.motionSensor.reset_yaw_angle() def fahre_gerade_aus_alt(self, cm: float, speed: int):
"""
Funktion zum gerade aus fahren mit Korrektur am Ende
self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed) :param int cm: Zentimeter die gerade aus gefahren werden soll
drehung = self.motionSensor.get_yaw_angle() :param speed: Geschwindigkeit mit der gefahren wird
"""
self.bewegungsSensor.reset_yaw_angle()
self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed)
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 heber(self, cm,speed):
self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed)
def schaufel(self,prozent):
volle_umdrehung=0.29
rotations=volle_umdrehung*prozent/100
self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True):
self.antrieb.start_at_power(speed)
abstand_gerade = self.abstandsSensor.get_distance_cm()
while abstand_gerade > abstand:
abstand_gerade = self.abstandsSensor.get_distance_cm()
print(str(abstand_gerade))
self.antrieb.stop()
print("successfully loaded the IQ Lego teams code :)") print("successfully loaded the IQ Lego teams code :)")
@ -167,3 +170,4 @@ print("successfully loaded the IQ Lego teams code :)")

14
main.py
View file

@ -74,12 +74,10 @@ 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 huenchenaufgabe(self): def huenchenaufgabe(self):
iqRobot.fahre_gerade_aus(40,60) iqRobot.fahre_gerade_aus(40,60)
@ -89,7 +87,11 @@ def huenchenaufgabe(self):
iqRobot.fahre_gerade_aus(55,60) iqRobot.fahre_gerade_aus(55,60)
iqRobot.heber(10,30) iqRobot.heber(10,30)
huenchenaufgabe() 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)