步进电机S型算法示例

#include <stdio.h>
#include <stdlib.h>
#include <assert.h>

#define SPEEDPRINCIPLE              3    //加减速法则，3分之一
#define MOTOMAXSPEED                1500    //电机最大转速 单位转/min
#define MOTOROUNDLENGTH             36     // 电机每转对应的传动距离，需要换算
#define MOTOMAXSTART                200    //电机最大初速度，超过该值，会损害电机
#define DRIVERSFRACTION              1     //驱动器细分数
#define MOTOFRACTION                200    //电机细分数
#define ACCELERATESPACE(V0,Vt,t)    (((V0) + (Vt)) * (t) / 2)   //根据初末速度，以及时间，计算加速路程
#define LASTVELOCITY(S,V0,t)        (2 * (S) / (t) - (V0))    //根据加速路程，初速度，以及时间计算末速度
#define STEPSPEED(RV)               ((RV) * MOTOFRACTION * DRIVERSFRACTION / 60)   //根据电机转速（转/min），计算电机的步速度（step/s）
#define TOTALSTEP(S)                S * MOTOFRACTION * DRIVERSFRACTION / MOTOROUNDLENGTH   //根据长度计算电机所需走的步数，S的单位为mm

struct SPEED
{
    int         V0;                //初速度 单位为：step/s
    int         Vt;                //末速度 单位为：step/s
    long long   S;                 //路程,单位为: step
    double      t;                 //加速时间， 单位为：s
    int         time;              //加速次数， 单位为：次
    int         *SpeedTab;          //加速速度表，速度单位：step/s
    int         SecSpeedPoint;      //减速点，单位：step（在电机运动的过程中，如果剩余路程少于该值，那么电机开始减速

};
typedef struct SPEED * Speed_t;

//各范围内的参数值，可以在此处设置参数的范围
//标准要求：初速度大于0且小于电机最高启动速度，末速度大于初速度且小于电机最高转速
#define IS_SPEED(V0,Vt)     (((V0) >= 0) && ((V0) <= STEPSPEED(MOTOMAXSTART))  && ((Vt) >= (V0)) && ((Vt) <= STEPSPEED(MOTOMAXSPEED)))                            
#define IS_SPACE(S)         ((S) > 0)
#define IS_ADDTIMING(t)     ((t) > 0)            
#define IS_ADDTIME(time)     ((time) >= 32)         //加速次数必须大于等于32，否则计算表格就没有什么意义了


static Speed_t CalculateSpeedTab(Speed_t Speed)
{
    int i;                        
    double aa;             //加加速度      
    int DeltaV;              //速度变化量    
    int tempVt;
    //如果速度记录表的指针为空，说明没有分配内存
    assert(Speed->SpeedTab != NULL);  
    //判断各参数是否达到标准
    assert(IS_SPEED(Speed->V0, Speed->Vt));
    assert(IS_SPACE(Speed->S));
    assert(IS_ADDTIMING(Speed->t));
    assert(IS_ADDTIME(Speed->time));
    //根据加速度公式进行末速度的计算
    tempVt = LASTVELOCITY(Speed->S / SPEEDPRINCIPLE, Speed->V0, Speed->t);
    Speed->Vt = (tempVt > Speed->Vt) ? Speed->Vt : tempVt;                                                   
    Speed->SecSpeedPoint = ACCELERATESPACE(Speed->V0, Speed->Vt, Speed->t);      //计算减速点位置，剩余路程为减速开始时的
    aa = (double)((Speed->Vt - Speed->V0) / 2)          //加速度变化时拐点的速度值       
        * 2                                              //根据加速度变化的三角图样，可知知道直角三角形面积时逆推三角形高度面积称2
        / (Speed->time / 2)                               // 除以底边
        / (Speed->time / 2);                              //再除以底边得到斜率，即加加速度
    // 开始进行速度的计算
    for(i = 0; i < ((Speed->time / 2) + 1); i++)
    {
        DeltaV = (aa * i * i) / 2;                     // V = V0 + a * t / 2; a = aa * t                       
        *(Speed->SpeedTab + i) = Speed->V0 + DeltaV;           //当前点速度       
        *(Speed->SpeedTab + Speed->time - i) = Speed->Vt - DeltaV;     //对称点速度
    }
    return Speed;
}

void TestSpeed(int V0, int Vt, int S, int time)
{
    int i;
    Speed_t Speed = (Speed_t)malloc(sizeof(struct SPEED));
    Speed->V0 = STEPSPEED(V0);    //起速
    Speed->Vt = STEPSPEED(Vt);    //末速
    Speed->S = TOTALSTEP(S);      //路程
    Speed->t = 0.2;               //加速时间设为0.2s
    Speed->time = time;           //加速次数
    Speed->SpeedTab = (int *)malloc(sizeof(int) * (Speed->time + 1));   //根据加速次数申请表格内存空间，+1是为防止用的时候过界
    CalculateSpeedTab(Speed);       
    for(i = 0; i < Speed->time; i++)
    {
        printf("SpeedTab[%d] = %d, Acceleration = %d\n"     //加速速度表step/s，加速度的打印
                  , i
                  , *(Speed->SpeedTab + i)
                  , *(Speed->SpeedTab + i + 1) - *(Speed->SpeedTab + i));
    }
    printf("SpeedTab[%d] = %d\n", i, *(Speed->SpeedTab + i));
    free(Speed->SpeedTab);              
    free(Speed);                    
}


int main()
{
    TestSpeed(100, 1500, 30, 64); //参数由实际情况进行确定
    return 0;

}