def __init__(self):
#Local variables
self._running = True
self.trajectory_queue = queue.Queue()
self.master_board = masterboard.MasterBoard()
self.plotter = pltr.Plotter("balance")
for i in range(config.N_SLAVES_CONTROLLED * 2):
self.plotter.create_plot(f"Joint {i}, Position Set Point",
"Position [rad]")
current_position = self.master_board.get_state()[0]
home = np.array([[0, 100], [0, 100], [0, 100], [0, 100]])
new_point = kinematics.simple_walk_controller(
home,
current_position,
zero_config=kinematics.idle_zero_config,
joint_polarities=kinematics.idle_joint_polarities,
elbows=kinematics.idle_elbow)
self.trajectory_queue.put(trj.Trajectory(
(current_position, new_point)))
self.initial_position = new_point
def __init__(self, trajectory_queue, message_queue, shared_position,
masterboard_started_event):
#Local variables
self._running = True
self._terminating = False
self.shared_position = shared_position
self.trajectory_queue = trajectory_queue
self.message_queue = message_queue
self.masterboard_started_event = masterboard_started_event
self.debug_plot = True
self.balance_mode = False
#self.plot = plotter.Plotter(f"Via points test | kp = {config.kp}, kd = {config.kd}, current saturation = {config.current_saturation}")
#for i in range(config.N_SLAVES_CONTROLLED * 2):
# self.plot.create_plot(f"Joint {i}, Current Reference", "Current [A]")
# self.plot.create_plot(f"Joint {i}, Position Reference", "Position [rad]")
# self.plot.create_plot(f"Joint {i}, Position Reading", "Position [rad]")
# self.plot.create_plot(f"Joint {i}, Position Error", "position [rad]")
self.master_board = masterboard.MasterBoard()
if self.debug_plot:
self.plotter = pltr.Plotter(
f"Via points test | kp = {config.kp}, kd = {config.kd}, current saturation = {config.current_saturation}"
)
for i in range(config.N_SLAVES_CONTROLLED * 2):
self.plotter.create_plot(f"Joint {i}, Position Reference",
"Position [rad]")
self.plotter.create_plot(f"Joint {i}, Position Reading",
"Position [rad]")
self.plotter.create_plot(f"Joint {i}, Velocity Reference",
"Velocity [rad/s]")
self.plotter.create_plot(f"Joint {i}, Velocity Reading",
"Velocity [rad/s]")
def __init__(self):
#Local variables
self._running = True
self.master_board = masterboard.MasterBoard()
self.plotter = pltr.Plotter("balance")
self.plotter.create_plot("Attitude Reading", "Rotation [rad]")
self.prev_att_err = 0
self.prev_t = 0
self.kp = 100 #200
self.kd = 0.25
self.ki = 4000 #3000
self.current_coordinates = np.array([[0, 100], [0, 100], [0, 100],
[0, 100]])
self.acc_error = np.zeros(2)
def __init__(self, trajectory_queue, message_queue, shared_position):
#Local variables
self._running = True
self._terminating = False
self.shared_position = shared_position
self.trajectory_queue = trajectory_queue
self.message_queue = message_queue
self.plot = plotter.Plotter(
f"Kinematics test | kp = {config.kp}, kd = {config.kd}, current saturation = {config.current_saturation}"
)
for i in range(config.N_SLAVES_CONTROLLED * 2):
# self.plot.create_plot(f"Joint {i}, Current Reference", "Current [A]")
self.plot.create_plot(f"Joint {i}, Position Reference",
"Position [rad]")
self.plot.create_plot(f"Joint {i}, Position Reading",
"Position [rad]")
# self.plot.create_plot(f"Joint {i}, Position Error", "position [rad]")
self.master_board = masterboard.MasterBoard(self.plot)
self.reset()
def wait_for_confirmation(self):
input("Press enter to continue")
print()
def reset(self):
self.input_flag = True
self.print_startup_message = True
input_task = InputTask()
input_thread = threading.Thread(target=input_task.run, daemon=True)
master_board = masterboard.MasterBoard()
input_thread.start()
dt = config.dt
timeout = False
running = True
cycle_start = time.perf_counter()
while (not timeout and running):
if input_task.zero_calibration_flag:
input_task.zero_offset = master_board.get_actual_state()[0]
input_task.zero_calibration_flag = False