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)
def calibrate(duration): from robot import motors global _xoffset global _yoffset motors.left() x = [] y = [] t = time.time() while time.time() - t < duration: p = readAxisData() x.append(p[0]) y.append(p[1]) time.sleep(1 / 220) motors.stop() distances = [] for i in range(1, len(x)): p1 = (x[i - 1], y[i - 1]) p2 = (x[i], y[i]) distances.append(math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)) average_distance = sum(distances) / len(distances) new_x = [] new_y = [] for i in range(1, len(x)): p1 = (x[i - 1], y[i - 1]) p2 = (x[i], y[i]) if math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2) < average_distance: new_x.append(p2[0]) new_y.append(p2[1]) _xoffset += (min(new_x) + max(new_x)) / 2 _yoffset += (min(new_y) + max(new_y)) / 2
def calibrate(duration): from robot import motors global _xoffset global _yoffset motors.left() x = [] y = [] t = time.time() while time.time() - t < duration: p = readAxisData() x.append(p[0]) y.append(p[1]) time.sleep(1/220) motors.stop() distances = [] for i in range(1, len(x)): p1 = (x[i-1], y[i-1]) p2 = (x[i], y[i]) distances.append(math.sqrt((p1[0]-p2[0])**2 + (p1[1]-p2[1])**2)) average_distance = sum(distances)/len(distances) new_x = [] new_y = [] for i in range(1, len(x)): p1 = (x[i-1], y[i-1]) p2 = (x[i], y[i]) if math.sqrt((p1[0]-p2[0])**2 + (p1[1]-p2[1])**2) < average_distance: new_x.append(p2[0]) new_y.append(p2[1]) _xoffset += (min(new_x) + max(new_x))/2 _yoffset += (min(new_y) + max(new_y))/2
import robot.compass as compass import robot.motors as motors import robot import time try: compass.wake() compass.setHighSpeedDataRate() compass.calibrate(10) print("Samlar data...") motors.left(100) with open("data.csv", "w") as f: while True: data = compass.readAxisData() print("{},{}".format(data[0], data[1]), file=f) time.sleep(1/220) except: motors.stop() compass.sleep() robot.clean()
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) 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)