116 lines
3.6 KiB
Python
116 lines
3.6 KiB
Python
# LEGO type:standard slot:7 autostart
|
|
|
|
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
|
|
from spike.control import wait_for_seconds
|
|
|
|
HELLO = "HELLO IQ 2"
|
|
|
|
'''
|
|
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
|
|
damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
|
|
'''
|
|
class IQRobot:
|
|
|
|
def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str):
|
|
self.hub: PrimeHub = hub
|
|
self.leftMotor: Motor = Motor(leftMotorPort)
|
|
self.rightMotor: Motor = Motor(rightMotorPort)
|
|
self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort)
|
|
#self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
|
|
#self.frontMotorLeft: Motor = Motor("C")
|
|
self.frontMotorRight: Motor = Motor("B")
|
|
self.motionSensor: MotionSensor = MotionSensor()
|
|
|
|
|
|
def show(self, image: str):
|
|
'''
|
|
Zeige Bild auf LED Matrix des Spikes
|
|
image: Bildname wie zB 'HAPPY'
|
|
'''
|
|
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):
|
|
self.motionSensor.reset_yaw_angle()
|
|
steering = 100 if grad > 0 else -100
|
|
self.movementMotors.start(steering=steering, speed=10)
|
|
while abs(self.motionSensor.get_yaw_angle()) < abs(grad):
|
|
pass
|
|
self.movementMotors.stop()
|
|
|
|
|
|
|
|
|
|
def schreibeL(self, schreibe=True, zurueck=False):
|
|
if zurueck:
|
|
step = 5
|
|
faktor = -1
|
|
else:
|
|
step = 1
|
|
faktor = 1
|
|
print("Schreibe L")
|
|
#self.frontMotorRight.run_for_rotations(-0.4)
|
|
radius=9.5
|
|
stift_versatz=2.2
|
|
if schreibe:
|
|
self.frontMotorRight.run_for_rotations(0.4)
|
|
self.movementMotors.set_default_speed(10)
|
|
|
|
while (True):
|
|
if step == 0:
|
|
break
|
|
if step == 1:
|
|
self.movementMotors.move(faktor * 5)
|
|
if schreibe:
|
|
self.frontMotorRight.run_for_rotations(-0.4)
|
|
if step == 2:
|
|
self.movementMotors.move(faktor * (-radius - stift_versatz))
|
|
if step == 3:
|
|
self.drehe(faktor * -90)
|
|
if step == 4:
|
|
self.movementMotors.move(faktor*(radius - stift_versatz))
|
|
if schreibe:
|
|
self.frontMotorRight.run_for_rotations(0.4)
|
|
if step == 5:
|
|
self.movementMotors.move(faktor * 2)
|
|
if schreibe:
|
|
self.frontMotorRight.run_for_rotations(-0.4)
|
|
if step == 6:
|
|
break
|
|
step += faktor
|
|
# Fahre 5 cm rückwerts
|
|
# dann drehe nach rechts 90°
|
|
# und fahre 2cm fohrwärts
|
|
#stift hoch
|
|
|
|
def schreibeE(self):
|
|
print("Schreibe E")
|
|
|
|
def schreibeG(self):
|
|
print("Schreibe G")
|
|
|
|
|
|
def schreibeO(self):
|
|
print("Schreibe O")
|
|
|
|
def schreibeLego(self):
|
|
self.schreibeL()
|
|
self.schreibeL(schreibe=False, zurueck=True)
|
|
self.schreibeE()
|
|
self.schreibeG()
|
|
self.schreibeO()
|
|
|
|
print("successfully loaded the IQ Lego teams code :)")
|
|
|
|
|