complete

Author: RoggeOhta

demo/handle_control/joy_control.py

@@ -5,6 +5,34 @@
 from threading import Thread
 from enum import Enum
 import typing as T
+import platform
+
+if "linux" in platform.platform().lower():
+    import RPi.GPIO as GPIO
+
+    # 初始化
+    GPIO.setmode(GPIO.BCM)
+    # 引脚20/21分别控制电磁阀和泄气阀门
+    GPIO.setup(20, GPIO.OUT)
+    GPIO.setup(21, GPIO.OUT)
+
+
+# 开启吸泵
+def pump_on():
+    # 打开电磁阀
+    GPIO.output(20, 0)
+
+
+# 停止吸泵
+def pump_off():
+    # 关闭电磁阀
+    GPIO.output(20, 1)
+    time.sleep(0.05)
+    # 打开泄气阀门
+    GPIO.output(21, 0)
+    time.sleep(1)
+    GPIO.output(21, 1)
+    time.sleep(0.05)
 
 
 class JoyStickKey(Enum):
@@ -116,7 +144,7 @@ def get_joystick():
 
 def dispatch_key_action(key: T.Union[JoyStickKey, JoyStickContinous], value: float):
     global mc, arm_angle_table, global_states
-    print(f"key : {key} value : {value}")
+    # print(f"key : {key} value : {value}")
 
     not_zero = lambda x: x < -0.1 or x > 0.1
     is_zero = lambda x: -0.1 < x < 0.1
@@ -152,27 +180,71 @@ def dispatch_key_action(key: T.Union[JoyStickKey, JoyStickContinous], value: flo
         global_states["enable"] = True
         time.sleep(3)
 
-    if global_states["enable"] and global_states["initialized"]:
-        # 坐标移动
-        if key in [
-            JoyStickContinous.LeftXAxis,
-            JoyStickContinous.LeftYAxis,
-            JoyStickContinous.RightYAxis,
-            JoyStickKey.ArrowUp,
-            JoyStickKey.ArrowDown,
-            JoyStickKey.ArrowLeft,
-            JoyStickKey.ArrowRight,
-            JoyStickContinous.RightXAxis,
-        ]:
+    if not global_states["enable"] or not global_states["initialized"]:
+        return
+
+    # 坐标移动
+    if key in [
+        JoyStickContinous.LeftXAxis,
+        JoyStickContinous.LeftYAxis,
+        JoyStickContinous.RightYAxis,
+        JoyStickKey.ArrowUp,
+        JoyStickKey.ArrowDown,
+        JoyStickKey.ArrowLeft,
+        JoyStickKey.ArrowRight,
+        JoyStickContinous.RightXAxis,
+        JoyStickKey.ArrowReleased,
+    ]:
+        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["move_key_states"][key] = 0
+        else:
             global_states["move_key_states"][key] = value
 
+        if key == JoyStickKey.ArrowReleased:
+            global_states["move_key_states"][JoyStickKey.ArrowUp] = 0
+            global_states["move_key_states"][JoyStickKey.ArrowDown] = 0
+            global_states["move_key_states"][JoyStickKey.ArrowLeft] = 0
+            global_states["move_key_states"][JoyStickKey.ArrowRight] = 0
+
     # 功能性
     if key == JoyStickContinous.L2 and not_zero(value):
         mc.release_all_servos()
         time.sleep(0.03)
     elif key == JoyStickContinous.R2 and not_zero(value):
         mc.power_on()
         time.sleep(0.03)
+    elif key == JoyStickKey.L1 and not_zero(value):
+        mc.send_angles([0, 0, 0, 0, 0, 0], 50)
+        time.sleep(3)
+
+    # 工具
+    if key == JoyStickKey.RLeftKey:
+        global_states["gripper_val"] = min(100, global_states["gripper_val"] + 5)
+        mc.set_gripper_value(global_states["gripper_val"], 50)
+        time.sleep(0.5)
+    elif key == JoyStickKey.RTopKey:
+        global_states["gripper_val"] = max(0, global_states["gripper_val"] - 5)
+        mc.set_gripper_value(global_states["gripper_val"], 50)
+        time.sleep(0.5)
+    elif key == JoyStickKey.RBottomKey:
+        pump_on()
+        time.sleep(0.05)
+    elif key == JoyStickKey.RRightKey:
+        pump_off()
+        time.sleep(0.05)
 
 
 def continous_move():
@@ -183,7 +255,16 @@ def continous_move():
 
     while True:
         if not global_states["enable"] or not global_states["initialized"]:
-            return
+            time.sleep(0.05)
+            continue
+
+        origin = None
+        if global_states["origin"] is None:
+            time.sleep(0.05)
+            continue
+        else:
+            origin = global_states["origin"]
+
         for key, value in global_states["move_key_states"].items():
             if key in [
                 JoyStickContinous.LeftXAxis,
@@ -194,61 +275,43 @@ def continous_move():
                 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"]
-
+            ] and not_zero(value):
+                x, y, z, rx, ry, rz = origin
                 # Y coord
                 if key == JoyStickContinous.LeftXAxis:
                     mc.send_coord(2, y - value * 10, 10)
-                    global_states["origin"][1] += value
-                    time.sleep(0.05)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][1] -= value
                 # X coord
                 elif key == JoyStickContinous.LeftYAxis:
                     mc.send_coord(1, x - value * 10, 10)
-                    global_states["origin"][0] += value
-                    time.sleep(0.05)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][0] -= value
                 # Z coord
                 elif key == JoyStickContinous.RightYAxis:
                     mc.send_coord(3, z - value * 10, 10)
-                    global_states["origin"][2] += value
-                    time.sleep(0.05)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][2] -= value
                 elif key == JoyStickContinous.RightXAxis:
                     mc.send_coord(6, rz - value * 10, 10)
-                    global_states["origin"][5] += value
-                    time.sleep(0.05)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][5] -= value
                 elif key == JoyStickKey.ArrowUp:
-                    mc.send_coord(4, rx + 10, 10)
-                    global_states["origin"][3] += 10
-                    time.sleep(0.05)
+                    mc.send_coord(4, rx + 1, 10)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][3] += 1
                 elif key == JoyStickKey.ArrowDown:
-                    mc.send_coord(4, rx - 10, 10)
-                    global_states["origin"][3] -= 10
-                    time.sleep(0.05)
+                    mc.send_coord(4, rx - 1, 10)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][3] -= 1
                 elif key == JoyStickKey.ArrowLeft:
-                    mc.send_coord(5, ry - 10, 10)
-                    global_states["origin"][4] -= 10
-                    time.sleep(0.05)
+                    mc.send_coord(5, ry - 1, 10)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][4] -= 1
                 elif key == JoyStickKey.ArrowRight:
-                    mc.send_coord(5, ry + 10, 10)
-                    global_states["origin"][4] += 10
-                    time.sleep(0.05)
+                    mc.send_coord(5, ry + 1, 10)
+                    if global_states["origin"] is not None:
+                        global_states["origin"][4] += 1
 
         time.sleep(0.05)
 
@@ -293,3 +356,4 @@ def retreive_joystick_input(joystick, context):
 if __name__ == "__main__":
     joystick = get_joystick()
     Thread(target=retreive_joystick_input, args=(joystick, context)).start()
+    Thread(target=continous_move).start()