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)