S加减速，脉冲S加减速

s型加减速在网格图上速度的变化现是缓慢增大然后快速增大再是缓慢增大再到匀速。也就是说加速度是变化的。f(x) 1/(1e^-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/(1e^(-x 3))这样当 x 0时y ≈ 0这时0 ≤ x ≤ 6。但是加速次数也不可能为小数而且6次加速太少了所以要横向拉长函数。改变x的系数就可以拉长或缩短y ≈ 0时x的范围。这个函数的x的系数越小函数看起来横向越长实际无限长x能取得的整数越多。当系数为0.1时1/(1e^(-0.1x 3))x的有效值就是0到60当系数为2时1/(1e^(-2x 3))x的有效值就是0到3。也就是说x取值的整数范围是系数的倒数即1/0.1 x 6个和1/2 x 6个。如果决定加速次数那么系数就是6/加速次数当加速次数为100次时也就是1/(1e^(-(6/100)x 3))。假如最大脉冲频率为1MHzy乘以最大频率就可以知道当前实际频率了即1000000 x 0.95257。如果将1/(1e^-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需要的步数 beltOrGearfalse同步带。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