线条厂各设备控制代码

servo/servo.py

@@ -0,0 +1,183 @@
+#!/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
+
+# -------------参数配置--------------
+BAUDRATE = 115200                        # 舵机的波特率
+PORT = 'COM4'
+SERVO_IDS = [1, 2, 3, 4, 5]              # 舵机们的 ID 号
+POS_START = 2047
+POS_END = 0
+SPEED = 1500
+ACC = 0
+TIME_INTERVAL1 = ((2047-0) / 1500) + 2   # 翻转回来的时间
+TIME_INTERVAL2 = 10                      # 不用翻转运行的时间
+
+class ServoController:
+    def __init__(self, config=None):
+        """
+        初始化舵机控制器
+        """
+        self.config = config
+        self.time_interval1 = self.config['time_interval1']
+        self.time_interval2 = self.config['time_interval2']
+
+        # 初始化串口和舵机处理器
+        self.port_handler = PortHandler(self.config['port'])
+        self.servo_handler = HxServoHandler(self.port_handler)
+
+        # 初始化状态
+        self.is_running = False
+
+    def setup_port(self):
+        """配置串口参数"""
+        # 打开串口
+        if self.port_handler.openPort():
+            logging.info("成功打开串口")
+        else:
+            raise RuntimeError("打开串口失败")
+
+        # 设置波特率
+        if self.port_handler.setBaudRate(self.config['baudrate']):
+            logging.info("设置波特率成功")
+        else:
+            self.port_handler.closePort()
+            raise RuntimeError("设置波特率失败")
+
+    def enable_all_servos(self):
+        """使能所有舵机"""
+        enable_success = True
+        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]}")
+                enable_success = False
+            else:
+                logging.info(f"[ID:{servo_id:02d}] 舵机使能成功")
+
+        if not enable_success:
+            self.cleanup()
+            raise RuntimeError("使能舵机失败,程序退出")
+
+        logging.info("所有舵机使能成功")
+
+    def disable_all_servos(self):
+        """失能所有舵机"""
+        disable_success = True
+        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]}")
+                disable_success = False
+            else:
+                logging.info(f"[ID:{servo_id:02d}] 舵机失能成功")
+
+        if not disable_success:
+            self.cleanup()
+            raise RuntimeError("失能舵机失败,程序退出")
+
+        logging.info("所有舵机失能成功")
+
+    def write_position(self, position, speed, acc):
+        """
+        写入目标位置到所有舵机
+        :param position: 目标位置
+        :param speed: 速度
+        :param acc: 加速度
+        """
+        for servo_id in self.config['servo_ids']:
+            add_param_result = self.servo_handler.syncWritePosEx(
+                servo_id, position, speed, acc
+            )
+            if not add_param_result:
+                logging.info(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("复位成功")
+
+        # 清空参数缓存
+        self.servo_handler.GroupSyncWrite.clearParam()
+
+    def stop(self):
+        """停止舵机运行"""
+        self.is_running = False
+
+    def cleanup(self):
+        """清理资源"""
+        self.disable_all_servos()
+        if self.port_handler.is_open():
+            self.port_handler.closePort()
+            logging.info("串口已关闭")
+
+
+# ---------舵机对外接口----------
+def servo_rotate():
+    """复位位置之后，先转到180度，延时TIME_INTERVAL1一段时间，再转到0度"""
+    custom_config = {
+        'port': PORT,
+        'baudrate': BAUDRATE,
+        'servo_ids': SERVO_IDS,
+        'pos_start': POS_START,
+        'pos_end': POS_END,
+        'speed': SPEED,
+        'acc': ACC,
+        'time_interval1': TIME_INTERVAL1,
+        'time_interval2': TIME_INTERVAL2
+    }
+
+    controller = ServoController(custom_config)
+
+    try:
+        # 初始化串口
+        controller.setup_port()
+        # 使能所有舵机
+        controller.enable_all_servos()
+        # 复位位置
+        controller.write_position(controller.config['pos_end'],
+                            controller.config['speed'],
+                            controller.config['acc'])
+
+        """运行舵机运动"""
+        logging.info("舵机开始循环运动（按Ctrl+C终止）...")
+
+        # 运动到起始位置（180度）
+        controller.write_position(controller.config['pos_start'],
+                            controller.config['speed'],
+                            controller.config['acc'])
+        logging.info("运动到180度")
+        time.sleep(controller.time_interval1)
+
+        # 运动到结束位置（0度）
+        controller.write_position(controller.config['pos_end'],
+                            controller.config['speed'],
+                            controller.config['acc'])
+        logging.info("运动到0度")
+        time.sleep(controller.time_interval2)
+
+    except KeyboardInterrupt:
+        logging.info("\n用户终止程序...")
+        controller.stop()
+    except RuntimeError as e:
+        logging.info(f"\n运行错误：{str(e)}")
+    except Exception as e:
+        logging.info(f"\n程序运行异常：{str(e)}")
+    finally:
+        controller.cleanup()
+
+
+# -----------测试接口------------
+if __name__ == '__main__':
+    servo_rotate()
+
+