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)
last = compass.getHeading() distance.start_measuring(count) motors.forward() elif middle <= 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() if time.time() - t >= 0.5: if abs(compass.angleDifference(compass.getHeading(), last)) >= math.radians(15): distance.stop_measuring() motors.stop() done = True logger.join() a = compass.getHeading() compass.calibrate(3) done = False logger = threading.Thread(target=supermagiskt) logger.start() robot.turn_to(a, math.radians(20)) time.sleep(1) distance.start_measuring(count) motors.forward() t = time.time() last = compass.getHeading()