Compare commits
14 commits
main
...
backsteinc
Author | SHA1 | Date | |
---|---|---|---|
|
aa4365c087 | ||
|
3b2ed61e01 | ||
|
377195855a | ||
|
ee76b97d83 | ||
|
756a382af3 | ||
|
324123b977 | ||
|
2dcd7cca0b | ||
|
8e7a0c2589 | ||
|
57096f642d | ||
|
2511adc3c6 | ||
|
a55bd56f0c | ||
|
fc3e54a5a4 | ||
|
b8e12603c1 | ||
|
e8fe98e0d1 |
2 changed files with 180 additions and 33 deletions
135
iqrobot.py
135
iqrobot.py
|
@ -1,9 +1,10 @@
|
||||||
# LEGO type:standard slot:6 autostart
|
# LEGO type:standard slot:6 autostart
|
||||||
|
|
||||||
from spike import PrimeHub, Motor, MotorPair, ColorSensor
|
from spike import PrimeHub, Motor, MotorPair, ColorSensor, DistanceSensor, MotionSensor
|
||||||
from spike.control import wait_for_seconds
|
from spike.control import wait_for_seconds
|
||||||
|
|
||||||
HELLO = "HELLO IQ"
|
HELLO = "HELLO IQ"
|
||||||
|
VERSION = "V1"
|
||||||
|
|
||||||
'''
|
'''
|
||||||
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
|
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
|
||||||
|
@ -11,12 +12,24 @@ damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
|
||||||
'''
|
'''
|
||||||
class IQRobot:
|
class IQRobot:
|
||||||
|
|
||||||
def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str):
|
def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str, stickMotorPort: str, gabelMotorPort: str, distanceSensorPort: str):
|
||||||
self.hub: PrimeHub = hub
|
self.hub: PrimeHub = hub
|
||||||
self.leftMotor: Motor = Motor(leftMotorPort)
|
if (leftMotorPort != ''):
|
||||||
self.rightMotor: Motor = Motor(rightMotorPort)
|
self.leftMotor: Motor = Motor(leftMotorPort)
|
||||||
self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort)
|
if (rightMotorPort != ''):
|
||||||
self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
|
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)
|
||||||
|
|
||||||
|
self.motionSensor = MotionSensor()
|
||||||
|
|
||||||
|
|
||||||
def show(self, image: str):
|
def show(self, image: str):
|
||||||
|
@ -33,15 +46,123 @@ class IQRobot:
|
||||||
wait_for_seconds(seconds)
|
wait_for_seconds(seconds)
|
||||||
self.movementMotors.stop()
|
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):
|
def getColorIntensity(self):
|
||||||
# Ermittele Farbintensität über den Farbsensor
|
# 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
|
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()
|
||||||
|
print("Disabled")
|
||||||
|
|
||||||
|
def gyroRotation(self, angle, rotate_mode = 0, stop = True):
|
||||||
|
|
||||||
|
global run_generator, runSmall
|
||||||
|
|
||||||
|
run_generator = False
|
||||||
|
|
||||||
|
speed = 30
|
||||||
|
|
||||||
|
#set standard variables
|
||||||
|
rotatedDistance = 0
|
||||||
|
steering = 1
|
||||||
|
|
||||||
|
#gyro sensor calibration
|
||||||
|
angle = angle * (2400/2443) #experimental value based on 20 rotations of the robot
|
||||||
|
|
||||||
|
#Setting variables based on inputs
|
||||||
|
loop = True
|
||||||
|
gyroStartValue = self.motionSensor.get_yaw_angle() #Yaw angle used due to orientation of the self.hub. This might need to be changed
|
||||||
|
|
||||||
|
#Inversion of steering value for turning counter clockwise
|
||||||
|
if angle < 0:
|
||||||
|
steering = -1
|
||||||
|
|
||||||
|
#Testing to see if turining is necessary, turns until loop = False
|
||||||
|
|
||||||
|
while loop:
|
||||||
|
rotatedDistance = self.motionSensor.get_yaw_angle() #Yaw angle used due to orientation of the self.hub. This might need to be changed
|
||||||
|
|
||||||
|
print(abs(rotatedDistance - gyroStartValue))
|
||||||
|
|
||||||
|
#Checking for variants
|
||||||
|
#Both Motors turn, robot moves on the spot
|
||||||
|
if rotate_mode == 0:
|
||||||
|
self.rightMotor.start_at_power(int(speed) * steering)
|
||||||
|
self.leftMotor.start_at_power(int(speed) * steering)
|
||||||
|
|
||||||
|
elif rotate_mode == 1:
|
||||||
|
|
||||||
|
if angle * speed > 0:
|
||||||
|
self.leftMotor.start_at_power(- int(speed))
|
||||||
|
else:
|
||||||
|
self.rightMotor.start_at_power(+ int(speed))
|
||||||
|
|
||||||
|
|
||||||
|
if abs(angle) <= abs(rotatedDistance - gyroStartValue):
|
||||||
|
loop = False
|
||||||
|
break
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
print("successfully loaded the IQ Lego teams code :)")
|
#Stops movement motors for increased accuracy while stopping
|
||||||
|
if stop:
|
||||||
|
self.rightMotor.stop()
|
||||||
|
self.leftMotor.stop()
|
||||||
|
|
||||||
|
return # End of gyroStraightDrive
|
||||||
|
|
||||||
|
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("Programm Library loaded")
|
78
main.py
78
main.py
|
@ -12,6 +12,7 @@ if battery.voltage() < 8000: #set threshold for battery level
|
||||||
print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
|
print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
|
||||||
+ "--------------------------------------- \n "
|
+ "--------------------------------------- \n "
|
||||||
+ "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
|
+ "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
|
||||||
|
+ "#### SONST KOMMT MARKUS OPTITZ !!!! #### \n"
|
||||||
+ "---------------------------------------- \n")
|
+ "---------------------------------------- \n")
|
||||||
else:
|
else:
|
||||||
print("Spannung der Batterie " + str(battery.voltage()) + "\n")
|
print("Spannung der Batterie " + str(battery.voltage()) + "\n")
|
||||||
|
@ -50,44 +51,69 @@ def importFile(slotid=0, precompiled=False, module_name='importFile'):
|
||||||
|
|
||||||
# Importiere Code aus der Datei "iqrobot.py"
|
# Importiere Code aus der Datei "iqrobot.py"
|
||||||
# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
|
# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
|
||||||
importFile(slotid=6, precompiled=True, module_name="iqrobot")
|
importFile(slotid=6, precompiled=False, module_name="iqrobot")
|
||||||
import iqrobot as iq
|
import iqrobot as iq
|
||||||
print(iq.HELLO)
|
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 ####################################
|
|
||||||
'''
|
|
||||||
Code zum Lösen einer Aufgabe, kann oben importierten Code nutzen
|
|
||||||
Es können auch pro Aufgabe eigene Funktionen geschrieben werden
|
|
||||||
Wichtig ist, dass die PORTS der Sensoren überall gleich sind
|
|
||||||
und auch `hub` als Instanz von PrimeHub
|
|
||||||
dh auch an die Funktionen im importierten Code übergeben werde
|
|
||||||
'''
|
|
||||||
|
|
||||||
# Definiere an welchen Ports die Sensoren angeschlossen sind
|
# Definiere an welchen Ports die Sensoren angeschlossen sind
|
||||||
COLOR_SENSOR_PORT = 'E'
|
|
||||||
LEFT_MOTOR_PORT = 'A'
|
|
||||||
RIGHT_MOTOR_PORT = 'B'
|
|
||||||
|
|
||||||
|
COLOR_SENSOR_PORT = 'D'
|
||||||
|
LEFT_MOTOR_PORT = 'B'
|
||||||
|
RIGHT_MOTOR_PORT = 'F'
|
||||||
|
STICK_MOTOR_PORT = ''
|
||||||
|
GABEL_MOTOR_PORT = 'E'
|
||||||
|
DISTANCE_SENSOR_PORT = ''
|
||||||
|
#penStatus = False
|
||||||
# Initialisieren des Hubs, der Aktoren und Sensoren
|
# Initialisieren des Hubs, der Aktoren und Sensoren
|
||||||
hub = PrimeHub()
|
hub = PrimeHub()
|
||||||
|
|
||||||
# Initialisiere Robot Klasse mit unseren Funktionen
|
# Initialisiere Robot Klasse mit unseren Funktionen
|
||||||
iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT)
|
iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT, STICK_MOTOR_PORT, GABEL_MOTOR_PORT, DISTANCE_SENSOR_PORT)
|
||||||
|
|
||||||
# Führe Funktionen aus unser Robot Klasse aus:
|
# Das Programm
|
||||||
iqRobot.show('HAPPY')
|
|
||||||
iqRobot.driveForward_for_sec(2.0)
|
# Variablen
|
||||||
colorIntensity = iqRobot.getColorIntensity()
|
penStatus = False # Oben
|
||||||
print("Farbintensität: " + str(colorIntensity))
|
|
||||||
|
|
||||||
|
# Methoden
|
||||||
|
def togglePen():
|
||||||
|
global penStatus
|
||||||
|
if(penStatus == True):
|
||||||
|
iqRobot.moveGabel(20)
|
||||||
|
penStatus = False
|
||||||
|
if(penStatus == False):
|
||||||
|
iqRobot.moveGabel(-20)
|
||||||
|
penStatus = True
|
||||||
|
|
||||||
|
def printChar(c):
|
||||||
|
global penStatus
|
||||||
|
if(c == "L"):
|
||||||
|
print("Printing the character L")
|
||||||
|
if not(penStatus == "False"):
|
||||||
|
print("Toggling pen")
|
||||||
|
togglePen()
|
||||||
|
iqRobot.driveForward_for_sec(2.0)
|
||||||
|
# iqRobot.leftMotor.run_for_degrees(360)
|
||||||
|
# iqRobot.rotateRobot(degrees=100, speed=100)
|
||||||
|
iqRobot.gyroRotation(angle=90)
|
||||||
|
iqRobot.driveForward_for_sec(1.0)
|
||||||
|
if(c == "E"):
|
||||||
|
print("Printing the character E")
|
||||||
|
if(penStatus == True):
|
||||||
|
togglePen()
|
||||||
|
iqRobot.driveForward_for_sec(0.1)
|
||||||
|
togglePen()
|
||||||
|
iqRobot.driveForward_for_sec(1.0)
|
||||||
|
togglePen()
|
||||||
|
|
||||||
|
if(c == "G"):
|
||||||
|
print("Printing the character G")
|
||||||
|
if(c == "O"):
|
||||||
|
print("Printing the character O")
|
||||||
|
|
||||||
|
wait_for_seconds(5)
|
||||||
|
|
||||||
|
printChar("L")
|
||||||
|
printChar("E")
|
||||||
|
printChar("G")
|
||||||
|
printChar("O")
|
||||||
|
|
Loading…
Reference in a new issue