步进电机 加减速 平滑启动 加速度

/********************************** 功能说明 *************************************
* 机械臂运行任务
*********************************************************************************/
/*
脉冲细分3200 总脉冲不能超过64000； 行程800mm
脉冲细分3200 总脉冲不能超过64000； 行程800mm
脉冲细分3200 总脉冲不能超过64000； 行程800mm
*/
const uint32_t FullCoordinate[pumpMax] = { 55000u, 55000u,55000u };
uint32_t CoordinateSet[pumpMax]; // 目标位置
uint32_t CoordinatePosition[pumpMax]; // 实时更新实时位置
uint32_t Position[pumpMax]; // 本次运行起始位置
_Bool TaskArmisRunning[pumpMax] = {0}; // 防止重入标志

uint32_t CoordinateSetOld[pumpMax]; // 上次所在位置
int32_t Differ[pumpMax]; // 本次所需运行脉冲数
uint32_t period[pumpMax]  = { 2000u, 2000u, 2000u }; // PWM实时周期
float  Velocity[pumpMax] = { 20.0f, 20.0f, 20.0f }; // 实时速度
uint32_t Shifting[pumpMax] = { 3000u, 3000u, 3000u }; // 加速过程脉冲数
float Speeda[pumpMax] ={ 5000.0f, 10000.0f, 10000.0f }; // 加速度 p/(s*s)
float VelocityH[pumpMax]={ 5000.0f, 5000.0f, 5000.0f }; // 最高速度p/s
float VelocityL[pumpMax]={ 50.0f, 50.0f, 50.0f }; // 最低速度p/s
float ShiftPluse[pumpMax]={ 500.0f, 500.0f, 500.0f }; // 标准最大加速过程脉冲数
float VelocityMax[pumpMax] = { 20.0f, 20.0f, 20.0f }; // 最大速度
_Bool Direction[pumpMax] = { true,true,true}; // 运行方向
/******************************** 功能说明 *************************************
* 速度加速度转换
*******************************************************************************/
void Speed_AcceleratedConver( enum enumPumpSelect Select, uint16_t Speed )

{
static uint16_t SpeedOld[pumpMax];
if( Speed != SpeedOld[Select] )
{
if( Speed <= 500 )
{
ShiftPluse[Select] = Speed;
}
else
{
ShiftPluse[Select] = 500;
}
Speeda[Select] = (Speed * Speed) / (2u * ShiftPluse[Select]);
SpeedOld[Select] = Speed;
}
}
#include "math.h"
static bool ArmInit_XIsRuning,ArmInit_YIsRuning,ArmInit_ZIsRuning = false;
void ArmSoftCtrl( enum enumPumpSelect Select )
{
/*Vmin = 50p/s Vmax = 20000p/s  At = 0.2s  a = 100000p/s2  △s = 2000p △t = 0.001s P = 0.000001s */

#define LowSpeed 40.0f
for(;;)
{
if( (CoordinateSet[Select] != CoordinateSetOld[Select]) && !TaskArmisRunning[Select] &&!(ArmInit_XIsRuning||ArmInit_YIsRuning/*||ArmInit_ZIsRuning*/) )
{

TaskArmisRunning[Select] = true; // 置位 防重入标志
VelocityMax[Select] = 0;