105 lines
3.7 KiB
Python
105 lines
3.7 KiB
Python
# LEGO type:standard slot:6 autostart
|
|
|
|
import math
|
|
|
|
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
|
|
from spike.control import wait_for_seconds
|
|
|
|
HELLO = "HELLO IQ"
|
|
|
|
'''
|
|
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.typ=typ
|
|
if self.typ=="backstein":
|
|
LEFT_MOTOR_PORT = 'F'
|
|
RIGHT_MOTOR_PORT = 'B'
|
|
FRONT_MOTOR_RIGHT_PORT = "E"
|
|
self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
|
|
self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
|
|
elif self.typ=="brickies":
|
|
LEFT_MOTOR_PORT = 'E'
|
|
RIGHT_MOTOR_PORT = 'F'
|
|
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)
|
|
self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
|
|
# Radumfang neu berechnen und Motor konfigurieren
|
|
rad_radius = 2.1
|
|
rad_umfang = 2 * math.pi * rad_radius
|
|
self.movementMotors.set_motor_rotation(rad_umfang)
|
|
# Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
|
|
self.abstand_rad_front = 5.55
|
|
|
|
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):
|
|
'''
|
|
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 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):
|
|
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
|
|
while sum_cm < cm:
|
|
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):
|
|
self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front)
|
|
self.drehe(grad)
|
|
self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
|
|
|
|
|
|
print("successfully loaded the IQ Lego teams code :)")
|
|
|
|
|