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,