def test_kd_gain():
MOTOR_ID = 3
b.init_bus()
b.set_pid(MOTOR_ID, 6, 0, 50, 0, 50, 50)
p0 = b.actuator_pos(MOTOR_ID)
p1 = p0 + np.pi / 10
T = 2
# startup
b.position_control(MOTOR_ID, p0)
print("coucou")
time.sleep(2)
# actual test
to_save = [[time.time(), b.actuator_pos(MOTOR_ID)]]
print("starting test")
start = time.time()
b.position_control(MOTOR_ID, p1)
while time.time() < start + T:
to_save.append([time.time(), b.actuator_pos(MOTOR_ID)])
to_save = np.asarray(to_save)
np.save("data.npy", to_save)
# end
b.position_control(MOTOR_ID, p0)
print("coucou")
time.sleep(2)
b.turn_off(MOTOR_ID)
def goto_rest (): dt = 2 for motor_id, targ_pos in zip (_motors_id, _rest_pos) : cur_pos = b.actuator_pos(motor_id) #speed = max((10, abs(cur_pos-targ_pos)/dt)) speed = abs(cur_pos-targ_pos)/dt b.position_control_at_speed (motor_id, targ_pos, speed)
def test_kp_gain():
MOTOR_ID = 3
b.init_bus()
b.set_pid(MOTOR_ID, 6, 0, 50, 0, 50, 50)
p0 = b.actuator_pos(MOTOR_ID)
T = 10
# startup
b.position_control(MOTOR_ID, p0)
print("starting countdown...")
time.sleep(T)
print(b.actuator_pos(MOTOR_ID) - p0)
b.turn_off(MOTOR_ID)
"""
def lock_motor():
MOTOR_ID = 1
b.init_bus()
b.set_pid(MOTOR_ID, 6, 0, 50, 0, 50, 50)
p0 = b.actuator_pos(MOTOR_ID)
b.position_control(MOTOR_ID, p0)
print("enter to end.")
input()
b.turn_off(MOTOR_ID)
def get_pos (fetch=True) :
global last_fetched_actuator_pos
if _origin is None :
raise ErrorName("origin non defined")
else :
if not fetch and last_fetched_actuator_pos is None:
raise ErrorName("You have to fetch the position of the motors at least once")
elif fetch:
last_fetched_actuator_pos = []
for motor_id, ori, red in zip (_motors_id, _origin, _reduction):
last_fetched_actuator_pos.append((b.actuator_pos(motor_id) - ori)/red)
return last_fetched_actuator_pos
def check_start(all_motor_id, motor_pose):
for motor_id in all_motor_id:
start_position = b.actuator_pos(motor_id)
delta = 0
while motor_pose["rest"][str(motor_id)] + delta < start_position - .5:
delta += 1
while motor_pose["rest"][str(motor_id)] + delta > start_position + .5:
delta -= 1
motor_pose["rest"][str(motor_id)] += delta
motor_pose["zero"][str(motor_id)] += delta
if abs(start_position -
motor_pose["rest"][str(motor_id)]) > 3.14 / 6 / 3:
print("start position :", start_position)
print("rest position :", motor_pose["rest"][str(motor_id)])
print(motor_id)
raise NameError(
"Motor {} too far from its default position.".format(motor_id))
return motor_pose
)
elif sys.argv[1] == "set":
b.init_bus()
all_motor_id = [int(x) for x in sys.argv[2:]]
motor_pose = load_motor_pose()
# choosing the rest pose
for motor_id in all_motor_id:
print(motor_id)
b.set_zero_torque(motor_id)
input("Put motors {} at their rest position.".format(all_motor_id))
for motor_id in all_motor_id:
motor_pose["rest"][str(motor_id)] = b.actuator_pos(motor_id)
input("Put motors {} at their zero position.".format(all_motor_id))
for motor_id in all_motor_id:
motor_pose["zero"][str(motor_id)] = b.actuator_pos(motor_id)
save_motor_pose(motor_pose)
for motor_id in all_motor_id:
b.position_control_at_speed(motor_id,
motor_pose["zero"][str(motor_id)], 0.5)
input("Get your hands away from the robot.")
for motor_id in all_motor_id:
b.position_control_at_speed(motor_id,