WIP: Fahren mit Regler
This commit is contained in:
Lars Haferkamp
parent
31510f9e21
commit
127137e79f
3 changed files with 253 additions and 9 deletions
70
iqrobot.py
70
iqrobot.py
|
|
@ -1,6 +1,8 @@
|
|||
# LEGO type:standard slot:6 autostart
|
||||
|
||||
from spike import PrimeHub, Motor, MotorPair, ColorSensor
|
||||
import math
|
||||
|
||||
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
|
||||
from spike.control import wait_for_seconds
|
||||
|
||||
HELLO = "HELLO IQ"
|
||||
|
|
@ -13,10 +15,35 @@ 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.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):
|
||||
|
|
@ -35,11 +62,42 @@ class IQRobot:
|
|||
|
||||
def getColorIntensity(self):
|
||||
# Ermittele Farbintensität über den Farbsensor
|
||||
(red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity()
|
||||
(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 :)")
|
||||
|
|
|
|||
187
iqrobot_schreiber.py
Normal file
187
iqrobot_schreiber.py
Normal file
|
|
@ -0,0 +1,187 @@
|
|||
# LEGO type:standard slot:7 autostart
|
||||
|
||||
########################################################################
|
||||
# "ALTE" VERSION MIT DER WIR VERSUCHT HABEN DAS WORT "LEGO" ZU SCHREIBEN
|
||||
########################################################################
|
||||
|
||||
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, colorSensorPort: str, typ: 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)
|
||||
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.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
|
||||
self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT)
|
||||
self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, 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 drehe(self, grad=90, with_reset=True):
|
||||
if with_reset:
|
||||
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 drehe_robot(self, grad=90):
|
||||
if self.typ == "backstein":
|
||||
radius=9.5
|
||||
stift_versatz=2.2
|
||||
if self.typ == "brickies":
|
||||
radius=17.4
|
||||
stift_versatz=0.3
|
||||
self.fahre_gerade(-radius - stift_versatz)
|
||||
self.drehe(grad)
|
||||
self.fahre_gerade(radius - stift_versatz)
|
||||
|
||||
def fahre_gerade(self, cm, zeichne=False):
|
||||
if zeichne:
|
||||
self.bewege_stift(1) # Stift runter
|
||||
self.motionSensor.reset_yaw_angle()
|
||||
if self.typ == "brickies":
|
||||
cm = -cm
|
||||
self.movementMotors.move(cm)
|
||||
if zeichne:
|
||||
self.bewege_stift(-1) # Stift hoch
|
||||
versatz = self.motionSensor.get_yaw_angle()
|
||||
self.drehe(grad=-versatz)
|
||||
|
||||
def buchstabe_zu_segmenten(self, buchstabe):
|
||||
# Segmente um Buchstaben zu schreiben
|
||||
# 4_
|
||||
# 5 |__|3
|
||||
# 0 |6_|2
|
||||
# 1
|
||||
#
|
||||
buchstabe_zu_segmenten = {"L": [1,1,0,0,0,1,0], "E": [1,1,0,0,1,1,1], "G": [1,1,1,0,1,1,0], "O": [1,1,1,1,1,1,0]}
|
||||
return buchstabe_zu_segmenten[buchstabe]
|
||||
|
||||
|
||||
def bewege_stift(self, richtung):
|
||||
if self.typ == "backstein":
|
||||
self.frontMotorRight.run_for_rotations(richtung*0.4)
|
||||
if self.typ == "brickies":
|
||||
#print("bewege stift brickies")
|
||||
self.bothFrontMotors.move(-richtung*0.2, unit='rotations', speed=5)
|
||||
|
||||
def schreibe_buchstabe(self, buchstabe):
|
||||
print("Schreibe " + buchstabe)
|
||||
segmente = self.buchstabe_zu_segmenten(buchstabe)
|
||||
grad_drehung=-90
|
||||
self.fahre_gerade(2)
|
||||
self.drehe_robot(-grad_drehung) # drehe rechts
|
||||
for segment_nummer, segment in enumerate(segmente):
|
||||
print("Segment: " + str(segment) + " , Segment Nummer: " + str(segment_nummer))
|
||||
if segment==1:
|
||||
self.fahre_gerade(5, zeichne=True)
|
||||
else:
|
||||
self.fahre_gerade(5)
|
||||
if segment_nummer != 2 and segment_nummer != 6:
|
||||
self.drehe_robot(grad_drehung) # drehe links
|
||||
|
||||
|
||||
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 schreibeLego(self):
|
||||
#self.schreibeL()
|
||||
#self.schreibeL(schreibe=False, zurueck=True)
|
||||
self.movementMotors.set_default_speed(10)
|
||||
self.bewege_stift(-1)
|
||||
self.fahre_gerade(4, zeichne=True)
|
||||
self.drehe_robot()
|
||||
self.fahre_gerade(4, zeichne=True)
|
||||
#self.schreibe_buchstabe("L")
|
||||
#self.schreibe_buchstabe("E")
|
||||
#self.schreibe_buchstabe("G")
|
||||
#self.schreibe_buchstabe("O")
|
||||
|
||||
print("successfully loaded the IQ Lego teams code :)")
|
||||
|
||||
|
||||
5
main.py
5
main.py
|
|
@ -83,9 +83,8 @@ iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_S
|
|||
|
||||
# Führe Funktionen aus unser Robot Klasse aus:
|
||||
iqRobot.show('HAPPY')
|
||||
iqRobot.driveForward_for_sec(2.0)
|
||||
colorIntensity = iqRobot.getColorIntensity()
|
||||
print("Farbintensität: " + str(colorIntensity))
|
||||
iqRobot.fahre_mit_drehung(strecke1=10, grad=90, strecke2=10)
|
||||
iqRobot.fahre_mit_drehung(strecke1=0, grad=-90, strecke2=10)
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in a new issue