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()
logger = threading.Thread(target=supermagiskt)
logger.start()
distance.start_measuring(count)
motors.forward()
t = 0
last = compass.getHeading()
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:
