163 lines
4.2 KiB
Python
163 lines
4.2 KiB
Python
#!/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() # 旋转180度
|
||
time.sleep(1)
|
||
# move_to_ori_position() # 旋转0度
|
||
# time.sleep(1)
|
||
|
||
while True:
|
||
time.sleep(1)
|
||
|