s型加减速：
在网格图上，速度的变化现是缓慢增大，然后快速增大，再是缓慢增大，再到匀速。也就是说加速度是变化的。
f(x) = 1/(1+e^-x)，这是y从左到右增加时的S曲线的原始函数，用这个生成一个表，加速正着用，减速倒着用，e是自然常数，约为2.71828。当-3 ≤ x ≤ 3，函数收敛较为明显，此时，y轴高度基本处于0到1之间，更精确点，当x = -3时， y ≈ 0.04742，当x = 3时，y ≈ 0.95257。如果精度不够，就加大x的取值范围。现在假设3 ≤ x ≤ 3时，y从0到1。
实际使用时，x作为加速次数，y就是速度。加速次数不可能为负数，所以需要对函数进行向右平移，也就是1/(1+e^(-x + 3))，这样当 x = 0时，y ≈ 0，这时0 ≤ x ≤ 6。但是加速次数也不可能为小数，而且6次加速太少了，所以要横向拉长函数。改变x的系数就可以拉长或缩短y ≈ 0时x的范围。这个函数的x的系数越小，函数看起来横向越长（实际无限长），x能取得的整数越多。
当系数为0.1时，1/(1+e^(-0.1x + 3))，x的有效值就是0到60，当系数为2时，1/(1+e^(-2x + 3))，x的有效值就是0到3。也就是说x取值的整数范围是系数的倒数，即1/0.1 x 6个，和1/2 x 6个。如果决定加速次数，那么系数就是6/加速次数，当加速次数为100次时，也就是1/(1+e^(-(6/100)x + 3))。
假如最大脉冲频率为1MHz，y乘以最大频率就可以知道当前实际频率了，即1000000 x 0.95257。如果将1/(1+e^-x)计算后的速度比例存起来，下次使用时只需查表，再乘以最大频率即可。
实现：
buff_pa是速度表数组。count_va是加速次数。pan_right_va是右移量，代表了精度，即右移多少后，y强制为0，一般用7，最后一次速度在99.9%。count_va越大，S曲线越平缓，加速过程越平滑，加速时间越长；count_va越小，S曲线越陡峭，加速过程越急剧，加速时间越短。
void curve_s_init(float *buff_pa, uint16_t count_va, uint8_t pan_right_va) { for (uint16_t x = 0; x < count_va; x++) buff_pa[x] = 1.0 / (1.0 + exp(((float)pan_right_va * 2 / count_va * -x) + pan_right_va)); //buff_pa[x] = 1.0 / (1.0 + pow(sqrt(m_e_v), (float)pan_right_va * 2 / count_va * -x + pan_right_va)); //这是通用计算方法，pow函数的第一个参数越接近1，该算式计算出的曲线越平缓，可以改的和线性加减速无异 } 例： float curve_s_table_v[500]; curve_s_init(curve_s_table_v, 500, 7);以下为c++版本
#ifndef ivesStepMotorH #define ivesStepMotorH #include "main.h" namespace xiaguangbo { namespace motorNamespace { namespace ivesStepMotorNamespace { class ivesStepMotorClass { private: static constexpr float pi = 3.1415926; //运行状态 static const uint8_t motorStopState = 0; static const uint8_t motorAccelerateState = 1; static const uint8_t motorMaxState = 2; static const uint8_t motorDecelerateState = 3; //固定参数 static const uint8_t sCurveExcursion = 3; //s曲线偏移的量 static const uint16_t sCurvePointNumber = 200; //在s曲线上取的点的数量 static const uint16_t aCircleStep = 3200; //一圈的步数 static const uint16_t maxSpeedThreshold = 0xffff; //速度的最大值，因为速度变量是16位的，所以最大就是0xffff //s曲线 static float sCurveTable[sCurvePointNumber]; //放s曲线数值的数组 static bool sCurveTableFlag; //s曲线是否已经计算过了 //算法核心参数 int32_t currentAbsolutePosition; //当前的绝对位置，以步数为单位 int32_t currentNeedRunStep; //将要运行的步数。负数就往复位方向移动 uint32_t temporaryStepCounter; //临时的步数计数，记录一次算法走了多少步 uint16_t currentSpeed; //当前速度 uint16_t targetSpeed; //要达到的速度 uint8_t runState; //运行状态 uint16_t accelerationInterval; //加减速一次的间隔 uint16_t interruptCounter; //加减速时的定时器中断次数计数 bool interruptCounterFlag; //中断计数的开关 uint32_t accelerationNumberCounter; //加减速次数计数 uint32_t accelerationStep; //加减速阶段的脉冲数 uint32_t speedAccumulator; //速度累加器 bool overflowFlag; //溢出标志 //定制化功能的参数 bool resetTimeDirPinLv; //复位时方向的io电平 uint16_t oneMmStep; //电机走1mm所需的步数 GPIO_TypeDef *stepGpioHandle, *dirGpioHandle, *enGpioHandle, *resetSensorGpioHandle; //IO组，脉冲、方向、复位限位 uint16_t stepGpioNumber, dirGpioNumber, enGpioNumber, resetSensorGpioNumber; //IO号，脉冲、方向、复位限位 bool resetSensorTriggerLv; //复位限位开关被触发后的电平 bool resetFlag; //是否处于复位 bool stopFlag; //是否被停止了 bool keepMoveFlag; //是否要一直转 bool enTimeEnPinSate; /* 初始化s曲线参数表 */ static void sCurveTableInit(); /* 电机每走一步都会执行的参数。主要用于检测复位开关 */ void runTimeImplement(); void setBeltOneMmStep(uint8_t pitch, uint8_t motorGearTeeth) { oneMmStep = aCircleStep / (pitch * motorGearTeeth); } void setGearOneMmStep(uint8_t pitch, uint8_t motorGearTeeth) { setBeltOneMmStep(pitch * pi, motorGearTeeth); } void SetEnPinState(bool state) { HAL_GPIO_WritePin(enGpioHandle, enGpioNumber, GPIO_PinState(state)); } public: ivesStepMotorClass(); ~ivesStepMotorClass(); /* 初始化 */ void init(GPIO_TypeDef *stepGpioHandle, uint16_t stepGpioNumber, GPIO_TypeDef *dirGpioHandle, uint16_t dirGpioNumber, GPIO_TypeDef *enGpioHandle, uint16_t enGpioNumber, GPIO_TypeDef *resetSensorGpioHandle, uint16_t resetSensorGpioNumber, bool resetSensorTriggerLv, bool resetTimeDirPinLv, bool enTimeEnPinSate); /* 移动相应的步数 need_run_step：步数。负值是向复位方向移动 acc_dec_interval：加速度 */ void move(int32_t currentNeedRunStep, uint16_t targetSpeed, uint16_t accelerationInterval); /* 移动到绝对位置。单位为mm */ void moveToAbsolutePosition(uint16_t absolutePosition, uint16_t targetSpeed, uint16_t accelerationInterval); //在没有使用set_mm_step设置mm_step之前，不要用 /* 移动到相对位置 */ void moveToRelativePosition(int16_t relativePosition, uint16_t targetSpeed, uint16_t accelerationInterval); /* 算法的循环。放在一个定时器里进行定时执行，调用的速度就是电机的最大速度 */ void loop(); /* 根据参数计算并设置每走1mm需要的步数 beltOrGear：false：同步带。true：齿条 pitch：同步带的型号或齿条的模数 gear_teeth：电机上的齿轮的齿数 */ void setOneMmStep(bool beltOrGear, uint8_t pitch, uint8_t motorGearTeeth) { if (beltOrGear == 0) setBeltOneMmStep(pitch, motorGearTeeth); else setGearOneMmStep(pitch, motorGearTeeth); } /* 复位 */ void reset(uint16_t targetSpeed = 5000, uint16_t accelerationInterval = 100); /* 停止 */ void stop(); /* 一直转 */ void keepMove(bool dir, uint16_t targetSpeed, uint16_t accelerationInterval = 100); bool getStopFlag() { return stopFlag; } void resetStopFlag() { stopFlag = false; } int32_t getCurrentAbsolutePosition() { return currentAbsolutePosition; } }; } // namespace ivesStepMotorNamespace } // namespace motorNamespace } // namespace xiaguangbo #endif#include "ivesStepMotor.hpp" #include <math.h> namespace xiaguangbo { namespace motorNamespace { namespace ivesStepMotorNamespace { float ivesStepMotorClass::sCurveTable[sCurvePointNumber]; bool ivesStepMotorClass::sCurveTableFlag; ivesStepMotorClass::ivesStepMotorClass() { } ivesStepMotorClass::~ivesStepMotorClass() { } void ivesStepMotorClass::sCurveTableInit() { for (uint16_t x = 0; x < sCurvePointNumber; x++) sCurveTable[x] = 1.0 / (1.0 + exp((sCurveExcursion * 2.0 / sCurvePointNumber * -x) + sCurveExcursion)); sCurveTableFlag = true; } void ivesStepMotorClass::init(GPIO_TypeDef *stepGpioHandle, uint16_t stepGpioNumber, GPIO_TypeDef *dirGpioHandle, uint16_t dirGpioNumber, GPIO_TypeDef *enGpioHandle, uint16_t enGpioNumber, GPIO_TypeDef *resetSensorGpioHandle, uint16_t resetSensorGpioNumber, bool resetSensorTriggerLv, bool resetTimeDirPinLv, bool enTimeEnPinSate) { if (sCurveTableFlag == false) { sCurveTableInit(); sCurveTableFlag = true; } this->stepGpioHandle = stepGpioHandle; this->stepGpioNumber = stepGpioNumber; this->dirGpioHandle = dirGpioHandle; this->dirGpioNumber = dirGpioNumber; this->enGpioHandle = enGpioHandle; this->enGpioNumber = enGpioNumber; this->resetSensorGpioHandle = resetSensorGpioHandle; this->resetSensorGpioNumber = resetSensorGpioNumber; this->resetSensorTriggerLv = resetSensorTriggerLv; this->resetTimeDirPinLv = resetTimeDirPinLv; stop(); SetEnPinState(enTimeEnPinSate); } void ivesStepMotorClass::move(int32_t currentNeedRunStep, uint16_t targetSpeed, uint16_t accelerationInterval) { if (currentNeedRunStep == 0) { stop(); return; } this->currentNeedRunStep = currentNeedRunStep; temporaryStepCounter = 0; currentSpeed = 0; this->targetSpeed = targetSpeed > maxSpeedThreshold - 100 ? maxSpeedThreshold : targetSpeed + 100; //防止速度为0 interruptCounterFlag = true; interruptCounter = 0; accelerationNumberCounter = 0; accelerationStep = 0; this->accelerationInterval = accelerationInterval; speedAccumulator = 0; if (currentNeedRunStep > 0) HAL_GPIO_WritePin(dirGpioHandle, dirGpioNumber, (GPIO_PinState)(!resetTimeDirPinLv)); else HAL_GPIO_WritePin(dirGpioHandle, dirGpioNumber, (GPIO_PinState)resetTimeDirPinLv); runState = motorAccelerateState; } void ivesStepMotorClass::moveToAbsolutePosition(uint16_t absolutePosition, uint16_t targetSpeed, uint16_t accelerationInterval) { move(absolutePosition * oneMmStep - this->currentAbsolutePosition, targetSpeed, accelerationInterval); } void ivesStepMotorClass::moveToRelativePosition(int16_t relativePosition, uint16_t targetSpeed, uint16_t accelerationInterval) { move(relativePosition * oneMmStep, targetSpeed, accelerationInterval); } void ivesStepMotorClass::runTimeImplement() { if (resetSensorGpioHandle == nullptr) return; if (HAL_GPIO_ReadPin(resetSensorGpioHandle, resetSensorGpioNumber) == resetSensorTriggerLv) //检查复位开关 { if (resetFlag == true) { stop(); currentAbsolutePosition = 0; //绝对位置置0 } else if (currentNeedRunStep < 0) //不能继续向复位的方向走，但可以向复位的反方向走 stop(); } } void ivesStepMotorClass::loop() { if (runState == motorStopState) return; if (overflowFlag) //如果产生了高电平 HAL_GPIO_WritePin(stepGpioHandle, stepGpioNumber, GPIO_PIN_RESET); //拉低脉冲信号 overflowFlag = false; speedAccumulator += currentSpeed; //叠加速度 if (speedAccumulator >= maxSpeedThreshold) //阈值，如果当前速度达到阈值，发送脉冲的速度就达到最快了，这个阈值就是算法的最大速度 { overflowFlag = true; speedAccumulator -= maxSpeedThreshold; } if (overflowFlag) //如果溢出 { temporaryStepCounter++; HAL_GPIO_WritePin(stepGpioHandle, stepGpioNumber, GPIO_PIN_SET); //拉高脉冲信号产生 if (currentNeedRunStep > 0) currentAbsolutePosition < 0x7fffffff ? currentAbsolutePosition++ : currentAbsolutePosition; else currentAbsolutePosition > (int32_t)0x80000000 ? currentAbsolutePosition-- : currentAbsolutePosition; runTimeImplement(); //额外的操作，比如检测复位开关 } //根据电机的状态进行工作 switch (runState) { case motorAccelerateState: //记录加速的步数 if (overflowFlag) accelerationStep++; if (interruptCounterFlag) { interruptCounter++; if (interruptCounter >= accelerationInterval) { interruptCounter = 0; accelerationNumberCounter++; //计录加速的次数 currentSpeed = sCurveTable[accelerationNumberCounter] * targetSpeed; //计算当前速度 //如果加速次数达到最高次数，就停止加速，并让速度等于最大速度，再切换状态 if (accelerationNumberCounter >= sCurvePointNumber) { interruptCounterFlag = false; currentSpeed = targetSpeed; runState = motorMaxState; } } } //如果总步数大于1步 if ((uint32_t)fabs(currentNeedRunStep) > 1) { if (temporaryStepCounter >= (uint32_t)fabs(currentNeedRunStep) / 2) //如果已走步数大于最大步数的一半 { //加速时中断计数被断，所以要调整，最后一次加速多长时间，减速的第一次的速度就维持多长时间 interruptCounter = accelerationInterval - interruptCounter; runState = motorDecelerateState; } } else if (temporaryStepCounter > 0) //只有1步就至少走1步 runState = motorDecelerateState; break; case motorMaxState: if (keepMoveFlag && overflowFlag == true) //当处于保持转动的状态时 temporaryStepCounter--; //加一次就减一次，防止步数达到进入减速的要求 if ((uint32_t)fabs(currentNeedRunStep) - temporaryStepCounter <= accelerationStep) { interruptCounterFlag = true; //进入减速状态就要切换为减速状态时的最大速度 accelerationNumberCounter--; currentSpeed = sCurveTable[accelerationNumberCounter] * targetSpeed; //计算当前速度 runState = motorDecelerateState; } break; case motorDecelerateState: if (interruptCounterFlag) { interruptCounter++; if (interruptCounter >= accelerationInterval) //如果中断次数达到设定次数 { interruptCounter = 0; accelerationNumberCounter--; currentSpeed = sCurveTable[accelerationNumberCounter] * targetSpeed; //计算当前速度 if (accelerationNumberCounter <= 1) interruptCounterFlag = false; } } if (temporaryStepCounter >= (uint32_t)fabs(currentNeedRunStep)) stop(); break; default: break; } } void ivesStepMotorClass::reset(uint16_t targetSpeed, uint16_t accelerationInterval) { resetFlag = true; keepMove(false, targetSpeed, accelerationInterval); //向复位方向一直走 } void ivesStepMotorClass::stop() { runState = motorStopState; stopFlag = true; keepMoveFlag = false; resetFlag = false; } void ivesStepMotorClass::keepMove(bool dir, uint16_t targetSpeed, uint16_t accelerationInterval) { keepMoveFlag = true; move(aCircleStep * 100 * (dir == false ? -1 : 1), targetSpeed, accelerationInterval); //步数至少让加速能加满 } } // namespace ivesStepMotorNamespace } // namespace motorNamespace } // namespace xiaguangbo
