Compare commits
No commits in common. "a64f0c639a4ca4919d068dc00ceb614a537173d7" and "1214813e2ad7e22eed0e5d6751b1953a9b962080" have entirely different histories.
a64f0c639a
...
1214813e2a
2 changed files with 98 additions and 104 deletions
182
iqrobot.py
182
iqrobot.py
|
|
@ -2,10 +2,13 @@
|
|||
|
||||
import math
|
||||
|
||||
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
|
||||
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
|
||||
from spike.control import wait_for_seconds
|
||||
|
||||
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`
|
||||
|
|
@ -13,34 +16,53 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
|
|||
'''
|
||||
class IQRobot:
|
||||
|
||||
def __init__(self, hub: PrimeHub):
|
||||
def __init__(self, hub: PrimeHub, typ: str):
|
||||
|
||||
self.hub: PrimeHub = hub
|
||||
self.typ=typ
|
||||
|
||||
# Radantrieb
|
||||
LEFT_MOTOR_PORT = 'E'
|
||||
RIGHT_MOTOR_PORT = 'F'
|
||||
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)
|
||||
|
||||
# Motoren für Aufsätze
|
||||
FRONT_MOTOR_RIGHT_PORT = "B"
|
||||
FRONT_MOTOR_LEFT_PORT = "A"
|
||||
elif self.typ==BRICKIES_BOT:
|
||||
# Radantrieb
|
||||
LEFT_MOTOR_PORT = 'E'
|
||||
RIGHT_MOTOR_PORT = 'F'
|
||||
# Motoren für Aufsätze
|
||||
FRONT_MOTOR_RIGHT_PORT = "B"
|
||||
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)
|
||||
# Radius der Antriebsräder
|
||||
self.rad_radius = 2.1
|
||||
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
|
||||
self.abstand_rad_front = 5.55
|
||||
|
||||
self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
|
||||
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
|
||||
|
||||
self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT)
|
||||
|
||||
self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
|
||||
## Allgemein ##
|
||||
self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
|
||||
# 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
|
||||
self.rad_umfang = 2 * math.pi * self.rad_radius
|
||||
self.antrieb.set_motor_rotation(self.rad_umfang)
|
||||
|
||||
self.bewegungsSensor: MotionSensor = MotionSensor()
|
||||
|
||||
self.abstandsSensor: DistanceSensor = DistanceSensor("D")
|
||||
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):
|
||||
|
|
@ -51,115 +73,90 @@ class IQRobot:
|
|||
self.hub.light_matrix.show_image(image)
|
||||
|
||||
|
||||
def strecke_gefahren(self):
|
||||
return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
|
||||
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):
|
||||
"""
|
||||
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 :
|
||||
print("nichts zu tun")
|
||||
return
|
||||
if with_reset:
|
||||
self.bewegungsSensor.reset_yaw_angle()
|
||||
self.motionSensor.reset_yaw_angle()
|
||||
#steering = 100 if grad > 0 else -100
|
||||
toleranz = 0
|
||||
aktuell = self.bewegungsSensor.get_yaw_angle()
|
||||
aktuell = self.motionSensor.get_yaw_angle()
|
||||
ziel = grad
|
||||
steering = 100 if ziel > aktuell else -100
|
||||
self.antrieb.start(steering=steering, speed=10)
|
||||
self.movementMotors.start(steering=steering, speed=10)
|
||||
differenz = ziel - aktuell
|
||||
print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
|
||||
while abs(differenz) > toleranz :
|
||||
aktuell = self.bewegungsSensor.get_yaw_angle()
|
||||
aktuell = self.motionSensor.get_yaw_angle()
|
||||
differenz = ziel - aktuell
|
||||
pass
|
||||
self.antrieb.stop()
|
||||
self.movementMotors.stop()
|
||||
print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
|
||||
|
||||
|
||||
def fahre_gerade_aus(self, cm, speed=20):
|
||||
self.linker_motor.set_degrees_counted(0)
|
||||
self.bewegungsSensor.reset_yaw_angle()
|
||||
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)
|
||||
|
||||
self.antrieb.start_tank(10, 10)
|
||||
self.antrieb.set_default_speed(10)
|
||||
|
||||
linker_speed=speed
|
||||
rechter_speed=speed
|
||||
kp = 1.5
|
||||
ki = 1.0
|
||||
def fahre_gerade_geregelt(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)
|
||||
|
||||
sum_cm = 0
|
||||
sum_versatz = 0
|
||||
while sum_cm < cm:
|
||||
wait_for_seconds(0.05)
|
||||
sum_cm = self.strecke_gefahren()
|
||||
versatz = self.bewegungsSensor.get_yaw_angle()
|
||||
sum_versatz = sum_versatz + versatz
|
||||
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(1)
|
||||
versatz = self.motionSensor.get_yaw_angle()
|
||||
self.drehe(grad=-versatz)
|
||||
self.motionSensor.reset_yaw_angle()
|
||||
sum_cm = sum_cm + 1
|
||||
|
||||
self.movementMotors.move(cm - sum_cm)
|
||||
|
||||
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.drehe(grad)
|
||||
self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
|
||||
|
||||
def fahre_gerade_aus(self, cm,speed):
|
||||
|
||||
def fahre_gerade_aus_alt(self, cm: float, speed: int):
|
||||
"""
|
||||
Funktion zum gerade aus fahren mit Korrektur am Ende
|
||||
self.motionSensor.reset_yaw_angle()
|
||||
|
||||
:param int cm: Zentimeter die gerade aus gefahren werden soll
|
||||
: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()
|
||||
self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed)
|
||||
drehung = self.motionSensor.get_yaw_angle()
|
||||
print(drehung)
|
||||
if drehung > 0:
|
||||
richtung = -1
|
||||
else:
|
||||
richtung = 1
|
||||
while abs(drehung) > 2:
|
||||
self.antrieb.move(amount=richtung * 0.1, steering=100)
|
||||
drehung = self.bewegungsSensor.get_yaw_angle()
|
||||
self.movementMotors.move(amount=richtung * 0.1, steering=100)
|
||||
drehung = self.motionSensor.get_yaw_angle()
|
||||
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 :)")
|
||||
|
||||
|
|
@ -170,4 +167,3 @@ print("successfully loaded the IQ Lego teams code :)")
|
|||
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
14
main.py
14
main.py
|
|
@ -74,10 +74,12 @@ dh auch an die Funktionen im importierten Code übergeben werde
|
|||
hub = PrimeHub()
|
||||
|
||||
# Initialisiere Robot Klasse mit unseren Funktionen
|
||||
iqRobot: iq.IQRobot = iq.IQRobot(hub)
|
||||
iqRobot: iq.IQRobot = iq.IQRobot(hub, typ=iq.BRICKIES_BOT)
|
||||
|
||||
# Führe Funktionen aus unser Robot Klasse aus:
|
||||
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):
|
||||
iqRobot.fahre_gerade_aus(40,60)
|
||||
|
|
@ -87,11 +89,7 @@ def huenchenaufgabe(self):
|
|||
iqRobot.fahre_gerade_aus(55,60)
|
||||
iqRobot.heber(10,30)
|
||||
|
||||
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)
|
||||
huenchenaufgabe()
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue