def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50-ret)
elif ret > 0:
motors.left(50+ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(), heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
def turn_to(heading, error=math.radians(4)):
pid = PID(3, 0.1, 5, -50, 50)
pid.set_target(heading)
pid.difference = compass.angleDifference
h = compass.getHeading()
while True:
ret = pid.update(h)
if ret < 0:
motors.right(50 - ret)
elif ret > 0:
motors.left(50 + ret)
if abs(compass.angleDifference(h, heading)) <= error:
motors.stop()
time.sleep(0.5)
if abs(compass.angleDifference(compass.getHeading(),
heading)) <= error:
break
h = compass.getHeading()
time.sleep(0.05)
while True:
left = ultrasonic.get_left()
middle = ultrasonic.get_middle()
right = ultrasonic.get_right()
if left <= 50 and right <= 50:
distance.stop_measuring()
motors.stop()
robot.turn_to(compass.getHeading() + math.pi, math.radians(10))
last = compass.getHeading()
distance.start_measuring(count)
motors.forward()
elif left <= 50:
distance.stop_measuring()
motors.stop()
motors.right()
time.sleep(0.5)
motors.stop()
time.sleep(0.1)
last = compass.getHeading()
distance.start_measuring(count)
motors.forward()
elif right <= 50:
distance.stop_measuring()
motors.stop()
motors.left()
time.sleep(0.5)
motors.stop()
time.sleep(0.1)
last = compass.getHeading()
distance.start_measuring(count)
while True:
left = ultrasonic.get_left()
middle = ultrasonic.get_middle()
right = ultrasonic.get_right()
if left <= 50 and right <= 50:
distance.stop_measuring()
motors.stop()
robot.turn_to(compass.getHeading() + math.pi, math.radians(10))
last = compass.getHeading()
distance.start_measuring(count)
motors.forward()
elif left <= 50:
distance.stop_measuring()
motors.stop()
motors.right()
time.sleep(0.5)
motors.stop()
time.sleep(0.1)
last = compass.getHeading()
distance.start_measuring(count)
motors.forward()
elif right <= 50:
distance.stop_measuring()
motors.stop()
motors.left()
time.sleep(0.5)
motors.stop()
time.sleep(0.1)
last = compass.getHeading()
distance.start_measuring(count)
