def main(BP):
klt_node = 'i' #The node, where the KLT should be dropped
klt = False
#Determine the shortest route through Dijkstra
shortestWay1 = Dijkstra.dijkstra('a', klt_node)
shortestWay2 = Dijkstra.dijkstra(klt_node, 'o')
shortestWay2.pop(0)
shortestWay = shortestWay1 + ["klt"] + shortestWay2
print('My route: ' + str(shortestWay))
shortestWay.pop(0)
#Move the robot to the startnode
################
stop = follower.line_follower(BP, 400, 7, 0.01, [5])
if stop == "Red":
#functions.driveforward_mm(BP, functions.marker_offset)
print("Startkoten erreicht")
#Build a loop which leads the robot through the network by using the route calculated above by Dijkstra
################
#Your code here#
################
while len(shortestWay) != 0:
find_new_path(BP, shortestWay, klt_node)
print("Naechster Node")
stop = follower.line_follower(BP, 400, 7, 0.01, [5])
def main(BP):
beladen = False
barcode = ""
while not barcode:
barcode = functions.barcodescanner()
print("Barcode:",barcode)
stop = follower.line_follower(BP,400,7,0.01,[2])
while True:
if stop == "Blue" and not beladen:
print("Blau gefunden")
functions.driveforward_mm(BP, functions.marker_offset)
functions.turn(BP, 90)
if barcode == functions.barcodescanner():
print("Ladevorgang")
functions.lift_klt(BP)
beladen = True
functions.turn(BP, -90)
stop = follower.line_follower(BP,400,7,0.01,[5])
continue
print("Falscher KLT")
functions.turn(BP, -90)
stop = follower.line_follower(BP,400,7,0.01,[2])
if stop == "Red":
print("Fertig")
break
def main(BP):
dist_x = 1060
dist_y = 1000
x = math.sqrt(dist_x**2 + dist_y**2)
alpha = math.atan(dist_x / dist_y) / math.pi * 180 * 0.97
follower.line_follower(BP, 250, 7, 0.01, [2])
functions.driveforward_mm(BP, 120)
functions.turn(BP, -alpha)
time.sleep(1)
fahren_bis(BP, x)
def main(BP):
#Set Start- and endpoint
start = []
end = []
stop = follower.line_follower(BP,120,1,0.001,[2])
#Follow the calculated, imaginary line
#Calculate the starting angle
angle = atan((end[1]-start[1])/(end[0]-start[0]))
angle_deg = angle*180/pi
print('Starting Angle: ' + str(angle_deg))
#Calculate the distance to the endpoint
distance = sqrt((end[1]-start[1])**2 + (end[0]-start[0])**2)
print('Distance: ' + str(distance))
#Turn into the right direction
functions.turn(BP,-angle_deg)
#Set encoder and startposition
enc_right_prev = BP.get_motor_encoder(BP.PORT_A)
enc_left_prev = BP.get_motor_encoder(BP.PORT_D)
x = start[0]
y = start[1]
#While the endpoint is not reached -> Drive
endpoint = False
BP.set_motor_dps(BP.PORT_A, 300)
BP.set_motor_dps(BP.PORT_D, 300)
while endpoint is False:
time.sleep(0.1)
try:
# ... your code for recognize obstacle...
except brickpi3.SensorError as error:
print (error)
#ODOMETRY#
#Read encoders
#Calculate the distance
#Odometry for straight movement
#Set variables for the next iteration
#Check, if endpoint is reached
#Error of 5mm is allowed
print('Made it! Coordinates: ' + str(x) + ' ' + str(y))
def main(BP, target_form):
beladen = True
stop = follower.line_follower(BP, 400, 7, 0.01, [2])
while True:
if stop == "Blue" and beladen:
print("Blau gefunden")
functions.driveforward_mm(BP, functions.marker_offset)
functions.turn(BP, 90)
if functions.shape_detection(target_form):
print("Ladevorgang")
functions.drop_klt(BP)
beladen = False
functions.turn(BP, -90)
stop = follower.line_follower(BP, 250, 7, 0.01, [5])
continue
print("Falscher Abladeort")
functions.turn(BP, -90)
stop = follower.line_follower(BP, 400, 7, 0.01, [2])
if stop == "Red":
print("Fertig")
break
def main(BP):
while True:
stop = follower.line_follower(BP, 300, 7, 0.01, [5])
if stop == "Obstacle":
print("Warte 5 sek")
time.sleep(5)
if BP.get_sensor(BP.PORT_1) <= 15:
print("Umfahre Hindernis")
avoid_obst(BP)
continue
else:
print("Fahre weiter")
else:
break
def main(BP):
BROKER = "192.168.0.99"
TOPIC = "raspi5"
stop = follower.line_follower(BP, 300, 7, 0.01, [2])
functions.mqtt_pub("Angekommen", BROKER, TOPIC)
print("Angekommen published")
#BP.set_motor_position_relative(BP.PORT_B, 900)
time.sleep(0.5)
found = False
forms = ["Circle", "Rectangle", "Triangle", "Ellipse", "Tentagon"]
while not found:
msg = functions.read_mqtt(BROKER, TOPIC)
print("Msg gelesen:", msg)
for i in forms:
if msg in i:
found = True
print("Nachricht ist enthalten")
break
else:
print("Form nicht enthalten")
print("Weiter")
stop = follower.line_follower(BP, 150, 7, 0.01, [5])
return msg
# INITS
BP = brickpi3.BrickPi3()
BP.set_motor_limits(BP.PORT_A, dps=300)
BP.set_motor_limits(BP.PORT_D, dps=300)
BP.set_motor_limits(BP.PORT_B, dps=400)
BP.set_sensor_type(BP.PORT_1, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
BP.set_sensor_type(BP.PORT_2, BP.SENSOR_TYPE.EV3_ULTRASONIC_CM)
BP.set_sensor_type(BP.PORT_3, BP.SENSOR_TYPE.EV3_COLOR_REFLECTED)
BP.set_sensor_type(BP.PORT_4, BP.SENSOR_TYPE.EV3_COLOR_COLOR)
time.sleep(3)
target_form = 'Circle' #Init for Task7
# Main loop - start from anywhere!
try:
while True:
stop = follower.line_follower(BP, 150, 7, 0.01, [5])
# Follow the line to the next red marker
if stop == 'Red':
task_number = 0
functions.turn(BP, -90)
print('searching for task number')
# Checking task number
while task_number == 0:
task_number = int(functions.barcodescanner())
time.sleep(0.01)
print("recognized task: " + str(task_number))
functions.turn(BP, 90)
#Ensures that only for valid tasks marker is passed
for i in range(1, 8, 1):
