刀片启停&转速控制算法 Python
功能说明
1. 工况联锁停刀：返航回站、坐标越界、机身倾翻、整机急停四类触发立即安全停机
2. 刀片软启动/软停机：斜坡缓升缓降PWM占空比，无冲击启停
3. 负载过载闭环保护：母线电流超限→降速→超时停机三级保护
4. PID恒转速闭环控制：目标转速+编码器实测转速闭环稳速
适配割草机直流无刷刀片驱动，可直接移植C单片机
import time
# ====================== 1.全局配置参数 ======================
# 转速PID参数
KP = 0.35
KI = 0.012
KD = 0.005
MAX_PWM = 100.0 # PWM最大输出百分比 0~100
MIN_PWM = 8.0 # 刀片最低有效驱动PWM
# 软启停配置
SOFT_RAMP_STEP = 0.8 # 每毫秒PWM变化步长
SOFT_DELAY_MS = 1
# 过载保护配置
OVERLOAD_CURR = 12.5 # 过载阈值电流A
SEVERE_OVERLOAD = 16.0 # 严重过载阈值
OVERLOAD_TIMEOUT_S = 1.2# 过载持续超时停机
# 工况安全标志位
SAFE_FLAG = {
"return_home": False, # 返航回站
"pos_outbound": False, # 坐标越界
"roll_over": False, # 机身倾翻
"emergency_stop": False, # 整机急停
}
class BladeCtrl:
def __init__(self, target_rpm:float):
self.tar_rpm = target_rpm
self.real_rpm = 0.0
self.pwm_out = 0.0
# PID缓存
self.err_last = 0.0
self.err_sum = 0.0
# 软启停标记
self.running_flag = False
self.soft_stop_flag = False
# 过载计时
self.overload_start_t = 0.0
self.curr_load = 0.0 # 实时负载电流(A)
# ------------------- PID恒转速计算 -------------------
def pid_calc(self):
err = self.tar_rpm - self.real_rpm
self.err_sum += err
diff = err - self.err_last
pid_out = KP*err + KI*self.err_sum + KD*diff
self.err_last = err
# PWM限幅
self.pwm_out += pid_out
self.pwm_out = max(MIN_PWM, min(MAX_PWM, self.pwm_out))
return self.pwm_out
# ------------------- 安全联锁判断：任意故障触发立即软停 -------------------
def safety_interlock_check(self)->bool:
"""True=需要停机"""
for v in SAFE_FLAG.values():
if v:
return True
return False
# ------------------- 过载三级保护逻辑 -------------------
def overload_protect(self):
now = time.time()
if self.curr_load >= OVERLOAD_CURR:
if self.overload_start_t == 0:
self.overload_start_t = now
dur = now - self.overload_start_t
# 一级过载：临时降速15%
if self.curr_load < SEVERE_OVERLOAD:
self.pwm_out = self.pwm_out * 0.85
# 二级严重过载：大幅降速35%
else:
self.pwm_out = self.pwm_out * 0.65
# 三级超时过载：触发软停机
if dur >= OVERLOAD_TIMEOUT_S:
self.running_flag = False
self.soft_stop_flag = True
else:
self.overload_start_t = 0 # 负载恢复清零计时
# ------------------- 软启动：PWM斜坡缓慢上升 -------------------
def soft_start(self):
if self.pwm_out < self.pid_calc():
self.pwm_out += SOFT_RAMP_STEP
time.sleep(SOFT_DELAY_MS/1000)
self.pwm_out = min(self.pid_calc(), MAX_PWM)
# ------------------- 软停止：PWM斜坡缓慢下降到0 -------------------
def soft_stop(self):
if self.pwm_out > 0:
self.pwm_out -= SOFT_RAMP_STEP
if self.pwm_out < 0:
self.pwm_out = 0
time.sleep(SOFT_DELAY_MS/1000)
else:
self.soft_stop_flag = False
# ------------------- 主控周期调度（1ms周期轮询） -------------------
def ctrl_loop(self, real_rpm_in:float, load_curr_in:float):
# 刷新实时采样
self.real_rpm = real_rpm_in
self.curr_load = load_curr_in
# 1.安全联锁优先
if self.safety_interlock_check():
self.running_flag = False
self.soft_stop_flag = True
# 2.过载保护
if self.running_flag:
self.overload_protect()
# 3.运行：软启动+PID稳速
if self.running_flag and not self.soft_stop_flag:
self.soft_start()
# 4.停机：软停止
if self.soft_stop_flag:
self.soft_stop()
return round(self.pwm_out,2)
# 外部调用：启动刀片
def blade_start(self):
self.running_flag = True
self.soft_stop_flag = False
self.err_sum = 0
self.err_last = 0
# 外部调用：手动停止刀片
def blade_stop(self):
self.running_flag = False
self.soft_stop_flag = True
# ====================== 业务调用示例 ======================
if __name__ == "__main__":
# 实例：目标转速3200RPM刀片
blade = BladeCtrl(target_rpm=3200)
# 示例1：正常启动运行
blade.blade_start()
for _ in range(30):
pwm = blade.ctrl_loop(real_rpm_in=2900, load_curr_in=7.2)
print(f"正常运行 PWM:{pwm}% 实测转速:{blade.real_rpm}")
# 示例2：模拟返航回站触发联锁停刀
SAFE_FLAG["return_home"] = True
for _ in range(30):
pwm = blade.ctrl_loop(real_rpm_in=3100, load_curr_in=7.5)
print(f"返航联锁停机 PWM:{pwm}%")
# 示例3：模拟过载工况
SAFE_FLAG["return_home"] = False
blade.blade_start()
for _ in range(50):
pwm = blade.ctrl_loop(real_rpm_in=3300, load_curr_in=13.8)
print(f"过载保护 PWM:{pwm}% 负载电流:{blade.curr_load}A")
关键逻辑拆解（对应需求）
1.工况联动停刀
SAFE_FLAG四个标志位由上层导航模块赋值：
• return_home=True：导航回站任务触发→软停刀（你之前返航停刀子逻辑）
• pos_outbound=True：GPS/坐标越界
• roll_over=True：倾角传感器>安全阈值倾翻
• emergency_stop=True：物理急停按键/整机故障
2.软起/软停
通过固定步长阶梯增减PWM，避免瞬间满占空比冲击电机、刀片抖动。
3.三级过载保护
1. 轻度过载(12.5~16A)：输出PWM降85%降载
2. 重度过载(＞16A)：输出PWM降65%强制降速
3. 过载持续＞1.2s：自动进入软停机流程
4.PID恒转速闭环
编码器回传实时转速，动态修正PWM，负载波动时自动补压稳转速。
移植C语言改造要点
1. 把time.sleep替换为单片机硬件定时器1ms中断
2. 浮点可改为定点Q格式节约MCU资源
3. SAFE_FLAG由导航/IMU/限位传感器寄存器赋值
