Compare commits
No commits in common. "main" and "treffen-221123" have entirely different histories.
main
...
treffen-22
2 changed files with 49 additions and 102 deletions
122
iqrobot.py
122
iqrobot.py
|
|
@ -5,8 +5,7 @@ import math
|
|||
from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor, DistanceSensor
|
||||
from spike.control import wait_for_seconds
|
||||
|
||||
print("Lade IQ-Bibliothek")
|
||||
|
||||
HELLO = "HELLO IQ"
|
||||
|
||||
'''
|
||||
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
|
||||
|
|
@ -27,10 +26,6 @@ class IQRobot:
|
|||
FRONT_MOTOR_LEFT_PORT = "A"
|
||||
|
||||
self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)
|
||||
self.linker_motor_vorne: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
|
||||
self.linker_motor_vorne.set_stall_detection(stop_when_stalled=True)
|
||||
self.rechter_motor_vorne: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
|
||||
self.rechter_motor_vorne.set_stall_detection(stop_when_stalled=True)
|
||||
|
||||
self.linker_motor: Motor = Motor(LEFT_MOTOR_PORT)
|
||||
|
||||
|
|
@ -47,6 +42,7 @@ class IQRobot:
|
|||
|
||||
self.abstandsSensor: DistanceSensor = DistanceSensor("D")
|
||||
|
||||
|
||||
def show(self, image: str):
|
||||
'''
|
||||
Zeige Bild auf LED Matrix des Spikes
|
||||
|
|
@ -54,99 +50,65 @@ class IQRobot:
|
|||
'''
|
||||
self.hub.light_matrix.show_image(image)
|
||||
|
||||
|
||||
def strecke_gefahren(self):
|
||||
'''
|
||||
Gibt die gefahrene Strecke basierend auf den Radumdrehungen zurück
|
||||
'''
|
||||
return -self.linker_motor.get_degrees_counted()/360 * self.rad_umfang
|
||||
|
||||
|
||||
def drehe(self, grad=90, with_reset=True, speed=10):
|
||||
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
|
||||
:param bool with_reset: Parameter, um den Gierwinkel zurückzusetzen, Standard: True
|
||||
"""
|
||||
|
||||
# ist überhaupt etwas zu tun für uns? d.h. grad ist enweder 0 oder 360
|
||||
if grad == 0 or grad == 360 :
|
||||
print("nichts zu tun")
|
||||
return
|
||||
|
||||
# soll der Gierwinkel zurückgesetzt werden?
|
||||
if with_reset:
|
||||
self.bewegungsSensor.reset_yaw_angle() # Gierwinkel zurücksetzen
|
||||
|
||||
self.bewegungsSensor.reset_yaw_angle()
|
||||
#steering = 100 if grad > 0 else -100
|
||||
toleranz = 0 # Toleranz soll null sein. Kann erhöht werden, falls der Roboter sich unendlich dreht.
|
||||
aktuell = self.bewegungsSensor.get_yaw_angle() # Aktuelle Position
|
||||
toleranz = 0
|
||||
aktuell = self.bewegungsSensor.get_yaw_angle()
|
||||
ziel = grad
|
||||
steering = 100 if ziel > aktuell else -100
|
||||
self.antrieb.start(steering=steering, speed=speed) # Mit bestimmer Geschwindigkeit starten
|
||||
self.antrieb.start(steering=steering, speed=10)
|
||||
differenz = ziel - aktuell
|
||||
print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
|
||||
|
||||
# wiederhole solange der Grad der Drehung noch nicht erreicht ist
|
||||
while abs(differenz) > toleranz :
|
||||
aktuell = self.bewegungsSensor.get_yaw_angle()
|
||||
differenz = ziel - aktuell
|
||||
pass
|
||||
|
||||
# stoppe die Bewegung
|
||||
self.antrieb.stop()
|
||||
print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
|
||||
|
||||
|
||||
def fahre_gerade_aus(self, cm, speed=20):
|
||||
"""
|
||||
Funktion um den Roboter geradeaus fahren zu lassen
|
||||
|
||||
:param int cm: Strecke in cm, die der Roboter geradeaus fahren soll
|
||||
:param int speed: Geschwindigkeit zum Fahren der Strecke, Standard: 20
|
||||
"""
|
||||
|
||||
# ist überhaupt etwas zu tun für uns? d.h. cm = 0
|
||||
if cm == 0 :
|
||||
print("nichts zu tun")
|
||||
return
|
||||
|
||||
# wollen wir vorwärts oder rückwarts fahren?
|
||||
richtung = 1
|
||||
if cm < 0:
|
||||
richtung = -1
|
||||
speed = speed * richtung # Die Geschwindigkeit soll negativ sein, wenn wir rückwärts fahren
|
||||
|
||||
# Alles zurücksetzen
|
||||
self.linker_motor.set_degrees_counted(0)
|
||||
self.bewegungsSensor.reset_yaw_angle()
|
||||
|
||||
# Mit irgendeiner Geschwindigkeit g>0 starten. Wert ist irrelevant
|
||||
self.antrieb.start_tank(10, 10)
|
||||
self.antrieb.set_default_speed(10)
|
||||
|
||||
linker_speed=speed # Geschwindigkeit linker Motor
|
||||
rechter_speed=speed # Geschwindigkeit rechter Motor
|
||||
kp = 1.5 # Verstärkungsfaktor zur Regelung
|
||||
ki = 1.0 # Integralfaktor zur Regelung
|
||||
sum_cm = 0 # bereits gefahrene Strecke
|
||||
versatz = 0 # aktueller Versatz
|
||||
sum_versatz = 0 # Summe des Versatzes über Zeit
|
||||
|
||||
# wiederhole solange die gefahrene Strecke noch nicht erreicht ist
|
||||
while sum_cm < cm * richtung:
|
||||
wait_for_seconds(0.05) # Sonst wird das zu oft ausgeführt
|
||||
sum_cm = self.strecke_gefahren() * richtung # Gefahrene Strecke, ggf. eben negativ machen
|
||||
versatz = self.bewegungsSensor.get_yaw_angle() # Um wie viel sind wir falsch?
|
||||
linker_speed=speed
|
||||
rechter_speed=speed
|
||||
kp = 1.5
|
||||
ki = 1.0
|
||||
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 # Abweichung berechnen, um zu korrigieren
|
||||
linker_speed = speed * (1 - abweichung * richtung)
|
||||
rechter_speed = speed * (1 + abweichung * richtung)
|
||||
self.antrieb.start_tank_at_power(int(linker_speed), int(rechter_speed)) # Mit neuer Geschwindigkeit starten
|
||||
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() # Stoppen
|
||||
self.drehe(-versatz) # Da Versatz immer != 0, korrigieren
|
||||
self.antrieb.stop()
|
||||
self.drehe(-versatz)
|
||||
|
||||
|
||||
def fahre_mit_drehung(self, strecke1, grad, strecke2):
|
||||
"""
|
||||
|
|
@ -159,12 +121,9 @@ class IQRobot:
|
|||
self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
|
||||
|
||||
|
||||
#deprecated
|
||||
def fahre_gerade_aus_alt(self, cm: float, speed: int):
|
||||
"""
|
||||
Funktion zum gerade aus fahren mit Korrektur am Ende
|
||||
Wird nicht mehr aktiv genutzt, da wir jetzt fahre_gerade_aus haben,
|
||||
welche geregelt ist, und der Roboter daher nicht schief wird.
|
||||
|
||||
:param int cm: Zentimeter die gerade aus gefahren werden soll
|
||||
:param speed: Geschwindigkeit mit der gefahren wird
|
||||
|
|
@ -185,34 +144,15 @@ class IQRobot:
|
|||
print(drehung)
|
||||
|
||||
def heber(self, cm,speed):
|
||||
"""
|
||||
Lässt den Heber fahren
|
||||
self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed)
|
||||
|
||||
:param speed: Geschwindigkeit, mit der der Heber bewegt wird
|
||||
:param cm: Um wie viel soll der Heber bewegt werden?
|
||||
"""
|
||||
self.bothFrontMotors.move_tank(-cm*3.3,"cm", -speed, speed) # Heber bewegen
|
||||
|
||||
def schaufel(self,prozent, speed=20):
|
||||
"""
|
||||
Lässt die Schaufel fahren
|
||||
|
||||
:param prozent: Auf wie viel Prozent soll die Schaufel bewegt werden?
|
||||
"""
|
||||
def schaufel(self,prozent):
|
||||
volle_umdrehung=0.29
|
||||
rotations=volle_umdrehung*prozent/100
|
||||
#self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
|
||||
self.bothFrontMotors.move_tank(rotations, 'rotations', speed, -speed)
|
||||
self.bothFrontMotors.move(rotations, unit='rotations',speed=20)
|
||||
|
||||
|
||||
# TODO: Geregeltes Fahren ist noch nicht eingebaut
|
||||
def fahre_bis_abstand(self, abstand: int, speed=30, geregelt=True):
|
||||
"""
|
||||
Nutzt den Abstandssensor, um zu fahren, bis ein Abstand erreicht ist
|
||||
|
||||
:param abstand: Abstand zum Objekt
|
||||
:param speed: Geschwindigkeit, mit der gefahren wird
|
||||
:param geregelt: Soll mit Regler gefahren werden?
|
||||
"""
|
||||
self.antrieb.start_at_power(speed)
|
||||
abstand_gerade = self.abstandsSensor.get_distance_cm()
|
||||
while abstand_gerade > abstand:
|
||||
|
|
@ -221,7 +161,7 @@ class IQRobot:
|
|||
self.antrieb.stop()
|
||||
|
||||
|
||||
print("Fertig geladen.")
|
||||
print("successfully loaded the IQ Lego teams code :)")
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
21
main.py
21
main.py
|
|
@ -27,11 +27,11 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
|
|||
with open("/projects/.slots","rt") as f:
|
||||
slots = eval(str(f.read()))
|
||||
print(slots)
|
||||
#print(os.listdir("/projects/"+str(slots[slotid]["id"])))
|
||||
print(os.listdir("/projects/"+str(slots[slotid]["id"])))
|
||||
with open("/projects/"+str(slots[slotid]["id"])+"/__init__"+suffix,"rb") as f:
|
||||
print("trying to read import program")
|
||||
program = f.read()
|
||||
#print(program)
|
||||
print(program)
|
||||
try:
|
||||
os.remove("/"+module_name+suffix)
|
||||
except:
|
||||
|
|
@ -52,6 +52,14 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
|
|||
# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
|
||||
importFile(slotid=6, precompiled=True, module_name="iqrobot")
|
||||
import iqrobot as iq
|
||||
print(iq.HELLO)
|
||||
|
||||
# Importiere Go Robot Code
|
||||
#importFile(slotid=3, precompiled=True, module_name="gorobot")
|
||||
#import gorobot as gr
|
||||
#gr.exampleFour()
|
||||
#gr.db.gyroRotation(90, 25, 35, 25)
|
||||
|
||||
|
||||
################### Hauptcode ####################################
|
||||
'''
|
||||
|
|
@ -112,8 +120,7 @@ def augmented_reality():
|
|||
iqRobot.drehe(-90)
|
||||
iqRobot.fahre_gerade_aus(5,20)
|
||||
|
||||
#iqRobot.fahre_gerade_aus(16, 20)
|
||||
#iqRobot.drehe(38)
|
||||
#iqRobot.fahre_gerade_aus(33,25)
|
||||
iqRobot.schaufel(1600, speed=100 )
|
||||
iqRobot.schaufel(-1600, speed=100 )
|
||||
|
||||
druckmaschine()
|
||||
hologram()
|
||||
augmented_reality()
|
||||
|
|
|
|||
Loading…
Reference in a new issue