def move_to(self, target, power=100):
"""Move head to DeltaCoord target."""
new_wires = wires.wires_from_coord(target)
print new_wires.a, new_wires.b, new_wires.c
diff_wires = self.wires.diff(new_wires)
print diff_wires.a, diff_wires.b, diff_wires.c
# Adjust power to achieve straightish movement
# largest = diff_wires.a;
# if diff_wires.b > largest:
# largest = diff_wires.b;
# if diff_wires.c > largest:
# largest = diff_wires.c;
# Issue motor commands
a, b, c = diff_wires.to_rotations()
self.motors.move_a(int(-math.floor(a)), power)
# math.ceil(power * diff_wires.a / largest));
self.motors.move_b(int(-math.floor(b)), power)
# math.ceil(power * diff_wires.b / largest));
self.motors.move_c(int(-math.floor(c)), power)
# math.ceil(power * diff_wires.c / largest));
# Wait to finish movement
self.motors.wait_a()
self.motors.wait_b()
self.motors.wait_c()
# Store new position
self.wires = new_wires
self.pos = target
def __init__(self, motors, pos=DeltaCoord(0, 0, 0)):
"""Setup control with initial head position."""
self.motors = motors
self.pos = pos
self.wires = wires.wires_from_coord(pos)
