cp

Author: RoggeOhta

demo/handle_control/joy_control.py

@@ -6,21 +6,6 @@
 from enum import Enum
 import typing as T
 
-mc = MyCobot("/dev/ttyAMA0", "1000000")
-mc.set_fresh_mode(1)
-
-context = {"running": True}
-arm_speed = 50
-sampling_rate = 10
-arm_angle_table = {"init": [0, 0, -90, 0, 0, 0]}
-global_states = {
-    "enable": True,
-    "initialized": True,
-    "origin": None,
-    "gripper_val": 0,
-    "pump": False,
-}
-
 
 class JoyStickKey(Enum):
     StartKey = 7
@@ -78,6 +63,31 @@ class JoyStickContinous(Enum):
     5: JoyStickContinous.R2,
 }
 
+mc = MyCobot("/dev/ttyAMA0", "1000000")
+mc.set_fresh_mode(1)
+
+context = {"running": True}
+arm_speed = 50
+sampling_rate = 10
+arm_angle_table = {"init": [0, 0, -90, 0, 0, 0]}
+global_states = {
+    "enable": True,
+    "initialized": True,
+    "origin": None,
+    "gripper_val": 0,
+    "pump": False,
+    "move_key_states": {
+        JoyStickContinous.LeftXAxis: 0,
+        JoyStickContinous.LeftYAxis: 0,
+        JoyStickContinous.RightYAxis: 0,
+        JoyStickKey.ArrowUp: 0,
+        JoyStickKey.ArrowDown: 0,
+        JoyStickKey.ArrowLeft: 0,
+        JoyStickKey.ArrowRight: 0,
+        JoyStickContinous.RightXAxis: 0,
+    },
+}
+
 
 def get_init_key_hold_timestamp():
     return {
@@ -154,71 +164,95 @@ def dispatch_key_action(key: T.Union[JoyStickKey, JoyStickContinous], value: flo
             JoyStickKey.ArrowRight,
             JoyStickContinous.RightXAxis,
         ]:
-            if global_states["origin"] is None:
-                coords = []
-                for _ in range(5):
-                    coords = mc.get_coords()
-                    if len(coords) != 0:
-                        break
-
-                if len(coords) != 0:
-                    global_states["origin"] = coords
-                else:
-                    print("Can't get coords.")
-                    return
-
-            if is_zero(value):
-                global_states["origin"] = None
-                return
-
-            x, y, z, rx, ry, rz = global_states["origin"]
-
-            # Y coord
-            if key == JoyStickContinous.LeftXAxis:
-                mc.send_coord(2, y - value * 10, 10)
-                global_states["origin"][1] += value
-                time.sleep(0.05)
-            # X coord
-            elif key == JoyStickContinous.LeftYAxis:
-                mc.send_coord(1, x - value * 10, 10)
-                global_states["origin"][0] += value
-                time.sleep(0.05)
-            # Z coord
-            elif key == JoyStickContinous.RightYAxis:
-                mc.send_coord(3, z - value * 10, 10)
-                global_states["origin"][2] += value
-                time.sleep(0.05)
-            elif key == JoyStickContinous.RightXAxis:
-                mc.send_coord(6, rz - value * 10, 10)
-                global_states["origin"][5] += value
-                time.sleep(0.05)
-            elif key == JoyStickKey.ArrowUp:
-                mc.send_coord(4, rx + 10, 10)
-                global_states["origin"][3] += 10
-                time.sleep(0.05)
-            elif key == JoyStickKey.ArrowDown:
-                mc.send_coord(4, rx - 10, 10)
-                global_states["origin"][3] -= 10
-                time.sleep(0.05)
-            elif key == JoyStickKey.ArrowLeft:
-                mc.send_coord(5, ry - 10, 10)
-                global_states["origin"][4] -= 10
-                time.sleep(0.05)
-            elif key == JoyStickKey.ArrowRight:
-                mc.send_coord(5, ry + 10, 10)
-                global_states["origin"][4] += 10
-                time.sleep(0.05)
+            global_states["move_key_states"][key] = value
 
     # 功能性
     if key == JoyStickContinous.L2 and not_zero(value):
-        print(123)
         mc.release_all_servos()
         time.sleep(0.03)
     elif key == JoyStickContinous.R2 and not_zero(value):
         mc.power_on()
         time.sleep(0.03)
 
 
+def continous_move():
+    global global_states
+
+    not_zero = lambda x: x < -0.1 or x > 0.1
+    is_zero = lambda x: -0.1 < x < 0.1
+
+    while True:
+        if not global_states["enable"] or not global_states["initialized"]:
+            return
+        for key, value in global_states["move_key_states"].items():
+            if key in [
+                JoyStickContinous.LeftXAxis,
+                JoyStickContinous.LeftYAxis,
+                JoyStickContinous.RightYAxis,
+                JoyStickKey.ArrowUp,
+                JoyStickKey.ArrowDown,
+                JoyStickKey.ArrowLeft,
+                JoyStickKey.ArrowRight,
+                JoyStickContinous.RightXAxis,
+            ]:
+                if global_states["origin"] is None:
+                    coords = []
+                    for _ in range(5):
+                        coords = mc.get_coords()
+                        if len(coords) != 0:
+                            break
+
+                    if len(coords) != 0:
+                        global_states["origin"] = coords
+                    else:
+                        print("Can't get coords.")
+                        return
+
+                if is_zero(value):
+                    global_states["origin"] = None
+                    return
+
+                x, y, z, rx, ry, rz = global_states["origin"]
+
+                # Y coord
+                if key == JoyStickContinous.LeftXAxis:
+                    mc.send_coord(2, y - value * 10, 10)
+                    global_states["origin"][1] += value
+                    time.sleep(0.05)
+                # X coord
+                elif key == JoyStickContinous.LeftYAxis:
+                    mc.send_coord(1, x - value * 10, 10)
+                    global_states["origin"][0] += value
+                    time.sleep(0.05)
+                # Z coord
+                elif key == JoyStickContinous.RightYAxis:
+                    mc.send_coord(3, z - value * 10, 10)
+                    global_states["origin"][2] += value
+                    time.sleep(0.05)
+                elif key == JoyStickContinous.RightXAxis:
+                    mc.send_coord(6, rz - value * 10, 10)
+                    global_states["origin"][5] += value
+                    time.sleep(0.05)
+                elif key == JoyStickKey.ArrowUp:
+                    mc.send_coord(4, rx + 10, 10)
+                    global_states["origin"][3] += 10
+                    time.sleep(0.05)
+                elif key == JoyStickKey.ArrowDown:
+                    mc.send_coord(4, rx - 10, 10)
+                    global_states["origin"][3] -= 10
+                    time.sleep(0.05)
+                elif key == JoyStickKey.ArrowLeft:
+                    mc.send_coord(5, ry - 10, 10)
+                    global_states["origin"][4] -= 10
+                    time.sleep(0.05)
+                elif key == JoyStickKey.ArrowRight:
+                    mc.send_coord(5, ry + 10, 10)
+                    global_states["origin"][4] += 10
+                    time.sleep(0.05)
+
+        time.sleep(0.05)
+
+
 def dispatch_continous_key_action(key: JoyStickContinous, value: float):
     print(f"{key}:{value}")