pengqi/wire_controlsystem
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修改传送带电机、舵机、达妙电机等相关接口

Browse Source
This commit is contained in:
pengqi
2026-02-26 15:42:22 +08:00
parent 5c9946dfc7
commit 8d2e91b8b3
10 changed files with 1642 additions and 331 deletions
Download Patch File Download Diff File Expand all files Collapse all files

145
servo/servo_control.py Normal file
View File

@ -0,0 +1,145 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
"""
# @Time : 2026/2/26 14:07
# @Author : reenrr
# @File : servo_control.py
# @Desc : 舵机对外控制接口
"""
import time
from servo_test import ServoController
# -------------参数配置--------------
BAUDRATE = 115200 # 舵机的波特率
PORT = 'COM4'
SERVO_IDS = [1, 2, 3, 4, 5] # 舵机们的 ID 号
POS_START = 2047
POS_END = 0
SPEED = 1500
ACC = 0
# 舵机参数换算常量（核心：0度=0，360度=4095）
DEGREE_TO_RAW_RATIO = 4095 / 360 # 1度对应的原始值
RAW_TO_DEGREE_RATIO = 360 / 4095 # 1个原始值对应的角度
custom_config = {
'port': PORT,
'baudrate': BAUDRATE,
'servo_ids': SERVO_IDS,
'pos_start': POS_START,
'pos_end': POS_END,
'speed': SPEED,
'acc': ACC,
}
class ServoInterface:
def __init__(self, config=None):
self.servo_controller = ServoController(config)
try:
# 初始化串口
self.servo_controller.setup_port()
self.servo_controller.enable_all_servos()
print("舵机控制器初始化成功（串口/舵机启用完成）")
except Exception as e:
raise RuntimeError(f"舵机初始化失败:{str(e)}") from e
# 全局变量
_Servo = ServoInterface(custom_config)
def _degree_to_raw(degree: int) -> int:
"""
角度转原始值
:param degree: 角度值(0-360)
:return: 原始值(0-4095)
"""
if not (0 <= degree <= 360):
raise ValueError(f"角度值必须在0-360度之间，当前输入:{degree}")
raw_value = int(degree * DEGREE_TO_RAW_RATIO)
return max(0, min(raw_value, 4095))
def _raw_to_degree(raw: int) -> float:
"""
原始值转角度
:param raw: 原始值(0-4095)
:return: 角度值(0-360)
"""
return round(raw * RAW_TO_DEGREE_RATIO, 2)
def set_servo_speed(value: int):
"""
设置舵机速度
:param value: 速度值
"""
global SPEED
if 0 <= value <= 3400:
SPEED = value
print(f"舵机速度已设置为:{SPEED}")
else:
raise ValueError("速度值超出限定范围")
def set_servo_acceleration(value: int):
"""
设置舵机加速度
:param value: 加速度值
"""
global ACC
if 0 <= value <= 254:
ACC = value
print(f"舵机加速度已设置为:{ACC}")
else:
raise ValueError("加速度值超出限定范围")
def set_servo_ori_position(ori_position: int):
"""
设置舵机原点位
:param ori_position: 舵机原点位
"""
global POS_START
POS_START = _degree_to_raw(ori_position)
print(f"舵机原点位置已设置为：{ori_position}度（对应原始值：{POS_START}）")
def set_servo_rot_position(rot_position: int):
"""
设置舵机翻转位置
:param rot_position: 舵机翻转位
"""
global POS_END
POS_END = _degree_to_raw(rot_position)
print(f"舵机翻转位置已设置为：{rot_position}度（对应原始值：{POS_END}）")
def move_to_rot_position():
"""舵机旋转到翻转位置"""
try:
_Servo.servo_controller.write_position(POS_END, SPEED, ACC)
time.sleep(2)
target_degree = _raw_to_degree(POS_END)
print(f"舵机已移动到翻转位置:{target_degree}度（原始值:{POS_END}）")
except Exception as e:
raise RuntimeError(f"舵机移动到翻转位置失败:{str(e)}") from e
def move_to_ori_position():
"""舵机旋转到原点"""
try:
_Servo.servo_controller.write_position(POS_START, SPEED, ACC)
time.sleep(2)
target_degree = _raw_to_degree(POS_START)
print(f"舵机已移动到原点位置:{target_degree}度（原始值:{POS_START}）")
except Exception as e:
raise RuntimeError(f"舵机移动到原点位置失败:{str(e)}") from e
# ----------调试接口----------
if __name__ == '__main__':
set_servo_speed(1500)
set_servo_acceleration(0)
set_servo_ori_position(0)
set_servo_rot_position(180)
move_to_rot_position()
time.sleep(1)

