初版测试
这个提交包含在:
98
EZArm.py
普通文件
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
普通文件
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
普通文件
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
普通文件
3
README.MD
普通文件
@ -0,0 +1,3 @@
|
||||
# EZArm框架
|
||||
## 概要
|
||||
`EZArm`拟通过`PCA9685A`模块,构建一个兼容`lerobot`生态的简易机械臂。
|
||||
3
__init__.py
普通文件
3
__init__.py
普通文件
@ -0,0 +1,3 @@
|
||||
from .PCA9685A import PCA9685A
|
||||
from .PWMMotorBus import PWMMotorBus
|
||||
from .EZArm import EZArm,EZArmConfig
|
||||
在新工单中引用
屏蔽一个用户