Add controller/main.py
This commit is contained in:
parent
428d790864
commit
3e5f9596bd
171
controller/main.py
Normal file
171
controller/main.py
Normal file
@ -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("<Up>", lambda event: self.chassis_move_arrow(y=1))
|
||||
master.bind("<Down>", lambda event: self.chassis_move_arrow(y=-1))
|
||||
master.bind("<Left>", lambda event: self.chassis_move_arrow(x=-1))
|
||||
master.bind("<Right>", lambda event: self.chassis_move_arrow(x=1))
|
||||
master.bind("<KeyPress-q>", lambda event: self.chassis_move_arrow(z=1))
|
||||
master.bind("<KeyPress-e>", lambda event: self.chassis_move_arrow(z=-1))
|
||||
master.bind("<KeyRelease-Up>", lambda event: self.chassis_move_arrow(y=0) if event.keysym == 'Up' else None)
|
||||
master.bind("<KeyRelease-Down>", lambda event: self.chassis_move_arrow(y=0) if event.keysym == 'Down' else None)
|
||||
master.bind("<KeyRelease-Left>", lambda event: self.chassis_move_arrow(x=0) if event.keysym == 'Left' else None)
|
||||
master.bind("<KeyRelease-Right>", lambda event: self.chassis_move_arrow(x=0) if event.keysym == 'Right' else None)
|
||||
master.bind("<KeyRelease-q>", lambda event: self.chassis_move_arrow(z=0) if event.keysym == 'q' else None)
|
||||
master.bind("<KeyRelease-e>", lambda event: self.chassis_move_arrow(z=0) if event.keysym == 'e' else None)
|
||||
master.bind("<space>", 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()
|
||||
Loading…
Reference in New Issue
Block a user