59 lines
1.6 KiB
Python
59 lines
1.6 KiB
Python
#!/usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
'''
|
|
# @Time : 2025/2/18 10:06
|
|
# @Author : hjw
|
|
# @File : controlAlgor.py
|
|
'''
|
|
|
|
from abc import ABC, abstractmethod
|
|
|
|
|
|
class ControlAlgorithm(ABC):
|
|
@abstractmethod
|
|
def calculate_speed(self, current, target):
|
|
pass
|
|
|
|
|
|
class PIDAlgorithm(ControlAlgorithm):
|
|
def __init__(self, kp=0.5, ki=0.01, kd=0.1):
|
|
self.kp = kp
|
|
self.ki = ki
|
|
self.kd = kd
|
|
self.integral = 0
|
|
self.last_error = 0
|
|
self.min_speed = 1 # 最小运行速度
|
|
self.max_speed = 100 # 最大运行速度
|
|
|
|
def calculate_speed(self, current, target):
|
|
error = target - current
|
|
self.integral += error
|
|
derivative = error - self.last_error
|
|
self.last_error = error
|
|
|
|
# 抗积分饱和
|
|
if self.integral > 1000:
|
|
self.integral = 1000
|
|
elif self.integral < -1000:
|
|
self.integral = -1000
|
|
|
|
output = self.kp * error + self.ki * self.integral + self.kd * derivative
|
|
#误差5g 5*0.5 + 1000* 0.01 + [0.5*5] = 2.5 + 10 + ... = 12.5
|
|
#误差500g 500*0.5 + 1000* 0.01 + [0.5*10] = 250 + 10 + ... = 260
|
|
return max(self.min_speed, min(self.max_speed, output)), error #
|
|
|
|
|
|
class FuzzyLogicAlgorithm(ControlAlgorithm):
|
|
def calculate_speed(self, current, target):
|
|
error = target - current
|
|
abs_error = abs(error)
|
|
|
|
if abs_error > 100:
|
|
return 100
|
|
elif abs_error > 50:
|
|
return 70
|
|
elif abs_error > 20:
|
|
return 50
|
|
else:
|
|
return 30
|