def test_set_command_position():
with MotionManager(ids, dt=.005, options=dopts) as mm:
# checking that the set_all/get_all position commands work
position_list = [[0], [-0.6234], [1.8903], [0.6234], [-1.8903],
[0.6234], [-1.8903], [-0.6234], [1.8903], [2.1056]]
for pl in position_list:
mm.set_goal_position(ids, to_player_angle_offset(pl, offset))
# mm.wait(0.8)
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
counter_time = 0
k = 0
# while np.max(abs(np.array(qcurr) - np.array(pl))) >= 0.005:
while k < 500:
start = time.time()
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
end_time = time.time() - start
counter_time += end_time
k = k + 1
# print(qcurr)
print(qcurr)
print(counter_time)
print(counter_time / 500)
def set_single_cmd_position(pos_list, motor_id):
# eg. motor_id = [1], pos_list = [[0],[pi/2]]
with MotionManager(motor_id, dt=.005, options=dopts) as mm:
# move one single motor with id
for pl in pos_list:
mm.set_goal_position(motor_id, to_player_angle_offset(pl, offset))
#mm.wait(2.0)
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
# Keep reading encoder until motor reach goal position
while np.max(abs(np.array(qcurr) - np.array(pl))) >= 0.005:
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
print(qcurr)
def set_all_cmd_position(pos_list):
with MotionManager(ids, dt=.005, options=dopts) as mm:
# move all motors together
# pos_list = [[pi,pi], [0,0], [pi/2,pi/2]]
for pl in pos_list:
mm.set_goal_position(ids, to_player_angle_offset(pl, offset))
# mm.wait(0.3)
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
# Keep reading encoder until motor reach goal position
k = 0
while np.max(abs(np.array(qcurr[1:]) -
np.array(pl[1:]))) >= 0.05 and k < 50:
qcurr = from_player_angle_offset(mm.get_all_present_position(),
offset)
k = k + 1
print(qcurr)