基本测试完毕
这个提交包含在:
7
EZArm.py
7
EZArm.py
@ -87,7 +87,7 @@ class EZArm(Robot):
|
|||||||
def get_observation(self) -> dict[str, Any]:
|
def get_observation(self) -> dict[str, Any]:
|
||||||
if not self.is_connected:
|
if not self.is_connected:
|
||||||
raise ConnectionError(f'{self} is Not 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]:
|
def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
|
||||||
if not self.is_connected:
|
if not self.is_connected:
|
||||||
@ -95,4 +95,7 @@ class EZArm(Robot):
|
|||||||
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items()}
|
goal_pos = {key.removesuffix(".pos"): val for key, val in action.items()}
|
||||||
# Send goal position to the arm
|
# Send goal position to the arm
|
||||||
self.bus.sync_write("Goal_Position", goal_pos)
|
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:
|
try:
|
||||||
self.bus = smbus2.SMBus(bus_num)
|
self.bus = smbus2.SMBus(bus_num)
|
||||||
self.address = address
|
self.address = address
|
||||||
self.current_angles = [90.0] * 16 # 存储所有通道的当前角度
|
self.current_angles = [90] * 16 # 存储所有通道的当前角度
|
||||||
self.reset()
|
self.reset()
|
||||||
print(f"PCA9685 initialized on I2C bus {bus_num}, address 0x{address:02X}")
|
print(f"PCA9685 initialized on I2C bus {bus_num}, address 0x{address:02X}")
|
||||||
except Exception as e:
|
except Exception as e:
|
||||||
@ -100,7 +100,7 @@ class PCA9685A:
|
|||||||
except Exception as e:
|
except Exception as e:
|
||||||
print(f"Set PWM channel {channel} failed: {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°
|
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)")
|
print(f"Channel {channel} set to {angle}° (pulse: {pulse_width:.0f}μs)")
|
||||||
return pulse_width
|
return pulse_width
|
||||||
|
|
||||||
def get_current_angle(self, channel:int) -> float:
|
def get_current_angle(self, channel:int) -> int:
|
||||||
"""获取指定通道的当前角度"""
|
"""获取指定通道的当前角度"""
|
||||||
if 0 <= channel <= 15:
|
if 0 <= channel <= 15:
|
||||||
return self.current_angles[channel]
|
return self.current_angles[channel]
|
||||||
|
|||||||
@ -1,5 +1,5 @@
|
|||||||
from lerobot.motors import MotorsBus
|
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
|
from .PCA9685A import PCA9685A
|
||||||
|
|
||||||
@ -20,6 +20,11 @@ class PWMMotorBus(MotorsBus):
|
|||||||
raise ConnectionError("No I2C device setted!")
|
raise ConnectionError("No I2C device setted!")
|
||||||
pass
|
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:
|
def _assert_protocol_is_compatible(self, instruction_name: str) -> None:
|
||||||
raise NotImplementedError
|
raise NotImplementedError
|
||||||
|
|
||||||
@ -50,7 +55,7 @@ class PWMMotorBus(MotorsBus):
|
|||||||
# 读取校准数据
|
# 读取校准数据
|
||||||
return {}
|
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
|
return None
|
||||||
|
|
||||||
@ -75,7 +80,7 @@ class PWMMotorBus(MotorsBus):
|
|||||||
raise TypeError()
|
raise TypeError()
|
||||||
else:
|
else:
|
||||||
for (name,angle) in values.items():
|
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
|
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]:
|
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():
|
for (name,Motor) in self.motors.items():
|
||||||
status[name]=(self.PWMBus.get_current_angle(Motor.id))
|
status[name]=(self.PWMBus.get_current_angle(Motor.id))
|
||||||
return status
|
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}")
|
||||||
在新工单中引用
屏蔽一个用户