def controller_draw(window,canvas,joystick,pos_last,t_draw,ball_input):
input = get_axis(joystick)
delta_t = time() - t_draw
delta_pos = input*delta_t*gain
pos = pos_last + delta_pos
t_draw = time()
canvas.coords(ball_input,
pos[0]-encoder_ball_radius,
pos[1]-encoder_ball_radius,
pos[0]+encoder_ball_radius,
pos[1]+encoder_ball_radius)
window.update()
return pos, t_draw
def controller_draw(window,canvas,joystick,pos_last,t_draw):
input = get_axis(joystick)
delta_t = time() - t_draw
delta_pos = input*delta_t*300
pos = pos_last + delta_pos
t_draw = time()
ball = canvas.create_oval(pos[0]-encoder_ball_radius,
pos[1]-encoder_ball_radius,
pos[0]+encoder_ball_radius,
pos[1]+encoder_ball_radius,
fill="white")
window.update()
return pos, t_draw
def controller_operation(joystick, group, hebi_feedback, command, L1, L2, T):
K = np.matrix([[0.05, 0], [0, 0.05]])
axis = get_axis(joystick)
axis_f = input_filter(axis, 10, T)
axis_f = np.squeeze(np.asarray(axis_f))
theta, omega, torque, hebi_limit_stop_flag = get_hebi_feedback(
group, hebi_feedback)
theta1 = theta[0]
theta2 = theta[1]
Jinv = np.matrix([[
-cos(theta1 + theta2) / (L1 * cos(theta2)),
-sin(theta1 + theta2) / (L1 * cos(theta2))
],
[(L2 * cos(theta1 + theta2) + L1 * sin(theta1)) /
(L1 * L2 * cos(theta2)),
(L2 * sin(theta1 + theta2) - L1 * cos(theta1)) /
(L1 * L2 * cos(theta2))]])
omega_d = Jinv @ K @ axis_f
omega_d = np.squeeze(np.asarray(omega_d))
command.velocity = omega_d
group.send_command(command)
K = np.matrix([[0.125, 0], [0, 0.125]])
while True:
theta, omega, torque, hebi_limit_stop_flag = get_hebi_feedback(
group, hebi_feedback)
t = time() - t0
pos = trajectory(t, traj)
animate_ball(animation_window, animation_canvas, pos, ball_traj)
pos_draw = encoder_draw(animation_window,
animation_canvas,
arduino,
ball_input,
offset,
draw=True)
axis = get_axis(joystick)
axis[1] = -axis[1]
theta_e = get_encoder_feedback(arduino, num_encoders=2)
theta = theta + np.array([-1.58170749, np.pi / 2 - 1.48693642
]) # offsetting transform
theta1 = theta[0]
theta2 = theta[1]
# print(axis)
axis_f = input_filter(axis, 10, t)
Jinv = np.matrix([[
-cos(theta1 + theta2) / (L1 * cos(theta2)),
-sin(theta1 + theta2) / (L1 * cos(theta2))
],
[(L2 * cos(theta1 + theta2) + L1 * sin(theta1)) /
(L1 * L2 * cos(theta2)),