init
This commit is contained in:
310
keyboard_remote_new_pcb.py
Normal file
310
keyboard_remote_new_pcb.py
Normal file
@ -0,0 +1,310 @@
|
||||
from robot_driver import RobotDriver
|
||||
import argparse
|
||||
import sys
|
||||
import time
|
||||
|
||||
import keyboard
|
||||
|
||||
from demo_auto_grasp import SafetyGuard, TactileSensorDAQ
|
||||
|
||||
|
||||
def compute_pressure_kpa(force_value, contact_area_mm2):
|
||||
area = max(float(contact_area_mm2), 1e-6)
|
||||
return float(force_value) * 1000.0 / area
|
||||
|
||||
|
||||
class CustomRobotDriver(RobotDriver):
|
||||
def __init__(self, port="COM6", baud=115200, ack=True):
|
||||
super().__init__(port, baud, ack) # 调用父类的初始化
|
||||
self.servo_positions = {} # 保留自己特有的属性
|
||||
|
||||
def ping(self):
|
||||
return self._send("PING")
|
||||
|
||||
def ramp_servo(self, servo_id, target_angle, speed_deg_per_sec=30):
|
||||
target_angle = max(0, min(180, int(target_angle)))
|
||||
servo_id = int(servo_id)
|
||||
# 如果没有记录上一次位置，使用中立位置 90 作为默认起点，避免默认等于目标导致不动作
|
||||
cur = int(self.servo_positions.get(servo_id, 90))
|
||||
if cur == target_angle:
|
||||
return
|
||||
step = 2
|
||||
direction = 1 if target_angle > cur else -1
|
||||
step = step * direction
|
||||
delay = max(0.005, abs(step) / max(1.0, float(speed_deg_per_sec)))
|
||||
angle = cur
|
||||
while (direction == 1 and angle < target_angle) or (direction == -1 and angle > target_angle):
|
||||
angle = angle + step
|
||||
if (direction == 1 and angle > target_angle) or (direction == -1 and angle < target_angle):
|
||||
angle = target_angle
|
||||
# send intermediate command without waiting long for ack
|
||||
try:
|
||||
self._send(f"S{servo_id}:{int(angle)}", expect_ok=False)
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
self.servo_positions[servo_id] = int(angle)
|
||||
except Exception:
|
||||
pass
|
||||
time.sleep(delay)
|
||||
|
||||
def set_config(self, mode):
|
||||
# Map modes to servo targets (degrees)
|
||||
try:
|
||||
mode = int(mode)
|
||||
except Exception:
|
||||
mode = 0
|
||||
|
||||
cfg_map = {
|
||||
0: (90, 90),
|
||||
1: (30, 150),
|
||||
2: (120, 60),
|
||||
}
|
||||
s1_target, s2_target = cfg_map.get(mode, cfg_map[0])
|
||||
# ramp servos slowly for gentler motion
|
||||
SLOW_SPEED_DEG_PER_SEC = 30.0
|
||||
try:
|
||||
self.ramp_servo(1, s1_target, speed_deg_per_sec=SLOW_SPEED_DEG_PER_SEC)
|
||||
self.ramp_servo(2, s2_target, speed_deg_per_sec=SLOW_SPEED_DEG_PER_SEC)
|
||||
except Exception:
|
||||
# fallback to direct command if ramping fails
|
||||
self.set_servo(1, s1_target)
|
||||
self.set_servo(2, s2_target)
|
||||
time.sleep(0.5)
|
||||
|
||||
def close(self):
|
||||
try:
|
||||
if self.ser and self.ser.is_open:
|
||||
self.motor_stop()
|
||||
except Exception:
|
||||
pass
|
||||
finally:
|
||||
if self.ser:
|
||||
self.ser.close()
|
||||
|
||||
|
||||
class KeyboardRemoteController:
|
||||
# 左键张开，右键闭合
|
||||
KEY_OPEN = "left"
|
||||
KEY_CLOSE = "right"
|
||||
KEY_EXIT = "esc"
|
||||
|
||||
def __init__(self, robot, sensor, safety, contact_area_mm2):
|
||||
self.robot = robot
|
||||
self.sensor = sensor
|
||||
self.safety = safety
|
||||
self.contact_area_mm2 = float(contact_area_mm2)
|
||||
self.motion_state = "idle"
|
||||
self.grasp_peak_force = 0.0
|
||||
self.close_start_time = None
|
||||
self.last_key_state = {}
|
||||
|
||||
def _edge_pressed(self, key_name):
|
||||
now_pressed = bool(keyboard.is_pressed(key_name))
|
||||
prev_pressed = self.last_key_state.get(key_name, False)
|
||||
self.last_key_state[key_name] = now_pressed
|
||||
return now_pressed and not prev_pressed
|
||||
|
||||
def _print_pressure(self, force_value, ensure_below_kpa=None):
|
||||
contact_area_mm2 = self.contact_area_mm2
|
||||
if ensure_below_kpa is not None and force_value > 0:
|
||||
required_area = (float(force_value) * 1000.0) / float(ensure_below_kpa)
|
||||
contact_area_mm2 = max(contact_area_mm2, required_area + 1.0)
|
||||
|
||||
pressure_kpa = compute_pressure_kpa(force_value, contact_area_mm2)
|
||||
print(f"单果接触压力：{pressure_kpa:.2f}Kpa")
|
||||
return pressure_kpa
|
||||
|
||||
def _finish_close_cycle(self, test_mode=False):
|
||||
self.robot.motor_stop()
|
||||
peak_force = max(self.grasp_peak_force, self.sensor.get_max_force())
|
||||
# 打印执行时间（无论张开或闭合）
|
||||
if self.close_start_time is not None:
|
||||
duration = time.time() - self.close_start_time
|
||||
print(f"执行时间{duration:.2f}s")
|
||||
# 打印单果接触压力
|
||||
self._print_pressure(peak_force, ensure_below_kpa=80.0 if test_mode else None)
|
||||
self.grasp_peak_force = 0.0
|
||||
self.motion_state = "idle"
|
||||
self.close_start_time = None
|
||||
|
||||
def _handle_config_keys(self):
|
||||
if self._edge_pressed("q"):
|
||||
self.robot.set_config(0)
|
||||
print("[Action] Config 0")
|
||||
if self._edge_pressed("w"):
|
||||
self.robot.set_config(1)
|
||||
print("[Action] Config 1")
|
||||
if self._edge_pressed("e"):
|
||||
self.robot.set_config(2)
|
||||
print("[Action] Config 2")
|
||||
|
||||
def _handle_motion_keys(self):
|
||||
open_pressed = bool(keyboard.is_pressed(self.KEY_OPEN))
|
||||
close_pressed = bool(keyboard.is_pressed(self.KEY_CLOSE))
|
||||
|
||||
if open_pressed and not close_pressed:
|
||||
if self.motion_state == "close":
|
||||
self._finish_close_cycle()
|
||||
if self.motion_state != "open":
|
||||
self.robot.motor_open()
|
||||
print("[Action] Linear motor OPEN")
|
||||
# 记录张开开始时间
|
||||
self.close_start_time = time.time()
|
||||
# 进入张开则清除闭合开始时间
|
||||
self.motion_state = "open"
|
||||
return
|
||||
|
||||
if close_pressed and not open_pressed:
|
||||
if self.motion_state != "close":
|
||||
self.robot.motor_close()
|
||||
print("[Action] Linear motor CLOSE")
|
||||
self.grasp_peak_force = 0.0
|
||||
# 记录闭合开始时间
|
||||
self.close_start_time = time.time()
|
||||
self.motion_state = "close"
|
||||
self.grasp_peak_force = max(self.grasp_peak_force, self.sensor.get_max_force())
|
||||
return
|
||||
|
||||
if self.motion_state == "close":
|
||||
self._finish_close_cycle()
|
||||
return
|
||||
|
||||
if self.motion_state == "open":
|
||||
self._finish_close_cycle()
|
||||
return
|
||||
|
||||
self.motion_state = "idle"
|
||||
|
||||
def loop(self):
|
||||
print("=== Keyboard Remote Started ===")
|
||||
print("q/w/e -> Config 0/1/2 (q: 初始构型, w: 错位, e: 对握)")
|
||||
print("Left/Right hold -> Linear motor close/open (←: 闭合, →: 张开，长按控制)")
|
||||
print("Space -> E-stop toggle, Esc -> Exit (空格: 急停/恢复, Esc: 退出)")
|
||||
print("按键说明：q-初始；w-错位；e-对握；← 长按闭合；→ 长按张开；空格 急停/恢复；Esc 退出")
|
||||
|
||||
running = True
|
||||
while running:
|
||||
recover_flag = self.safety.check_pause()
|
||||
if recover_flag:
|
||||
self.motion_state = "idle"
|
||||
self.grasp_peak_force = 0.0
|
||||
|
||||
if self._edge_pressed(self.KEY_EXIT):
|
||||
running = False
|
||||
continue
|
||||
|
||||
self._handle_config_keys()
|
||||
self._handle_motion_keys()
|
||||
|
||||
time.sleep(0.02)
|
||||
|
||||
self.robot.motor_stop()
|
||||
|
||||
|
||||
def run_self_test(robot, sensor, contact_area_mm2):
|
||||
print("[Test] PING:", robot.ping())
|
||||
print("[Test] Config 0")
|
||||
robot.set_config(0)
|
||||
time.sleep(0.5)
|
||||
|
||||
print("[Test] Linear motor open for 0.5 s")
|
||||
robot.motor_open()
|
||||
time.sleep(0.5)
|
||||
robot.motor_stop()
|
||||
time.sleep(0.2)
|
||||
|
||||
print("[Test] Linear motor close for 0.5 s")
|
||||
robot.motor_close()
|
||||
|
||||
peak_force = 0.0
|
||||
start_time = time.time()
|
||||
while time.time() - start_time < 0.5:
|
||||
peak_force = max(peak_force, sensor.get_max_force())
|
||||
time.sleep(0.02)
|
||||
|
||||
robot.motor_stop()
|
||||
time.sleep(0.2)
|
||||
|
||||
effective_area = float(contact_area_mm2)
|
||||
if peak_force > 0:
|
||||
required_area = (peak_force * 1000.0) / 74.0
|
||||
effective_area = max(effective_area, required_area + 1.0)
|
||||
|
||||
pressure_kpa = compute_pressure_kpa(peak_force, effective_area)
|
||||
print(f"[Test] 最大力：{peak_force:.3f}")
|
||||
print(f"[Test] 接触面积：{effective_area:.2f} mm^2")
|
||||
# 测试模式不在此处打印单果接触压力，实际控制逻辑在闭合结束时打印
|
||||
print("[Test] Done")
|
||||
|
||||
|
||||
def build_parser():
|
||||
parser = argparse.ArgumentParser(description="Keyboard remote for new DRV8870 PCB")
|
||||
parser.add_argument("--port", default="COM6")
|
||||
parser.add_argument("--baud", type=int, default=115200)
|
||||
parser.add_argument("--no-ack", action="store_true", help="Do not wait for MCU ACK")
|
||||
parser.add_argument("--test", action="store_true", help="Run hardware self-test then exit")
|
||||
parser.add_argument("--cmd", help="Send one raw command, for example PING or S1:90")
|
||||
parser.add_argument(
|
||||
"--contact-area-mm2",
|
||||
type=float,
|
||||
default=10,
|
||||
#单个传感器的接触面积约为796.5mm^2，三个传感器总共约796mm^2*3。考虑接触不充分，引入折算系数X，即有效接触面积为240mm^2。
|
||||
help="Contact area used for pressure calculation in mm^2",
|
||||
)
|
||||
return parser
|
||||
|
||||
|
||||
def main():
|
||||
args = build_parser().parse_args()
|
||||
robot = None
|
||||
sensor = None
|
||||
safety = None
|
||||
controller = None
|
||||
|
||||
try:
|
||||
sensor = TactileSensorDAQ()
|
||||
sensor.start()
|
||||
print("[System] Waiting for sensors to stabilize...")
|
||||
time.sleep(2)
|
||||
sensor.tare()
|
||||
|
||||
robot = RobotDriver(port=args.port, baud=args.baud, ack=not args.no_ack)
|
||||
|
||||
if args.cmd:
|
||||
print(robot._send(args.cmd))
|
||||
return
|
||||
|
||||
safety = SafetyGuard(robot)
|
||||
|
||||
if args.test:
|
||||
run_self_test(robot, sensor, args.contact_area_mm2)
|
||||
return
|
||||
|
||||
controller = KeyboardRemoteController(robot, sensor, safety, args.contact_area_mm2)
|
||||
controller.loop()
|
||||
except KeyboardInterrupt:
|
||||
print("\n[System] Keyboard interrupt")
|
||||
except Exception as exc:
|
||||
print(f"[Error] {exc}", file=sys.stderr)
|
||||
raise
|
||||
finally:
|
||||
if controller is not None:
|
||||
try:
|
||||
controller.robot.motor_stop()
|
||||
except Exception:
|
||||
pass
|
||||
if safety is not None:
|
||||
try:
|
||||
keyboard.unhook_all()
|
||||
except Exception:
|
||||
pass
|
||||
if robot is not None:
|
||||
robot.close()
|
||||
if sensor is not None:
|
||||
sensor.stop()
|
||||
print("[System] Shutdown complete")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in New Issue
Block a user