251124

RP2350ZERO/utils.py

@@ -162,21 +162,22 @@ class WS2812:
 
 
 class Servo:
-    """舵机（基类）"""
+    """舵机基类"""
 
 
-    class Easing:
+    class EasingObj:
-        """缓动曲线对象"""
+        """缓动曲线对象，用于封装缓动曲线函数、转动单位角度对应的步数和平均速率"""
-        def __init__(self, function, density):
+        def __init__(self, function, steps_per_degree):
             """
-            :param function: 缓动曲线函数，X轴为进度（0至1），Y轴为单位速率
+            :param function: 缓动曲线函数，X轴为进程、Y轴为速率
-            :param density: 步数密度
+            :param steps_per_degree: 转动单位角度对应的步数，单位为步
             """
             self.function = function
-            self.density = density
+            self.steps_per_degree = steps_per_degree
-            self.speed = self._calculate_speed()
+            # 平均速率
+            self.average_speed = self._calculate_average_speed()
 
-        def _calculate_speed(self, intervals=100):
+        def _calculate_average_speed(self, intervals=100):
             """
             基于复核中点矩形法计算平均速率
             :param intervals: 积分间隔数
@@ -185,13 +186,14 @@ class Servo:
 
 
     # 缓动曲线
-    _EASINGS = {
+    EASINGS = {
-        "linear": Easing(lambda x: (x, 1), 1.0),
+        "uniform": EasingObj(lambda x: (x, 1), 1.0),  # 匀速
-        "ease_in": Easing(lambda x: (x ** 2, 2 * x), 1.5),
+        "ease_in": EasingObj(lambda x: (x ** 2, 2 * x), 1.5),  # 慢→快
-        "ease_out": Easing(lambda x: (1 - (1 - x) ** 2, 2 * (1 - x)), 1.5),
+        "ease_out": EasingObj(lambda x: (1 - (1 - x) ** 2, 2 * (1 - x)), 1.5),  # 快→慢
+        "ease_in_out": EasingObj(lambda x: (3 * x ** 2 - 2 * x ** 3, 6 * x * (1 - x)), 1.65), # 慢→快→慢
     }
 
-    def __init__(self, pin, frequency=50, min_angle=0, max_angle=180, min_pulsewidth=500, max_pulsewidth=2500, dead_zone=8, duration_per_angle=0.13):
+    def __init__(self, pin, frequency=50, min_angle=0, max_angle=180, min_pulsewidth=500, max_pulsewidth=2500, dead_zone=8, duration_per_degree=0.13):
         """
         :param pin: 引脚编号
         :param frequency: 频率，单位为赫兹
@@ -200,7 +202,7 @@ class Servo:
         :param min_pulsewidth: 最小转动角度对应的脉宽，单位为微秒
         :param max_pulsewidth: 最大转动角度对应的脉宽，单位为微秒
         :param dead_zone: 死区，单位为微秒
-        :param duration_per_angle: 转动单位角度对应的时长，单位为秒/六十度
+        :param duration_per_degree: 转动单位角度对应的时长，单位为秒/六十度
         """
 
         import utime
@@ -211,26 +213,38 @@ class Servo:
             self.pwm = PWM(Pin(pin, Pin.OUT))
             # 设置频率
             self.pwm.freq(frequency)
+            self.pwm.duty_ns(0)  # 关闭输出
         except Exception as exception:
             raise RuntimeError(f"初始化PWM发生异常，{str(exception)}") from exception
 
         self.min_angle, self.max_angle = min_angle, max_angle
-        
         # 脉宽单位由微秒转为纳秒（默认脉宽单位为纳秒）
         self.min_pulsewidth, self.max_pulsewidth = min_pulsewidth * 1000, max_pulsewidth * 1000
+        # 转动单位角度对应的脉宽，单位为纳秒/度
+        self.pulsewidth_per_degree = (self.max_pulsewidth - self.min_pulsewidth) / (self.max_angle - self.min_angle)
+        
         # 死区单位由微秒转为纳秒
         self.dead_zone = dead_zone * 1000
-        # 转动单位角度对应的脉宽，单位为纳秒/度
+
-        self.pulsewidth_per_angle = (self.max_pulsewidth - self.min_pulsewidth) / (self.max_angle - self.min_angle)
+        # 转动单位角度对应的时长单位由秒/六十度转为毫秒/度（默认转动、延迟单位为毫秒）
-        # 转动单位角度对应的时长单位由秒/六十度转为纳秒/度
+        self.duration_per_degree = duration_per_degree * 1000 / 60
-        self.duration_per_angle = duration_per_angle * 1000 * 1000 * 1000 / 60
+        
-        # 是否旋转中
+        # 初始化标记为停止转动
         self.rotating = False
-
+        # 初始化目标转动角度
-        # 当前角度
+        self.target_angle = self.min_angle
+        # 初始化目标转动角度对应的脉宽
+        self.target_pulsewidth = self._angle_to_pulsewidth(self.target_angle)
+        # 初始化当前转动角度
         self.current_angle = self.min_angle
-
+        # 初始化当前转动角度对应的脉宽
-    def set_angle(self, target_angle, easing="line"):
+        self.current_pulsewidth = self._angle_to_pulsewidth(self.current_angle)
+        # 初始化缓动曲线
+        self.easing = None
+        # 初始化转动步骤生成器
+        self.rotation_steps = None
+  
+    def set_angle(self, target_angle, easing="ease_in_out"):
         """
         设置旋转角度
         :param target_angle: 目标转动角度，单位为度
