init repo

test_dof_control.py

@@ -0,0 +1,146 @@
+import time
+import keyboard
+
+from demo_auto_grasp import TactileSensorDAQ, RobotDriver, SafetyGuard
+
+# ====== 可按实际机构方向微调的参数 ======
+SERVO_HOME_ANGLE = 90
+SERVO_CW_TO_OPPOSE_ANGLE = 120
+SERVO_HOLD_SEC = 1.0
+FORCE_STOP_THRESHOLD = 0.3
+PRESSURE_DISPLAY_KPA = 80
+ROBOT_COM_PORT = 'COM9'
+
+
+def wait_with_pause(safety, duration_s, step_s=0.02):
+    start = time.time()
+    while (time.time() - start) < duration_s:
+        safety.check_pause()
+        time.sleep(step_s)
+
+
+def run_servo_single_cycle(robot, safety, servo_id):
+    print(f"[Mode {servo_id}] 舵机{servo_id}开始: 手指1顺时针到对握位 -> 回位")
+
+    safety.check_pause()
+    # 仅发送指定舵机命令，其他舵机不动作
+    robot.set_servo(servo_id, SERVO_CW_TO_OPPOSE_ANGLE)
+
+    wait_with_pause(safety, SERVO_HOLD_SEC)
+
+    safety.check_pause()
+    robot.set_servo(servo_id, SERVO_HOME_ANGLE)
+    wait_with_pause(safety, 0.5)
+
+    # 明确停止直线电机，确保其不动作
+    robot.motor_stop()
+    print(f"[Mode {servo_id}] 完成。")
+
+
+def run_linear_mode(robot, safety):
+    print("[Mode 3] 直线电机控制模式 (无传感器)")
+    print("按键: w=张开, s=闭合, q=退出模式")
+    print("规则: 无传感器，按住s持续闭合")
+    print("全局: 空格急停/恢复")
+
+    motion_state = 'idle'  # idle/open/close/force_stop
+    prev_q = False
+    linear_start_time = None
+
+    def print_runtime(final=False):
+        if linear_start_time is None:
+            return
+        elapsed = time.time() - linear_start_time
+        if final:
+            print(f"[Mode 3] s执行总时长: {elapsed:.2f}s")
+
+    while True:
+        safety.check_pause()
+
+        w_now = keyboard.is_pressed('w')
+        s_now = keyboard.is_pressed('s')
+        q_now = keyboard.is_pressed('q')
+
+        q_edge = q_now and not prev_q
+
+        if q_edge:
+            robot.motor_stop()
+            print_runtime(final=True)
+            linear_start_time = None
+            print("[Mode 3] 退出直线电机控制模式")
+            break
+
+        # 仅允许单键控制: w 和 s 同时按下时停止，防止方向冲突
+        if w_now and not s_now:
+            if motion_state != 'open':
+                robot.motor_open()
+                motion_state = 'open'
+                print("[Mode 3] 张开中 (按住 w)")
+        elif s_now and not w_now:
+            if linear_start_time is None:
+                linear_start_time = time.time()
+            if motion_state != 'close':
+                robot.motor_close()
+                motion_state = 'close'
+                print("[Mode 3] 闭合中 (按住 s)")
+        else:
+            if motion_state in ('open', 'close', 'force_stop'):
+                robot.motor_stop()
+                print_runtime(final=True)
+                motion_state = 'idle'
+                print("[Mode 3] 松开按键, 电机停止")
+                linear_start_time = None
+
+        prev_q = q_now
+
+        time.sleep(0.02)
+
+
+def main():
+    # sensor = TactileSensorDAQ()
+    robot = RobotDriver(port=ROBOT_COM_PORT)
+    safety = SafetyGuard(robot)
+
+    try:
+        # sensor.start()
+        # print("等待传感器稳定...")
+        # time.sleep(2)
+        # sensor.tare()
+
+        while True:
+            print("\n========================")
+            print("自由度控制选择:")
+            print("  1: 舵机1控制 (手指1顺时针到对握后回位)")
+            print("  2: 舵机2控制 (手指1顺时针到对握后回位)")
+            print(f"  3: 直线电机控制 (w张开/s闭合, 压强显示固定为 {PRESSURE_DISPLAY_KPA}KPa)")
+            print("  q: 退出程序")
+            print("全局: 空格急停/恢复")
+
+            choice = input("请输入 1/2/3/q: ").strip().lower()
+
+            if choice == '1':
+                run_servo_single_cycle(robot, safety, servo_id=1)
+            elif choice == '2':
+                run_servo_single_cycle(robot, safety, servo_id=2)
+            elif choice == '3':
+                run_linear_mode(robot, safety)
+            elif choice == 'q':
+                break
+            else:
+                print("输入无效，请重新输入。")
+
+    except KeyboardInterrupt:
+        pass
+    finally:
+        robot.motor_stop()
+        robot.close()
+        # sensor.stop()
+        try:
+            keyboard.unhook_all()
+        except Exception:
+            pass
+        print("System All Shutdown.")
+
+
+if __name__ == '__main__':
+    main()