49
servo/servo_test.py
View File

@ -1,20 +1,11 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
"""
# @Time : 2026/1/5 17:01
# @Author : reenrr
# @File : servo_test.py
# @Desc : 测试舵机
'''
#!/usr/bin/env python
# -*- coding: utf-8 -*-
'''
# @Time : 2025/12/2 14:05
# @Author : reenrr
# @File :servo.py
# @Description : 控制舵机正反转
'''
"""
from servo_sdk import *
import logging
@ -49,13 +40,13 @@ class ServoController:
"""配置串口参数"""
# 打开串口
if self.port_handler.openPort():
logging.info("成功打开串口")
print("成功打开串口")
else:
raise RuntimeError("打开串口失败")
# 设置波特率
if self.port_handler.setBaudRate(self.config['baudrate']):
logging.info("设置波特率成功")
print("设置波特率成功")
else:
self.port_handler.closePort()
raise RuntimeError("设置波特率失败")
@ -66,16 +57,16 @@ class ServoController:
for servo_id in self.config['servo_ids']:
enable_result = self.servo_handler.torqueEnable(servo_id)
if enable_result[1] != 0:
logging.info(f"[ID:{servo_id:02d}] 舵机使能失败，错误信息：{enable_result[1]}")
print(f"[ID:{servo_id:02d}] 舵机使能失败，错误信息：{enable_result[1]}")
enable_success = False
else:
logging.info(f"[ID:{servo_id:02d}] 舵机使能成功")
print(f"[ID:{servo_id:02d}] 舵机使能成功")
if not enable_success:
self.cleanup()
raise RuntimeError("使能舵机失败,程序退出")
logging.info("所有舵机使能成功")
print("所有舵机使能成功")
def disable_all_servos(self):
"""失能所有舵机"""
@ -83,16 +74,16 @@ class ServoController:
for servo_id in self.config['servo_ids']:
disable_result = self.servo_handler.torqueDisable(servo_id)
if disable_result[1] != 0:
logging.info(f"[ID:{servo_id:02d}] 舵机失能失败，错误信息：{disable_result[1]}")
print(f"[ID:{servo_id:02d}] 舵机失能失败，错误信息：{disable_result[1]}")
disable_success = False
else:
logging.info(f"[ID:{servo_id:02d}] 舵机失能成功")
print(f"[ID:{servo_id:02d}] 舵机失能成功")
if not disable_success:
self.cleanup()
raise RuntimeError("失能舵机失败,程序退出")
logging.info("所有舵机失能成功")
print("所有舵机失能成功")
def write_position(self, position, speed, acc):
"""
@ -106,13 +97,13 @@ class ServoController:
servo_id, position, speed, acc
)
if not add_param_result:
logging.info(f"[ID:{servo_id:02d}] 添加参数失败")
print(f"[ID:{servo_id:02d}] 添加参数失败")
continue
result = self.servo_handler.GroupSyncWrite.txPacket()
if result != COMM_SUCCESS:
logging.info(f"[ID:{servo_id:02d}] 发送指令失败：{result.getTxRxResult(result)}")
logging.info("复位成功")
print(f"[ID:{servo_id:02d}] 发送指令失败：{result.getTxRxResult(result)}")
print("复位成功")
# 清空参数缓存
self.servo_handler.GroupSyncWrite.clearParam()
@ -126,26 +117,26 @@ class ServoController:
self.disable_all_servos()
if self.port_handler.is_open():
self.port_handler.closePort()
logging.info("串口已关闭")
print("串口已关闭")
def run_cycle(self):
"""运行舵机循环运动"""
self.is_running = True
logging.info("舵机开始循环运动（按Ctrl+C终止）...")
print("舵机开始循环运动（按Ctrl+C终止）...")
while self.is_running:
# 运动到起始位置（180度）
self.write_position(self.config['pos_start'],
self.config['speed'],
self.config['acc'])
logging.info("运动到180度")
print("运动到180度")
time.sleep(self.time_interval1)
# 运动到结束位置（0度）
self.write_position(self.config['pos_end'],
self.config['speed'],
self.config['acc'])
logging.info("运动到0度")
print("运动到0度")
time.sleep(self.time_interval2)
def run(self):
@ -162,12 +153,12 @@ class ServoController:
# 运行循环
self.run_cycle()
except KeyboardInterrupt:
logging.info("\n用户终止程序...")
print("\n用户终止程序...")
self.stop()
except RuntimeError as e:
logging.info(f"\n运行错误：{str(e)}")
print(f"\n运行错误：{str(e)}")
except Exception as e:
logging.info(f"\n程序运行异常：{str(e)}")
print(f"\n程序运行异常：{str(e)}")
finally:
self.cleanup()