@@ -245,24 +259,99 @@ class Servo:
         if not isinstance(easing, str):
             raise TypeError("easing数据类型应为字符")
 
-        # 缓动函数
+        # 限制缓动曲线在指定范围之内
-        self.easing = self._EASINGS.get(easing.low(), self._EASINGS["linear"])
+        easing = self.EASINGS.get(easing.lower(), self.EASINGS["ease_in_out"])
 
         # 若为旋转中则返回设置旋转角度失败
         if self.rotating:
             return False
 
-        # 若目标转动角度对应的脉宽和当前转动角度对应的脉宽差小于死区则返回设置旋转角度成功
+        # 目标转动角度对应的脉宽
-        if abs(self._angle_to_pulsewidth(target_angle) - self._angle_to_pulsewidth(self.current_angle)) < self.dead_zone:
+        target_pulsewidth = self._angle_to_pulsewidth(target_angle)
+
+        # 若目标转动角度对应的脉宽和当前转动角度对应的脉宽之差小于死区则返回设置旋转角度成功
+        if abs(target_pulsewidth - self.current_pulsewidth) < self.dead_zone:
+            # 更新目标转动角度
+            self.target_angle = target_angle
+            # 更新目标转动角度对应的脉宽
+            self.target_pulsewidth = target_pulsewidth
             return True
 
-        steps, duration = self._calculate_steps(abs(target_angle - self.current_angle))
+        # 标记为旋转中
+        self.rotating = True
+        # 更新目标转动角度
+        self.target_angle = target_angle
+        # 更新目标转动角度对应的脉宽
+        self.target_pulsewidth = target_pulsewidth
+        # 更新缓动曲线
+        self.easing = easing
+        # 初始化转动步骤生成器
+        self.rotation_steps = self._generate_rotation_steps()
+        return True
+
+    def _generate_rotation_steps(self):
+        """生成转动步骤"""
+        # 起始转动角度
+        start_angle = self.current_angle
+        # 目标转动角度和起始转动角度之差
+        angle_difference = self.target_angle - start_angle
+
+        # 转动步数，单位为步
+        steps = max(int(round(abs(angle_difference) * self.easing.steps_per_degree)), max(10, int(round(abs(angle_difference) / 2))))
+        # 每步延迟，单位为毫秒
+        step_delay = max(5, min(50, int(round((abs(angle_difference) * self.duration_per_degree / self.easing.average_speed) / steps))))
+
+        for step in range(steps + 1):
+            # 当前转动角度
+            self.current_angle = start_angle + angle_difference * self.easing.function(step / steps)[0]
+            # 当前转动角度对应的脉宽
+            self.current_pulsewidth = self._angle_to_pulsewidth(self.current_angle)
+
+            # 输出PWM信号，更新当前转动角度
+            self.pwm.duty_ns(self.current_pulsewidth)
+
+            if step < steps:  # 最后一步无需等待
+                yield
+                utime.sleep_ms(step_delay)
+
+        # 兜底：确保最后一步达到目标转动角度对应的脉宽
+        self.current_angle = self.target_angle
+        self.current_pulsewidth = self.target_pulsewidth
+        self.pwm.duty_ns(self.current_pulsewidth)
+
+        self._stop()
+        yield
 
     def _angle_to_pulsewidth(self, angle):
         """将转动角度转为脉宽"""
-        pulsewidth = self.min_pulsewidth + (angle - self.min_angle) * self.pulsewidth_per_angle
+        pulsewidth = self.min_pulsewidth + (angle - self.min_angle) * self.pulsewidth_per_degree
+        return max(self.min_pulsewidth, min(self.max_pulsewidth, int(round(pulsewidth))))
 
-        return int(round(pulsewidth))
+    def rotate(self):
+        """转动：需在主循环调用，执行转动步骤"""
+        if self.rotating and self.rotation_steps:
+            try:
+                next(self.rotation_steps)
+            # 生成器耗尽，正常停止
+            except StopIteration:
+                self._stop()
+            except Exception as exception:
+                self._stop()
+                raise RuntimeError(f"转动时发生异常，{str(exception)}") from exception
 
-    def _calculate_steps(self, angle_difference):
+    def _stop(self):
-        """计算转动步数"""
+        """停止转动"""
+        # 标记为停止转动
+        self.rotating = False
+        self.rotation_steps = None
+        self.pwm.duty_ns(0)
+
+
+class SG90(Servo):
+    """SG90（9G舵机）"""
+    def __init__(self, pin):
+        super().__init__(
+            pin=pin,
+        )
+
+