初版测试

这个提交包含在:
2025-09-10 22:16:35 +08:00
当前提交 3619ac91f2
共有 5 个文件被更改,包括 383 次插入 和 0 次删除

98
EZArm.py 普通文件
查看文件

@ -0,0 +1,98 @@
from dataclasses import dataclass, field
from typing import Any
from lerobot.robots import RobotConfig,Robot
from lerobot.motors import Motor,MotorNormMode
from .PWMMotorBus import PWMMotorBus
@RobotConfig.register_subclass("EZRobot")
@dataclass
class EZArmConfig(RobotConfig):
port: str
class EZArm(Robot):
config_class = EZArmConfig
name = "EasyArm"
def __init__(self, config: EZArmConfig):
super().__init__(config)
self.config = config
self.bus = PWMMotorBus(
port=self.config.port,
motors={
"Ground":Motor(0,"PWM",MotorNormMode.DEGREES),
"Joint0":Motor(1,"PWM",MotorNormMode.DEGREES),
"Joint2":Motor(2,"PWM",MotorNormMode.DEGREES),
"Joint3":Motor(3,"PWM",MotorNormMode.DEGREES),
"Turner":Motor(4,"PWM",MotorNormMode.DEGREES),
"Claw":Motor(5,"PWM",MotorNormMode.DEGREES)
},
calibration=self.calibration)
pass
@property
def _motors_ft(self) -> dict[str, type]:
return {
"Ground.pos": float,
"Joint0.pos": float,
"Joint2.pos": float,
"Joint3.pos": float,
"Turner.pos": float,
"Claw.pos": float
}
@property
def _cameras_ft(self) -> dict[str, tuple|None]:
return {
"cam": None
}
@property
def observation_features(self) -> dict:
return {**self._motors_ft,**self._cameras_ft}
@property
def action_features(self) -> dict:
return self._motors_ft
@property
def is_connected(self) -> bool:
return self.bus.is_connected
def connect(self, calibrate: bool = True) -> None:
self.bus.connect()
if not self.is_calibrated and calibrate:
self.calibrate()
self.configure()
return super().connect(calibrate)
def disconnect(self) -> None:
self.bus.disconnect()
return super().disconnect()
@property
def is_calibrated(self) -> bool:
# 设备无需校准,处于协议兼容性,这里留下检测是否校准的属性
return True
def calibrate(self) -> None:
# 设备无需校准
pass
def configure(self) -> None:
# 设备无需特殊配置
return super().configure()
def get_observation(self) -> dict[str, Any]:
if not self.is_connected:
raise ConnectionError(f'{self} is Not Connected!')
return {key+".pos": val for key, val in self.bus.sync_read("Goal_Position")}
def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
if not self.is_connected:
raise ConnectionError(f'{self} is Not Connected!')
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items()}
# Send goal position to the arm
self.bus.sync_write("Goal_Position", goal_pos)
return action

195
PCA9685A.py 普通文件
查看文件

