147 lines
4.4 KiB
Python
147 lines
4.4 KiB
Python
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()
|