diff --git a/controller.py b/controller.py new file mode 100644 index 0000000..a9beec2 --- /dev/null +++ b/controller.py @@ -0,0 +1,171 @@ +import tkinter as tk, threading, robomasterpy as rm +from tkinter import ttk + +class RobomasterUI: + def __init__(self, master, cmd): + self.master = master + master.title("Robomaster Control") + self.cmd = cmd + self.output_text = tk.Text(master, height=10, width=50) + self.output_text.grid(row=6, column=0, columnspan=4, padx=10, pady=10) + + arm_frame = ttk.LabelFrame(master, text="Arm Control") + arm_frame.grid(row=0, column=0, padx=10, pady=10, sticky="nw") + + tk.Label(arm_frame, text="X:").grid(row=0, column=0, padx=5, pady=5, sticky="e") + self.x_entry = tk.Entry(arm_frame, width=10) + self.x_entry.grid(row=0, column=1, padx=5, pady=5) + + tk.Label(arm_frame, text="Y:").grid(row=1, column=0, padx=5, pady=5, sticky="e") + self.y_entry = tk.Entry(arm_frame, width=10) + self.y_entry.grid(row=1, column=1, padx=5, pady=5) + + tk.Button(arm_frame, text="Move", command=self.move_arm).grid(row=0, column=2, padx=5, pady=5) + tk.Button(arm_frame, text="Move To", command=self.moveto_arm).grid(row=1, column=2, padx=5, pady=5) + tk.Button(arm_frame, text="Recenter", command=self.recenter_arm).grid(row=2, column=0, columnspan=3, pady=5, sticky="we") + tk.Button(arm_frame, text="Stop", command=self.stop_arm).grid(row=3, column=0, columnspan=3, pady=5, sticky="we") + tk.Button(arm_frame, text="Get Position", command=self.get_arm_pos).grid(row=4, column=0, columnspan=3, pady=5, sticky="we") + + gripper_frame = ttk.LabelFrame(master, text="Gripper Control") + gripper_frame.grid(row=0, column=1, padx=10, pady=10, sticky="nw") + + tk.Label(gripper_frame, text="Force (1-4, optional):").grid(row=0, column=0, padx=5, pady=5, sticky="e") + self.force_entry = tk.Entry(gripper_frame, width=5) + self.force_entry.grid(row=0, column=1, padx=5, pady=5) + + tk.Button(gripper_frame, text="Open Gripper", command=self.open_gripper).grid(row=1, column=0, columnspan=2, pady=5, sticky="we") + tk.Button(gripper_frame, text="Close Gripper", command=self.close_gripper).grid(row=2, column=0, columnspan=2, pady=5, sticky="we") + tk.Button(gripper_frame, text="Gripper Status", command=self.get_gripper_status).grid(row=3, column=0, columnspan=2, pady=5, sticky="we") + + chassis_frame = ttk.LabelFrame(master, text="Chassis Control") + chassis_frame.grid(row=1, column=0, columnspan=2, padx=10, pady=10, sticky="nw") + + tk.Label(chassis_frame, text="X Speed (m/s):").grid(row=0, column=0, padx=5, pady=5, sticky="e") + self.chassis_x_speed_entry = tk.Entry(chassis_frame, width=5) + self.chassis_x_speed_entry.insert(0, "0.1") + self.chassis_x_speed_entry.grid(row=0, column=1, padx=5, pady=5) + + tk.Label(chassis_frame, text="Y Speed (m/s):").grid(row=0, column=2, padx=5, pady=5, sticky="e") + self.chassis_y_speed_entry = tk.Entry(chassis_frame, width=5) + self.chassis_y_speed_entry.insert(0, "0.1") + self.chassis_y_speed_entry.grid(row=0, column=3, padx=5, pady=5) + + tk.Label(chassis_frame, text="Z Speed (rad/s):").grid(row=0, column=4, padx=5, pady=5, sticky="e") + self.chassis_z_speed_entry = tk.Entry(chassis_frame, width=5) + self.chassis_z_speed_entry.insert(0, "0.5") + self.chassis_z_speed_entry.grid(row=0, column=5, padx=5, pady=5) + + tk.Button(chassis_frame, text="Move Chassis", command=self.move_chassis_button).grid(row=1, column=0, columnspan=6, pady=5, sticky="we") + tk.Button(chassis_frame, text="Stop Chassis", command=self.stop_chassis).grid(row=2, column=0, columnspan=6, pady=5, sticky="we") + + master.bind("", lambda event: self.chassis_move_arrow(y=1)) + master.bind("", lambda event: self.chassis_move_arrow(y=-1)) + master.bind("", lambda event: self.chassis_move_arrow(x=-1)) + master.bind("", lambda event: self.chassis_move_arrow(x=1)) + master.bind("", lambda event: self.chassis_move_arrow(z=1)) + master.bind("", lambda event: self.chassis_move_arrow(z=-1)) + master.bind("", lambda event: self.chassis_move_arrow(y=0) if event.keysym == 'Up' else None) + master.bind("", lambda event: self.chassis_move_arrow(y=0) if event.keysym == 'Down' else None) + master.bind("", lambda event: self.chassis_move_arrow(x=0) if event.keysym == 'Left' else None) + master.bind("", lambda event: self.chassis_move_arrow(x=0) if event.keysym == 'Right' else None) + master.bind("", lambda event: self.chassis_move_arrow(z=0) if event.keysym == 'q' else None) + master.bind("", lambda event: self.chassis_move_arrow(z=0) if event.keysym == 'e' else None) + master.bind("", lambda event: self.stop_chassis()) + + def _run_command(self, func, *args, **kwargs): + threading.Thread(target=lambda: self._command_executor(func, *args, **kwargs)).start() + + def _command_executor(self, func, *args, **kwargs): + try: + result = func(*args, **kwargs) + self.master.after(0, self._display_output, str(result)) + except Exception as e: + self.master.after(0, self._display_output, f"Error: {e}") + + def _display_output(self, output_str): + self.output_text.insert(tk.END, output_str + "\n") + self.output_text.see(tk.END) + + def move_arm(self): + x_str, y_str = self.x_entry.get(), self.y_entry.get() + try: x, y = float(x_str), float(y_str) + except ValueError: return self._display_output("Error: Invalid X or Y") + self._run_command(self.cmd.arm_move, x=x, y=y) + + def moveto_arm(self): + x_str, y_str = self.x_entry.get(), self.y_entry.get() + try: x, y = float(x_str), float(y_str) + except ValueError: return self._display_output("Error: Invalid X or Y") + self._run_command(self.cmd.arm_moveto, x=x, y=y) + + def recenter_arm(self): + self._run_command(self.cmd.arm_recenter) + + def stop_arm(self): + self._run_command(self.cmd.arm_stop) + + def get_arm_pos(self): + self._run_command(self.cmd.arm_pos) + + def open_gripper(self): + force_str = self.force_entry.get() + force = None + if force_str: + try: + force = int(force_str) + if force not in range(1, 5): raise ValueError + except ValueError: return self._display_output("Error: Force must be 1-4") + self._run_command(self.cmd.gripper_open, force=force) + + def close_gripper(self): + force_str = self.force_entry.get() + force = None + if force_str: + try: + force = int(force_str) + if force not in range(1, 5): raise ValueError + except ValueError: return self._display_output("Error: Force must be 1-4") + self._run_command(self.cmd.gripper_close, force=force) + + def get_gripper_status(self): + self._run_command(self.cmd.gripper_status) + + def move_chassis_button(self): + try: + x = float(self.chassis_x_speed_entry.get()) + y = float(self.chassis_y_speed_entry.get()) + z = float(self.chassis_z_speed_entry.get()) + except ValueError: + return self._display_output("Error: Invalid speed value") + self._run_command(self.cmd.chassis_move, x=x, y=y, z=z) + + def stop_chassis(self): + self._run_command(self.cmd.chassis_move, x=0, y=0, z=0) + + def chassis_move_arrow(self, x=None, y=None, z=None): + try: + xs = float(self.chassis_x_speed_entry.get()) if x is not None else 0 + ys = float(self.chassis_y_speed_entry.get()) if y is not None else 0 + zs = float(self.chassis_z_speed_entry.get()) if z is not None else 0 + current_x = xs * (x or 0) + current_y = ys * (y or 0) + current_z = zs * (z or 0) + self._run_command(self.cmd.chassis_move, x=current_x, y=current_y, z=current_z, speed_xy=None, speed_z=None) + except ValueError: + self._display_output("Error: Invalid speed value") + + +def main(): + ip_address = "192.168.2.1" + try: + cmd = rm.Commander(ip=ip_address) + print(f"Connected to Robomaster. Version: {cmd.version()}") + root = tk.Tk() + RobomasterUI(root, cmd) + root.mainloop() + except Exception as e: + print(f"Error connecting to Robomaster: {e}") + print("Make sure the Robomaster is powered on and connected.") + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..e23ab2d --- /dev/null +++ b/requirements.txt @@ -0,0 +1,2 @@ +robomasterpy +tkinter \ No newline at end of file