@ -0,0 +1,195 @@
import time
import smbus2
# PCA9685寄存器地址
MODE1 = 0x00
MODE2 = 0x01
PRESCALE = 0xFE
LED0_ON_L = 0x06
LED0_ON_H = 0x07
LED0_OFF_L = 0x08
LED0_OFF_H = 0x09
ALLLED_ON_L = 0xFA
ALLLED_ON_H = 0xFB
ALLLED_OFF_L = 0xFC
ALLLED_OFF_H = 0xFD
class PCA9685A:
def __init__(self, bus_num=1, address=0x40):
"""初始化PCA9685控制器"""
try:
self.bus = smbus2.SMBus(bus_num)
self.address = address
self.current_angles = [90.0] * 16 # 存储所有通道的当前角度
self.reset()
print(f"PCA9685 initialized on I2C bus {bus_num}, address 0x{address:02X}")
except Exception as e:
print(f"PCA9685 initialization failed: {e}")
raise
def reset(self):
"""重置PCA9685"""
try:
self.write_reg(MODE1, 0x00)
time.sleep(0.01)
self.write_reg(MODE2, 0x04)
print("PCA9685 reset successfully")
except Exception as e:
print(f"Reset failed: {e}")
def write_reg(self, reg, value):
"""写入寄存器"""
try:
self.bus.write_byte_data(self.address, reg, value)
time.sleep(0.001)
except Exception as e:
print(f"Write register 0x{reg:02X} failed: {e}")
raise
def read_reg(self, reg):
"""读取寄存器"""
try:
return self.bus.read_byte_data(self.address, reg)
except Exception as e:
print(f"Read register 0x{reg:02X} failed: {e}")
return 0
def set_pwm_freq(self, freq):
"""设置PWM频率"""
if freq < 24:
freq = 24
elif freq > 1526:
freq = 1526
# 计算预分频值
prescaleval = 25000000.0 # 25MHz
prescaleval /= 4096.0 # 12-bit
prescaleval /= float(freq)
prescaleval -= 1.0
prescale = int(prescaleval + 0.5)
# 保存当前模式并设置睡眠模式
old_mode = self.read_reg(MODE1)
new_mode = (old_mode & 0x7F) | 0x10
self.write_reg(MODE1, new_mode)
# 设置预分频器
self.write_reg(PRESCALE, prescale)
# 恢复模式
self.write_reg(MODE1, old_mode)
time.sleep(0.005)
# 启用自动递增
self.write_reg(MODE1, old_mode | 0x20)
print(f"PWM frequency set to {freq}Hz")
def set_pwm(self, channel, on, off):
"""设置单个PWM通道"""
if channel < 0 or channel > 15:
raise ValueError("Channel must be between 0-15")
try:
# 设置ON时间
self.bus.write_byte_data(self.address, LED0_ON_L + 4 * channel, on & 0xFF)
self.bus.write_byte_data(self.address, LED0_ON_H + 4 * channel, on >> 8)
# 设置OFF时间
self.bus.write_byte_data(self.address, LED0_OFF_L + 4 * channel, off & 0xFF)
self.bus.write_byte_data(self.address, LED0_OFF_H + 4 * channel, off >> 8)
except Exception as e:
print(f"Set PWM channel {channel} failed: {e}")
def set_servo_angle(self, channel:int, angle:float, min_us=500, max_us=2400):
"""
设置并保持舵机角度
SG90参数: 500μs = 0°, 2400μs = 180°
"""
# 限制角度范围
angle = max(0, min(180, angle))
# 更新当前角度
self.current_angles[channel] = angle
# 计算脉冲宽度
pulse_width = min_us + (angle * (max_us - min_us) / 180)
# 转换为PWM值 (50Hz频率,周期20000μs)
pulse = int(pulse_width * 4096 / 20000)
pulse = max(0, min(4095, pulse))
# 设置PWM并保持
self.set_pwm(channel, 0, pulse)
print(f"Channel {channel} set to {angle}° (pulse: {pulse_width:.0f}μs)")
return pulse_width
def get_current_angle(self, channel:int) -> float:
"""获取指定通道的当前角度"""
if 0 <= channel <= 15:
return self.current_angles[channel]
else:
raise ValueError("Channel must be between 0-15")
def set_all_servos(self, angle, min_us=500, max_us=2400):
"""设置所有舵机到相同角度"""
for channel in range(16):
self.set_servo_angle(channel, angle, min_us, max_us)
time.sleep(0.01) # 短暂延迟
print(f"All channels set to {angle}°")
def set_servo_range(self, channels, angle, min_us=500, max_us=2400):
"""设置多个舵机到指定角度"""
if isinstance(channels, int):
channels = [channels]
for channel in channels:
if 0 <= channel <= 15:
self.set_servo_angle(channel, angle, min_us, max_us)
time.sleep(0.1)
else:
print(f"Invalid channel: {channel}")
def smooth_move(self, channel, target_angle, duration=2.0, min_us=500, max_us=2400):
"""平滑移动到目标角度"""
start_angle = self.get_current_angle(channel)
steps = int(duration * 20) # 20 updates per second
step_delay = duration / steps
for i in range(steps + 1):
progress = i / steps
current_angle = start_angle + (target_angle - start_angle) * progress
self.set_servo_angle(channel, current_angle, min_us, max_us)
time.sleep(step_delay)
print(f"Channel {channel} smoothly moved to {target_angle}°")
def hold_position(self, channel):
"""保持舵机当前位置"""
current_angle = self.get_current_angle(channel)
self.set_servo_angle(channel, current_angle)
print(f"Channel {channel} holding position at {current_angle}°")
def hold_all_positions(self):
"""保持所有舵机当前位置"""
for channel in range(16):
self.hold_position(channel)
print("All channels holding their positions")
def scan_channels(self):
"""扫描并测试所有通道"""
print("Scanning all PCA9685 channels...")
for channel in range(16):
try:
# 快速测试通道
self.set_servo_angle(channel, 90)
time.sleep(0.05)
self.set_servo_angle(channel, 120)
time.sleep(0.05)
self.set_servo_angle(channel, 60)
time.sleep(0.05)
self.set_servo_angle(channel, 90)
print(f"Channel {channel}: OK")
except Exception as e:
print(f"Channel {channel}: Error - {e}")
print("Channel scan completed")

