104 lines
No EOL
3.9 KiB
Python
104 lines
No EOL
3.9 KiB
Python
# LEGO type:standard slot:6 autostart
|
|
|
|
from spike import PrimeHub, Motor, MotorPair, ColorSensor, DistanceSensor, MotionSensor
|
|
from spike.control import wait_for_seconds
|
|
|
|
HELLO = "HELLO IQ"
|
|
VERSION = "V1"
|
|
|
|
'''
|
|
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, stickMotorPort: str, gabelMotorPort: str, distanceSensorPort: str):
|
|
self.hub: PrimeHub = hub
|
|
if (leftMotorPort != ''):
|
|
self.leftMotor: Motor = Motor(leftMotorPort)
|
|
if (rightMotorPort != ''):
|
|
self.rightMotor: Motor = Motor(rightMotorPort)
|
|
if (leftMotorPort != '' and rightMotorPort != ''):
|
|
self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort)
|
|
if (colorSensorPort != ''):
|
|
self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
|
|
if (stickMotorPort != ''):
|
|
self.stickMotor: Motor = Motor(stickMotorPort)
|
|
if (gabelMotorPort != ''):
|
|
self.gabelMotor: Motor = Motor(gabelMotorPort)
|
|
if (distanceSensorPort != ''):
|
|
self.distanceSensor: DistanceSensor = DistanceSensor(distanceSensorPort)
|
|
|
|
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 driveBackward_for_sec(self, seconds: float):
|
|
# Fahre die übergebene Anzahl seconds gerade aus
|
|
self.movementMotors.set_default_speed(-100)()
|
|
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 main(self):
|
|
|
|
'''
|
|
self.moveStick(30)
|
|
if self.colorSensor.get_reflected_light() > 1:
|
|
self.show('ANGRY')
|
|
else:
|
|
self.show('SAD')
|
|
|
|
|
|
colorIntensity = self.getColorIntensity()
|
|
print("Farbintensität: " + str(colorIntensity))
|
|
'''
|
|
|
|
def rotateRobot(self,degrees,speed) :
|
|
#Lasse den Roboter bei Bedarf um eine bestimmte Anzahl an Grad sanft und behutsam nach Links oder Rechts drehen,
|
|
# um den Nutzer zu befriedigen
|
|
self.motionSensor.reset_yaw_angle()
|
|
if (degrees > 0):
|
|
self.leftMotor.start(speed)
|
|
self.rightMotor.start(-speed)
|
|
elif (degrees < 0):
|
|
self.leftMotor.start(-speed)
|
|
self.rightMotor.start(speed)
|
|
while (self.motionSensor.get_yaw_angle() > degrees) :
|
|
self.leftMotor.stop()
|
|
self.rightMotor.stop()
|
|
|
|
def moveStick(self,degrees) :
|
|
#Bewege sanft und langsam die Schleuderstange am hinteren Rumpf des Geräts
|
|
self.stickMotor.run_for_degrees(degrees)
|
|
|
|
def moveGabel(self,degrees) :
|
|
#Bewege sanft und langsam die ungewöhnlich große sich bewegende Stange am vorderen Rumpf des Geräts
|
|
self.gabelMotor.run_for_degrees(degrees)
|
|
|
|
def getDistance (self) :
|
|
cmDistance = self.distanceSensor.get_distance_cm(short_range=False)
|
|
return cmDistance
|
|
|
|
def getShortDistance (self) :
|
|
cmDistance = self.distanceSensor.get_distance_cm(short_range=True)
|
|
return cmDistance
|
|
|
|
print("Loading library IQRobot in version " + VERSION)from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor |