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)