84
PWMMotorBus.py 普通文件
查看文件

@ -0,0 +1,84 @@
from lerobot.motors import MotorsBus
from lerobot.motors.motors_bus import Motor, MotorCalibration
from PCA9685A import PCA9685A
class PWMMotorBus(MotorsBus):
"""`Lerobot`兼容的PWM舵机总线"""
def __init__(self, port: str, motors: dict[str, Motor], calibration: dict[str, MotorCalibration] | None = None):
self.PWMBus : PCA9685A
super().__init__(port, motors, calibration)
if "/dev/i2c-" in port:
self.PWMBus = PCA9685A(int(port.replace("/dev/i2c-","")))
else:
raise ConnectionError("No I2C device setted!")
pass
def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
raise NotImplementedError
def _handshake(self) -> None:
raise NotImplementedError
def _find_single_motor(self, motor: str, initial_baudrate: int | None) -> tuple[int, int]:
raise NotImplementedError
def configure_motors(self) -> None:
raise NotImplementedError
def disable_torque(self, motors: int | str | list[str] | None = None, num_retry: int = 0) -> None:
raise NotImplementedError
def _disable_torque(self, motor: int, model: str, num_retry: int = 0) -> None:
raise NotImplementedError
def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
raise NotImplementedError
@property
def is_calibrated(self) -> bool:
# 设备无需额外校准,默认校准完毕。
return True
def read_calibration(self) -> dict[str, MotorCalibration]:
# 读取校准数据
return {}
def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
# 写入校准数据
return None
def _get_half_turn_homings(self, positions: dict[str | int, int | float]) -> dict[str | int, int | float]:
raise NotImplementedError
def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
raise NotImplementedError
def _decode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
raise NotImplementedError
def _split_into_byte_chunks(self, value: int, length: int) -> list[int]:
raise NotImplementedError
def broadcast_ping(self, num_retry: int = 0, raise_on_error: bool = False) -> dict[int, int] | None:
raise NotImplementedError
def sync_write(self, data_name: str, values: int | float | dict[str, int | float], *, normalize: bool = True, num_retry: int = 0) -> None:
if data_name == "Goal_Position":
if not isinstance(values,dict):
raise TypeError()
else:
for (name,angle) in values.items():
self.PWMBus.set_servo_angle(self.motors[name].id, angle)
pass
def sync_read(self, data_name: str, motors: str | list[str] | None = None, *, normalize: bool = True, num_retry: int = 0) -> dict[str, int | float]:
# return super().sync_read(data_name, motors, normalize=normalize, num_retry=num_retry)
if data_name == "Goal_Position":
status = {}
for (name,Motor) in self.motors.items():
status[name]=(self.PWMBus.get_current_angle(Motor.id))
return status
return {}

3
README.MD 普通文件
查看文件

@ -0,0 +1,3 @@
# EZArm框架
## 概要
`EZArm`拟通过`PCA9685A`模块,构建一个兼容`lerobot`生态的简易机械臂。

3
__init__.py 普通文件
查看文件

@ -0,0 +1,3 @@
from .PCA9685A import PCA9685A
from .PWMMotorBus import PWMMotorBus
from .EZArm import EZArm,EZArmConfig