def wandfahren(delay, run_event):
global speed
global linie_found
zielDist = 25
dist = distanz("L")
steer = 1
print("Distanz: ", dist)
while run_event.is_set():
speed = 4
lastDist = dist
lastSteer = steer
dist = distanz("L")
zielAngle = math.degrees(-math.atan((zielDist - dist) / 20)) # Winkel zum ziel-Abstand
angle = math.degrees(math.atan((lastDist - dist) / 10)) # Vermutlicher Winkel des Autos zur Wand
if zielAngle == angle:
steer = 1
elif angle < zielAngle:
steer = ((angle + 90) / (zielAngle + 90)) ** speed
elif angle > zielAngle:
steer = 2 - (((zielAngle + 90) / (angle + 90)) ** speed)
steer = (lastSteer + steer) / 2
if linie_found == False:
lenken(steer)
print(" " * int(dist), "█", " " * int(70 - dist), "d = %.1f cm" % dist, ";s = %.2f" % steer, ";a = %.1f°" % angle,
";z = %.1f°" % zielAngle)
print(" " * zielDist, "|")
time.sleep(delay)
def wandfahren(delay, run_event):
global wall_steer
global wall_found
global speed
zielDist = 30
dist = distanz("L")
steer = 1
print("Distanz: ", dist)
while run_event.is_set():
lastDist = dist
lastSteer = steer
dist = statistics.median([distanz("L"), distanz("L"), distanz("L"), distanz("L"), distanz("L"), distanz("L")])
if dist > 60:
wall_found = False
else:
wall_found = True
zielAngle = math.degrees(-math.atan((zielDist - dist) / 20)) # Winkel zum ziel-Abstand
angle = math.degrees(math.atan((lastDist - dist) / 10)) # Vermutlicher Winkel des Autos zur Wand
if zielAngle == angle:
steer = 1
elif angle < zielAngle:
steer = ((angle + 90) / (zielAngle + 90)) ** speed
elif angle > zielAngle:
steer = 2 - (((zielAngle + 90) / (angle + 90)) ** speed)
if steer > 2:
steer = 2
if steer < 0:
steer = 0
steer = (lastSteer + steer) / 2
setup.wall_steer = steer
print(" " * int(dist), "█", " " * int(70 - dist), "d = %.1f cm" % dist, ";s = %.2f" % steer, ";a = %.1f°" % angle,
";z = %.1f°" % zielAngle)
print(" " * zielDist, "|")
time.sleep(delay)
def wandfahren():
abstand = distanz("L")
print("Gemessene Entfernung Links = %.1f cm" % abstand)
if uGrenze <= abstand <= oGrenze:
forward()
elif abstand < uGrenze:
rechts()
elif abstand > oGrenze:
links()
threading.Timer(0.01, wandfahren).start()
def rechts():
p1.ChangeDutyCycle(.2 * speed) # Tastverhältnis ändern
p2.ChangeDutyCycle(1.5 * speed) # Motor B, 100% Tastverhältnis
def links():
p1.ChangeDutyCycle(1.5 * speed) # Tastverhältnis ändern
p2.ChangeDutyCycle(.2 * speed) # Motor B, 100% Tastverhältnis
GPIO.output(IN1, 1) # Motor A Rechtslauf
GPIO.output(IN2, 0) # Motor A Rechtslauf
GPIO.output(IN3, 1) # Motor B Rechtslauf
GPIO.output(IN4, 0) # Motor B Rechtslauf
anfangsDistanz = distanz("L")
uGrenze = anfangsDistanz - 2
oGrenze = anfangsDistanz + 2
print("Distanz: ", anfangsDistanz)
print("U: ", uGrenze)
print("O ", oGrenze)
def wandfahren():
abstand = distanz("L")
print("Gemessene Entfernung Links = %.1f cm" % abstand)
if uGrenze <= abstand <= oGrenze:
forward()
elif abstand < uGrenze:
def wandfahren(delay, run_event):
zielAngle = 0
angle = 0
zielDist = 30
steer = 0
dist = distanz("L")
lastDist = dist
vorne = 40 #distanz("R")
lastVorne = vorne
lastSteer = 1
print("Distanz: ", dist)
while run_event.is_set():
time.sleep(delay)
lastDist = dist
dist = statistics.median([
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L"),
distanz("L")
])
lastVorne = vorne
vorne = 40 #distanz("R")
lastSteer = steer
zielAngle = math.degrees(-math.atan(
(zielDist - dist) / 20)) # Winkel zum ziel-Abstand
angle = math.degrees(math.atan(
(lastDist - dist) / 10)) # Vermutlicher Winkel des Autos zur Wand
if zielAngle == angle:
steer = 1
elif angle < zielAngle:
diffAngle = angle + zielAngle
if angle < 0 and zielAngle > 0:
diffAngle = -(angle - zielAngle)
diffAngle = 1 - diffAngle / 90
steer = diffAngle
steer = ((angle + 90) / (zielAngle + 90))**speed
#steer = 0.5
elif angle > zielAngle:
diffAngle = angle + zielAngle
if angle > 0 and zielAngle < 0:
diffAngle = zielAngle - angle
diffAngle = 1 - diffAngle / 90
steer = diffAngle
steer = 2 - (((zielAngle + 90) / (angle + 90))**speed)
#steer = 1.5
if steer > 2:
steer = 2
if steer < 0:
steer = 0
steer = (lastSteer + steer) / 2
print(" " * int(dist), "█", " " * int(70 - dist), "d = %.1f cm" % dist,
";s = %.2f" % steer, ";a = %.1f°" % angle,
";z = %.1f°" % zielAngle, ";d = %.1f°" % diffAngle)
print(" " * zielDist, "|")
if vorne < 10 and lastVorne < 10:
bremsen()
print("Bremsen!")
return
elif vorne < 30 and lastVorne < 30:
lenken(2)
print("Lenken!")
time.sleep(.4)
elif abs(lastDist - dist) < 20:
lenken(steer)