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 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 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 disengage(): for motor_id in _motors_id : b.turn_off(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,
motor_pose["rest"][str(motor_id)], 0.5)
input("Enter to go back to rest.")
for motor_id in all_motor_id:
b.turn_off(motor_id)
elif sys.argv[1] == "check":
b.init_bus()
all_motor_id = [int(x) for x in sys.argv[2:]]
motor_pose = load_motor_pose()
print("raw motor pose info : ", motor_pose)
motor_pose = check_start(all_motor_id, motor_pose)
print("refined motor pose info : ", motor_pose)
input("Enter to go to zero pose.")
for motor_id in all_motor_id:
b.position_control_at_speed(motor_id,
motor_pose["zero"][str(motor_id)], 0.5)