mirror of
https://github.com/m1ngsama/robot_arm.git
synced 2026-03-25 19:53:49 +00:00
refactor: extract magic constants to config.py (closes #7)
Create config.py with env-var-overridable hardware constants: - SERIAL_PORT / SERIAL_BAUD - LLM_MODEL_PATH / YOLO_MODEL_PATH - Z_HOVER / Z_GRAB / Z_AFTER_PICK (Z_TABLE eliminated — same as Z_GRAB) - REST_X / REST_Y / REST_Z (replaces three copies of [120,0,60]) - WS_X / WS_Y / WS_Z (workspace clipping bounds) - DAMPING_BUFFER_SIZE / DAMPING_MAX_SPEED / DAMPING_FACTOR - OFFSET_Y / OFFSET_Z arm_main.py: port/baud/damping defaults now read from config. voice_main.py: all magic constants replaced; remaining Chinese non-i18n strings translated to English. .gitignore: whitelist config.py.
This commit is contained in:
parent
fc02886bfa
commit
54c1dc8c01
4 changed files with 408 additions and 452 deletions
1
.gitignore
vendored
1
.gitignore
vendored
|
|
@ -45,6 +45,7 @@ temp_voice.wav
|
|||
!voice_main.py
|
||||
!arm_main.py
|
||||
!whisper_main.py
|
||||
!config.py
|
||||
!项目介绍文档.md
|
||||
!ck.md
|
||||
!lora.md
|
||||
21
arm_main.py
21
arm_main.py
|
|
@ -8,8 +8,10 @@ import numpy as np
|
|||
import serial
|
||||
from scipy.optimize import minimize
|
||||
|
||||
import config
|
||||
|
||||
class RobotArmUltimate:
|
||||
def __init__(self, port='COM3', baud=115200):
|
||||
def __init__(self, port=config.SERIAL_PORT, baud=config.SERIAL_BAUD):
|
||||
# --- 1. Physical parameters (mm) ---
|
||||
self.L1, self.L2, self.L3, self.L4 = 70.0, 75.0, 50.0, 130.0
|
||||
self.current_servos_logic = np.array([90.0, 90.0, 90.0, 90.0])
|
||||
|
|
@ -22,10 +24,10 @@ class RobotArmUltimate:
|
|||
self.OFFSET_Z = 0.0
|
||||
|
||||
# --- 3. Damping parameters ---
|
||||
self.buffer_size = 3
|
||||
self.buffer_size = config.DAMPING_BUFFER_SIZE
|
||||
self.servo_buffer = deque(maxlen=self.buffer_size)
|
||||
self.max_servo_speed = 30.0 # max servo speed (deg/frame)
|
||||
self.damping_factor = 0.7
|
||||
self.max_servo_speed = config.DAMPING_MAX_SPEED # max servo speed (deg/frame)
|
||||
self.damping_factor = config.DAMPING_FACTOR
|
||||
|
||||
try:
|
||||
self.ser = serial.Serial(port, baud, timeout=1)
|
||||
|
|
@ -150,11 +152,14 @@ class RobotArmUltimate:
|
|||
if self.ser: self.ser.close()
|
||||
|
||||
if __name__ == "__main__":
|
||||
arm = RobotArmUltimate(port='COM3')
|
||||
arm = RobotArmUltimate() # uses config.SERIAL_PORT / SERIAL_BAUD
|
||||
|
||||
arm.set_damping_params(buffer_size=3, max_speed=30.0, damping_factor=0.7)
|
||||
# Tune offset_y if end-effector droops during horizontal moves
|
||||
arm.set_correction(offset_y=-10.0, offset_z=0.0)
|
||||
arm.set_damping_params(
|
||||
buffer_size=config.DAMPING_BUFFER_SIZE,
|
||||
max_speed=config.DAMPING_MAX_SPEED,
|
||||
damping_factor=config.DAMPING_FACTOR,
|
||||
)
|
||||
arm.set_correction(offset_y=config.OFFSET_Y, offset_z=config.OFFSET_Z)
|
||||
|
||||
p_standby = np.array([110, 100, 20])
|
||||
p_pick1 = np.array([210, 110, 20])
|
||||
|
|
|
|||
52
config.py
Normal file
52
config.py
Normal file
|
|
@ -0,0 +1,52 @@
|
|||
# config.py — hardware constants and motion parameters
|
||||
# All values can be overridden via environment variables.
|
||||
import os
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Serial
|
||||
# ---------------------------------------------------------------------------
|
||||
SERIAL_PORT = os.environ.get("ROBOT_PORT", "COM3")
|
||||
SERIAL_BAUD = int(os.environ.get("ROBOT_BAUD", "115200"))
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Model paths
|
||||
# ---------------------------------------------------------------------------
|
||||
LLM_MODEL_PATH = os.environ.get("LLM_MODEL_PATH", r"D:\lora\2")
|
||||
YOLO_MODEL_PATH = os.environ.get("YOLO_MODEL_PATH", "best.pt")
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Z-axis key heights (mm), relative to robot base
|
||||
# ---------------------------------------------------------------------------
|
||||
Z_HOVER = float(os.environ.get("Z_HOVER", "120.0")) # safe transit height
|
||||
Z_GRAB = float(os.environ.get("Z_GRAB", "-15.0")) # table / grasp surface
|
||||
Z_AFTER_PICK = float(os.environ.get("Z_AFTER_PICK", "50.0")) # clearance after grasp
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Rest (home) position [x, y, z] in mm
|
||||
# Exposed as three scalars so config.py has no numpy dependency.
|
||||
# Callers: np.array([config.REST_X, config.REST_Y, config.REST_Z])
|
||||
# ---------------------------------------------------------------------------
|
||||
REST_X = float(os.environ.get("REST_X", "120.0"))
|
||||
REST_Y = float(os.environ.get("REST_Y", "0.0"))
|
||||
REST_Z = float(os.environ.get("REST_Z", "60.0"))
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Workspace hard limits (mm) — movements are clipped to these bounds
|
||||
# ---------------------------------------------------------------------------
|
||||
WS_X = (float(os.environ.get("WS_X_MIN", "80.0")), float(os.environ.get("WS_X_MAX", "250.0")))
|
||||
WS_Y = (float(os.environ.get("WS_Y_MIN", "-120.0")), float(os.environ.get("WS_Y_MAX", "120.0")))
|
||||
WS_Z = (float(os.environ.get("WS_Z_MIN", "-20.0")), float(os.environ.get("WS_Z_MAX", "200.0")))
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Damping / smoothing
|
||||
# ---------------------------------------------------------------------------
|
||||
DAMPING_BUFFER_SIZE = int(os.environ.get("DAMPING_BUFFER", "3"))
|
||||
DAMPING_MAX_SPEED = float(os.environ.get("DAMPING_MAX_SPEED", "25.0")) # deg/frame
|
||||
DAMPING_FACTOR = float(os.environ.get("DAMPING_FACTOR", "0.6"))
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Tilt correction offsets (servo degrees)
|
||||
# Tune OFFSET_Y > 0 if the end-effector droops during horizontal moves.
|
||||
# ---------------------------------------------------------------------------
|
||||
OFFSET_Y = float(os.environ.get("OFFSET_Y", "-10.0"))
|
||||
OFFSET_Z = float(os.environ.get("OFFSET_Z", "0.0"))
|
||||
786
voice_main.py
786
voice_main.py
File diff suppressed because it is too large
Load diff
Loading…
Reference in a new issue