class HardwareAbstractionLayer:
ARM_ONE_LENGTH = 1
ARM_TWO_LENGTH = 1
Z_MOVEMENT_FACTOR = 5
ANGLE_DISPLACEMENT_MAGNITUDE = 0.5
def __init__(self):
z_pin = 18
angle_one_pin = 13
angle_two_pin = 12
self.z_motor = Motor(z_pin)
self.shoulder_motor = Motor(angle_one_pin, 20 * pi / 180)
self.elbow_motor = Motor(angle_two_pin, 70 * pi / 180)
self.shoulder_motor.change_angle(-1)
self.elbow_motor.change_angle(-1)
def translate_instrument(self, delta_x, delta_y):
desired = np.array([delta_x, delta_y])
theta_one = self.shoulder_motor.get_angle()
theta_two = self.elbow_motor.get_angle()
jacobian = self.compute_jacobian(theta_one, theta_two)
control_inputs = np.linalg.lstsq(jacobian, desired)[0]
if control_inputs[0] != 0 and control_inputs[1] != 0:
control_inputs[1] = -control_inputs[1]
# control_inputs[0] = -control_inputs[0] # TODO: check this
control_inputs = self.remap_controls(control_inputs)
return self.shoulder_motor.change_angle(
control_inputs[0]) and self.elbow_motor.change_angle(
control_inputs[1])
else:
return False
def remap_controls(self, control_inputs):
return control_inputs * (self.ANGLE_DISPLACEMENT_MAGNITUDE /
np.linalg.norm(control_inputs))
def move_instrument_up(self):
return self.z_motor.change_angle(self.Z_MOVEMENT_FACTOR)
def move_instrument_down(self):
return self.z_motor.change_angle(-self.Z_MOVEMENT_FACTOR)
def compute_jacobian(self, theta_one, theta_two):
l1 = self.ARM_ONE_LENGTH
l2 = self.ARM_TWO_LENGTH
t1 = theta_one
t2 = theta_two
dx_dt1 = l1 * sin(t1) - l2 * sin(t1 + t2)
dx_dt2 = -l2 * sin(t1 + t2)
dy_dt1 = l1 * cos(t1) + l2 * cos(t1 + t2)
dy_dt2 = l2 * cos(t1 + t2)
return np.array([[dx_dt1, dx_dt2], [dy_dt1, dy_dt2]])
def test_move(self, x, y, r=20):
for i in range(r):
self.translate_instrument(x, y)
