转一个步进电机加减速程序
-
转一个步进电机加减速程序
源码转载自bilibili UP主 专业瞎搞:
https://www.bilibili.com/video/BV1op4y1C7vR他原版本是C的,改成了python
我只是搬运工from pyb import udelay import random import math from pyb import Pin import time import utime class Stepper: def __init__(self, step_pin, dir_pin, step_time=200): # 初始化 self.DirPin = dir_pin self.PulPin = step_pin self.DirPin.init(Pin.OUT_PP) self.PulPin.init(Pin.OUT_PP) self.CurrentStepTime = step_time self.TargetStepTime = 500 self.LastStepSt = 0 self.CurrentStep = 0 self.TargetStep = 0 self.StartStep = 0 self.Acceleration = 600 # 加速度(步 / 秒~2) self.SpeedUpStep = 0 # 加速段的步数 self.SpeedDownStep = 0 # 减速段的步数 self.IsDebug = False self._speedChangeMinimumDis = 0 # 速度变化时重新计算的最小的加减速步数 def SetAcceleration(self, acceleration): self.Acceleration = acceleration self.calCulateSpeedChangeMinimunDis() def IsRunToTarget(self): return self.TargetStep == self.CurrentStep def CanRun(self): return (utime.ticks_us() - self.LastStepSt) >= self.CurrentStepTime def RunTo(self, target, stepTime): if (self.TargetStep != target): self.TargetStepTime = stepTime self.TargetStep = target self.CurrentStepTime = 0 self.StartStep = self.CurrentStep self.calCulateSpeedChangeMinimunDis() souldMoveSteps = self.TargetStep - self.CurrentStep # 提前规划好加减速的步数以及中间匀速运动的步数 if (self._speedChangeMinimumDis * 2 > abs(souldMoveSteps)): #如果加减速时间过长没有匀速过程,则将应走步数对半分 self.SpeedUpStep = souldMoveSteps / 2 + self.CurrentStep #加速到的位置为当前位置加上增量的一半 self.SpeedDownStep = self.SpeedUpStep else: if (self.CurrentStep < self.TargetStep): # 正向 self.SpeedUpStep = self._speedChangeMinimumDis + self.CurrentStep self.SpeedDownStep = self.TargetStep - self._speedChangeMinimumDis else: # 反向 self.SpeedUpStep = self.CurrentStep - self._speedChangeMinimumDis self.SpeedDownStep = self.TargetStep + self._speedChangeMinimumDis print(self.TargetStep, self._speedChangeMinimumDis,self.SpeedUpStep,self.SpeedDownStep) if (self.IsRunToTarget() == False): if (self.CanRun()): self.runStep(self.CurrentStep < target) return self.IsRunToTarget() # 从0加速到指定目标速度所用的最小步数 def calCulateSpeedChangeMinimunDis(self): if self.Acceleration == 0: self._speedChangeMinimumDis = 0 return v = 1000000.0 / self.TargetStepTime # 步 / 秒 self._speedChangeMinimumDis = pow(v, 2) / 2 / self.Acceleration # 步进电机运行一步 def runStep(self, dir): self.LastStepSt = utime.ticks_us() self.DirPin.value(dir) self.PulPin.value(1) #udelay(2) self.PulPin.value(0) self.CurrentStep += (1 if dir else -1) # 记录每次只运行一步 # 若加速度为0,则没有加减速部分 if (self.Acceleration == 0): self.CurrentStepTime = self.TargetStepTime return #若加速度不为0,通过下一脉冲的终点速度计算下一个脉冲的时间间隔,重置步进间隔 #v = 0.0 if (dir): if (self.CurrentStep < self.SpeedUpStep or self.CurrentStep > self.SpeedDownStep): if (self.CurrentStep < self.SpeedUpStep): v = math.sqrt(float(self.Acceleration * 2.0 * (self.CurrentStep - self.StartStep))) # 步 / 秒 else: v = 1000000.0 / self.TargetStepTime # 步 / 秒 a = float((-self.Acceleration) * 2.0 * (self.CurrentStep - self.SpeedDownStep) + pow(v, 2)) if a > 0: v = math.sqrt(a) # 步 / 秒 self.CurrentStepTime = 1000000.0 / v self.CurrentStepTime = self.TargetStepTime if self.CurrentStepTime <= self.TargetStepTime else self.CurrentStepTime else: self.CurrentStepTime = self.TargetStepTime else: if (self.CurrentStep > self.SpeedUpStep or self.CurrentStep < self.SpeedDownStep): if (self.CurrentStep > self.SpeedUpStep): v = math.sqrt(self.Acceleration * 2 * (self.StartStep - self.CurrentStep)) # 步 / 秒 else: v = 1000000.0 / self.TargetStepTime # 步 / 秒 a = (-self.Acceleration) * 2 * abs(self.CurrentStep - self.SpeedDownStep) + pow(v, 2) if a > 0: v = math.sqrt(a) # 步 / 秒 self.CurrentStepTime = 1000000.0 / v self.CurrentStepTime = self.TargetStepTime if self.CurrentStepTime <= self.TargetStepTime else self.CurrentStepTime else: self.CurrentStepTime = self.TargetStepTime step_pin = Pin('P1') dir_pin = Pin('P0') my_stepper = Stepper(step_pin,dir_pin) my_stepper.SetAcceleration(8000) da = 50000 while(True): a = my_stepper.RunTo(da, 80) if a: da = -da