69 lines
2.2 KiB
Python
69 lines
2.2 KiB
Python
# -190000 190000
|
||
# -180000 0 90 -55000 - -185000 = 130000
|
||
# -210000 0 90 -110000 - -240000 = 130000
|
||
import time
|
||
from src.ZDT_Servo import Emm42Driver
|
||
|
||
def test_three_motors():
|
||
# 初始化三个电机（假设串口号为COM3-COM5）
|
||
motors = [
|
||
Emm42Driver(port="/dev/ttyAMA5", device_id=1),
|
||
Emm42Driver(port="/dev/ttyAMA5", device_id=2),
|
||
Emm42Driver(port="/dev/ttyAMA5", device_id=3)
|
||
]
|
||
|
||
try:
|
||
# 能所有电机
|
||
|
||
for motor in motors:
|
||
motor.enable_motor(enable=False)
|
||
|
||
|
||
# 初始化位置极值记录
|
||
position_stats = [
|
||
{"min": float('inf'), "max": float('-inf')}, # 电机1
|
||
{"min": float('inf'), "max": float('-inf')}, # 电机2
|
||
{"min": float('inf'), "max": float('-inf')} # 电机3
|
||
]
|
||
|
||
# 无限循环读取位置，按q退出
|
||
print("Reading positions (press 'q' to quit)...")
|
||
try:
|
||
while True:
|
||
for i, motor in enumerate(motors):
|
||
pos = motor.read_position()
|
||
if pos is not None:
|
||
position_stats[i]["min"] = min(position_stats[i]["min"], pos)
|
||
position_stats[i]["max"] = max(position_stats[i]["max"], pos)
|
||
|
||
# 非阻塞检测键盘输入
|
||
try:
|
||
import sys
|
||
import select
|
||
if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
|
||
key = sys.stdin.read(1)
|
||
if key.lower() == 'q':
|
||
break
|
||
except:
|
||
pass
|
||
|
||
time.sleep(0.1)
|
||
|
||
except KeyboardInterrupt:
|
||
pass
|
||
|
||
# 打印结果
|
||
print("\nPosition statistics:")
|
||
for i, stats in enumerate(position_stats, 1):
|
||
print(f"Motor {i}: min={stats['min']}, max={stats['max']}")
|
||
|
||
finally:
|
||
# 失能所有电机
|
||
print("\nDisabling motors...")
|
||
for motor in motors:
|
||
motor.enable_motor(enable=False)
|
||
time.sleep(0.1)
|
||
|
||
if __name__ == "__main__":
|
||
test_three_motors()
|