def encoder_draw(window, canvas, arduino, L1, L2):
theta = get_encoder_feedback(arduino, num_encoders=2)
pos = 1000 * np.array([
L1 * np.sin(theta[0]) + L2 * np.cos(theta[1]),
L1 * np.cos(theta[0]) - L2 * np.sin(theta[1])
])
pos = pos + np.array([400, 400])
print(pos)
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()
def encoder_draw(window,canvas,arduino,ball_input,offset,draw):
theta = get_encoder_feedback(arduino, num_encoders=2)
pos = 5000*np.array([-L1*np.sin(theta[0]) - L2*np.cos(theta[0]+theta[1]), L1*np.cos(theta[0])-L2*np.sin(theta[0]+theta[1])])
pos[1] = animation_window_height - pos[1]
pos += offset
if draw == True:
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()
print(pos)
return pos
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)),
(L2 * sin(theta1 + theta2) - L1 * cos(theta1)) /
(L1 * L2 * cos(theta2))]])
arduino = initialize_encoders()
t0 = time()
t_1 = t0
sleep(0.001)
i = 1
output = []
while True:
t = time() - t0
T = time() - t_1
t_1 = time()
h_theta, h_omega, hf_torque, hebi_limit_stop_flag = get_hebi_feedback(
group, hebi_feedback)
theta = get_encoder_feedback(arduino)
omega = velocity(theta, T)
omega = velocity_filter(omega, 5, T)
t, theta_d, omega_d = trajectory(t, "trajectories/star0")
#---------- Output feedback
# effort = PD_controller(theta,theta_d,omega,omega_d)
effort = PD_controller(h_theta, theta_d, h_omega, omega_d)
if i == 1:
effort = np.array([0, 0])
print("First if")
command.effort = effort
send_hebi_effort_command(group, command)
output += [[