基本测试完毕
这个提交包含在:
7
EZArm.py
7
EZArm.py
@ -87,7 +87,7 @@ class EZArm(Robot):
|
||||
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")}
|
||||
return {key+".pos": val for key, val in self.bus.sync_read("Goal_Position").items()}
|
||||
|
||||
def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
|
||||
if not self.is_connected:
|
||||
@ -95,4 +95,7 @@ class EZArm(Robot):
|
||||
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
|
||||
return action
|
||||
|
||||
def quick_test(self) ->None:
|
||||
self.bus.quick_test()
|
||||
@ -20,7 +20,7 @@ class PCA9685A:
|
||||
try:
|
||||
self.bus = smbus2.SMBus(bus_num)
|
||||
self.address = address
|
||||
self.current_angles = [90.0] * 16 # 存储所有通道的当前角度
|
||||
self.current_angles = [90] * 16 # 存储所有通道的当前角度
|
||||
self.reset()
|
||||
print(f"PCA9685 initialized on I2C bus {bus_num}, address 0x{address:02X}")
|
||||
except Exception as e:
|
||||
@ -100,7 +100,7 @@ class PCA9685A:
|
||||
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):
|
||||
def set_servo_angle(self, channel:int, angle:int, min_us=500, max_us=2400):
|
||||
"""
|
||||
设置并保持舵机角度
|
||||
SG90参数: 500μs = 0°, 2400μs = 180°
|
||||
@ -124,7 +124,7 @@ class PCA9685A:
|
||||
print(f"Channel {channel} set to {angle}° (pulse: {pulse_width:.0f}μs)")
|
||||
return pulse_width
|
||||
|
||||
def get_current_angle(self, channel:int) -> float:
|
||||
def get_current_angle(self, channel:int) -> int:
|
||||
"""获取指定通道的当前角度"""
|
||||
if 0 <= channel <= 15:
|
||||
return self.current_angles[channel]
|
||||
|
||||
@ -1,5 +1,5 @@
|
||||
from lerobot.motors import MotorsBus
|
||||
from lerobot.motors.motors_bus import Motor, MotorCalibration
|
||||
from lerobot.motors.motors_bus import Motor, MotorCalibration, PortHandler
|
||||
|
||||
from .PCA9685A import PCA9685A
|
||||
|
||||
@ -20,6 +20,11 @@ class PWMMotorBus(MotorsBus):
|
||||
raise ConnectionError("No I2C device setted!")
|
||||
pass
|
||||
|
||||
@property
|
||||
def is_connected(self) -> bool:
|
||||
"""bool: `True` if the underlying serial port is open."""
|
||||
return True
|
||||
|
||||
def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
|
||||
raise NotImplementedError
|
||||
|
||||
@ -50,7 +55,7 @@ class PWMMotorBus(MotorsBus):
|
||||
# 读取校准数据
|
||||
return {}
|
||||
|
||||
def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
|
||||
def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache:bool = True) -> None:
|
||||
# 写入校准数据
|
||||
return None
|
||||
|
||||
@ -75,7 +80,7 @@ class PWMMotorBus(MotorsBus):
|
||||
raise TypeError()
|
||||
else:
|
||||
for (name,angle) in values.items():
|
||||
self.PWMBus.set_servo_angle(self.motors[name].id, angle)
|
||||
self.PWMBus.set_servo_angle(self.motors[name].id, int(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]:
|
||||
@ -85,4 +90,27 @@ class PWMMotorBus(MotorsBus):
|
||||
for (name,Motor) in self.motors.items():
|
||||
status[name]=(self.PWMBus.get_current_angle(Motor.id))
|
||||
return status
|
||||
return {}
|
||||
return {}
|
||||
|
||||
def disconnect(self, disable_torque: bool = True) -> None:
|
||||
for i in range(16):
|
||||
self.PWMBus.set_servo_angle(i,90)
|
||||
self.PWMBus.reset()
|
||||
|
||||
def quick_test(self) -> None:
|
||||
for i in self.motors:
|
||||
channel = self.motors[i].id
|
||||
try:
|
||||
import time
|
||||
self.PWMBus.set_servo_angle(channel, 90)
|
||||
time.sleep(1)
|
||||
self.PWMBus.set_servo_angle(channel, 135)
|
||||
time.sleep(1)
|
||||
self.PWMBus.set_servo_angle(channel, 90)
|
||||
time.sleep(1)
|
||||
self.PWMBus.set_servo_angle(channel, 45)
|
||||
time.sleep(1)
|
||||
self.PWMBus.set_servo_angle(channel, 90)
|
||||
print(f"Channel {channel}: OK")
|
||||
except Exception as e:
|
||||
print(f"Channel {channel}: Error - {e}")
|
||||
在新工单中引用
屏蔽一个用户