devbox
繁依Fanyi0
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

【嵌入式必读】【一文彻底理解】步进电机驱动算法——梯形加减速运动算法代码实现_步进电机加减速程序编写

作者：繁依Fanyi0 | 2024-07-03 10:34:42

赞

踩

步进电机加减速程序编写

文章目录

1.介绍

上篇文章我们主要讲述了梯形加速算法的主要原理，我们一步步推导出了主要的公式，以及针对公式，为了计算方便和计算效率进行了优化。本章我们使用C语言来实现其算法。
大家有什么疑问或者有什么好的想法，可以在下方留言，我看到会在第一时间进行回复。
我们在使用电机时，通常使用的是多个电机，我们设计代码时需要考虑多个电机同时运行，各自控制的情况。所以我们的代码要做到可重入、多线程安全，理论支持任意多个电机。

2.实现原理

我们使用状态机的形式来实现电机代码。
按照上篇理论表述，电机的运行总共可以分为3个状态：加速、匀速和减速。
整个状态机流程如下：
在这里插入图片描述

3.模块接口

每一个模块都有接口，所谓接口就是模块要正常运行，需要从外部获取哪些参数以及正常运行需要输出哪些参数。

3.1 输入参数

在上一原理篇中我们已经介绍过,我们要完整的的描画出一个梯形图，我们需要知道以下几个参数：

  • 电机步距角或步长。使用StepSize表示
  • 定时器频率，即给步进电机驱动器输入的PWM频率的定时器分频以后的频率。ClkFrq表示。
  • 加速段加速度。使用Accel表示。
  • 减速段的减速度。使用Decel表示。
  • 匀速段的速度。使用ConstSpeed表示。
  • 电机运行的总角度或者总步数，需要与StepSize单位相同，即要么都是角度，要么都是长度。使用TotalStep表示。

为了代码结构化，操作方便，我们将以上输入参数使用一个结构体进行封装。

    typedef struct TRAPE_ACCEL_PARAM_tag    
    {
        float               stepsize;         //电机步长或步距角
        uint32_t            ClkFrq;           //定时器频率
        float               accel;            //加速段加速度
        float               decel;            //减速段减速度
        float               constSpeed;       //最大速度，即匀速运行段速度        
        uint32_t            totalStep;        //电机走过的总步长或总角度，需要与stepLengh单位相同 
    } TRAPE_ACCEL_PARAM_t,*pTRAPE_ACCEL_PARAM_t;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

以上参数以怎样的方式传入我们的模块内，供模块调用呢，我们采用函数的方式，通过形参的方式将以上参数传递进入模块内。

/*---------------------------------------------------------------------------------------
 函数原型:  void TrapeAccel_Init(pTRAPE_ACCEL_PARAM_t pParam)
 功    能:  加速模块初始化
 输入参数:  pParam:梯度加速输入参数
 输出参数:	NA
 返 回 值:	如果初始化成功，返回梯度加速度ID,其他函数的调用需要使用此ID，失败，返回-1
---------------------------------------------------------------------------------------*/
void TrapeAccel_Init(pTRAPE_ACCEL_PARAM_t pParam)

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

3.2输出参数：

本模块的目的是让电机以梯形加速度的方式运行。所以最终的输出参数肯定是定时器重载寄存器的值CounterVal。
另外，我们还需要知道电机具体的运行状态status，即是正在加速、减速还是匀速运行中，调用者可能需要根据运行状态，做一些控制操作。
此外，可能还需要知道电机运行的步数StepCount,加速节点的步数AccelStep、运行阶段的步数ConstStep和减速阶段的步数DecelStep。
同理，我们也需要以函数的方式将上述参数传递出去，定义的函数如下：

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_Start(void)
 功    能:  开始计算梯形加减速相关参数
 输入参数:  NA
 输出参数:	NA
 返 回 值:	第一个Counter值
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_Start(void);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetAccelSteps(void)
 功    能:  获取加速阶段步数
 输入参数:  NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetAccelSteps(void);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetdecelSteps(void)
 功    能:  获取减速阶段步数
 输入参数:  NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetdecelSteps(void);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetConstSteps(void)
 功    能:  获取匀速阶段的步数
 输入参数:  NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetConstSteps(void);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetAccelSteps(void)
 功    能:  获取已运行的步数
 输入参数:  NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetStepCount(void);


/*---------------------------------------------------------------------------------------
 函数原型:  TRAPE_ACCEL_STAT_t TrapeAccel_GetStatus(void)
 功    能:  获取运行状态
 输入参数:  NA
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
TRAPE_ACCEL_STAT_t TrapeAccel_GetStatus(void);

/*-------------------------------------------------------------------------------------
函数原型: TRAPE_ACCEL_STAT_t  TrapeAccel_CalNextCounterVal(void, uint32_t * pCounterVal)
功    能:  计算下一步
输入参数:  NA
输出参数:  pCounterVal：计数器的值指针
返 回 值:  电机当前状态，返回状态为STOP时，则停止计算
---------------------------------------------------------------------------------------*/
TRAPE_ACCEL_STAT_t  TrapeAccel_CalNextCounterVal(void, uint32_t * pCounterVal);

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69

此函数需要同属输出两个参数，计算得到的定时器寄存器值和当前运行状态，运行状态我们以返回值形式传出，寄存器我们通过指针形式传出

4.模块控制块定义

我们前面说过，我们的代码要支持可重入、多线程、多电机运行。那么在代码中肯定没法使用一般的静态局部变量、全局变量了，但是我们需要保存加速段步数AccelStep、减速段步数DecelStep、匀速段步数ConstStep、上一步的寄存器值PrevCounterVal等等在计算过程中用到的变量，我们怎么实现呢？

4.1 电机数量设置

我们在代码中引入一个宏定义MAX_TRAPE_ACCEL_COUNT，用来定义可同时支持的做梯形加速度的电机数量:

#define MAX_TRAPE_ACCEL_COUNT 10  //目前设置的同时支持10个电机的加减速

  • 1

此宏定义开放给调用者，调用者通过它可以设置需要同时支持多少个电机。

4.2 定义数据控制块

接下来我们将上述所说的在过程中用到的参数封装起来即为我们所说的模块控制块。
控制块定义如下：

typedef struct TRAPE_ACCEL_CB_tag
{
    uint32_t                        accelLimStep;      //理论最大步数
    uint32_t                        accelStep;         //达到匀速段速度要运行的步数 
    int32_t                         decelStep;         //减速段步数
    int32_t                         stepCount;         //用来记录各状态下的步数，共过程计算使用
    uint32_t                        prevCounterVal;    //上一个定时器寄存器的值
    int32_t                         rest;              //上次计算所得余数
    TRAPE_ACCEL_STAT_t              status;            //状态
}__attribute__((packed))TRAPE_ACCEL_CB_t,*pTRAPE_ACCEL_CB_t;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10

然后定义一个元素个数为MAX_TRAPE_ACCEL_COUNT的数组,用来给每一个电机定义一份自己独有的过程参数，这样在函数调用过程中就解决了重入以及多线程问题。

static TRAPE_ACCEL_CB_t s_TrapeAccelCB[MAX_TRAPE_ACCEL_COUNT]; //内部数据控制块

  • 1

但是还有一个问题，就是3.1节中所说的输入参数如何传递给每一个电机的控制块？
我们可以在控制块中，给每个输入参数也定义了存储空间，输入参数在3.1节中的TrapeAccel_Init初始化函数中被复制到控制块中相应的变量中。

typedef struct TRAPE_ACCEL_CB_tag
{
    float                           stepsize;          //电机步长或步距角
    uint32_t                        ClkFrq;            //定时器频率
    uint32_t                        accel;             //加速段加速度
    uint32_t                        decelSpeed;        //减速段减速度
    uint32_t                        constSpeed;        //匀速运行段速度
    uint32_t                        total_step;        //电机走过的总步长或总角度，需要与stepLengh单位相同 
    uint32_t                        accelLimStep;      //理论最大步数
    uint32_t                        accelStep;         //达到匀速段速度要运行的步数 
    int32_t                         decelStep;         //减速段步数
    int32_t                         stepCount;         //用来记录各状态下的步数，共过程计算使用
    uint32_t                        prevCounterVal;    //上一个定时器寄存器的值
    int32_t                         rest;              //上次计算所得余数
    TRAPE_ACCEL_STAT_t              status;            //状态
}__attribute__((packed))TRAPE_ACCEL_CB_t,*pTRAPE_ACCEL_CB_t;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

这样做，至少有2个好处：（1）输入参数可以定义为局部变量，传值完成就可释放，无需调用者管理。（2）输入的参数均是浮点型的，但是看过上篇原理文章的读者肯定知道，浮点型数据计算量大，我们上篇中做了优化，在计算时，应该使用整形数据计算效率更高。所以我们在控制块中可以定义所有数据为整形，在TrapeAccel_Init初始化函数复赋值时，进行转化即可。

4.3控制块句柄定义

我们调用函数时，如何区分使用的是哪个控制块？
使用TrapeAccel_Init给控制块赋值完成后，我们把其数组序号返回给调用者，这样调用者想要控制电机，给函数传输相应数组序号即可。我们在第3节中定义的所有函数都没有输入参数，无法区分具体是操作哪个控制块，所以我们需要将控制块数组序号以输入参数的形式传递给这些函数。
为了代码清晰阅读，我们将控制块数据序号定义为梯形加速模块控制句柄TrapeAccelID_t:

#define TrapeAccelID_t      int32_t          //梯形加速控制句柄

  • 1

给TrapeAccel_Init函数加上返回值：控制块句柄。这样初始化后，我们就可以获得一个控制块句柄，后续的所有操作，通过传递控制块句柄，调用相应的控制块。如此，各个接口函数最终形式为如下：

/*---------------------------------------------------------------------------------------
 函数原型:  TrapeAccelID_t TrapeAccel_Init(pTRAPE_ACCEL_PARAM_t pParam)
 功    能:  加速模块初始化
 输入参数:  pParam:梯度加速输入参数
 输出参数:	NA
 返 回 值:	如果初始化成功，返回梯度加速度ID,其他函数的调用需要使用此ID，失败，返回-1
---------------------------------------------------------------------------------------*/ TrapeAccelID_t TrapeAccel_Init(pTRAPE_ACCEL_PARAM_t pParam);

/*---------------------------------------------------------------------------------------
 函数原型:  void TrapeAccel_Deinit(TrapeAccelID_t id)
 功    能: TrapeAccelID_t: 加速模块释放初始化
 输入参数:  id：梯度加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
void TrapeAccel_Deinit(TrapeAccelID_t id);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_Start(TrapeAccelID_t id)
 功    能:  开始计算梯形加减速相关参数
 输入参数:  id：梯度加速度ID
 输出参数:	NA
 返 回 值:	第一个Counter值
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_Start(TrapeAccelID_t id);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetAccelSteps(TrapeAccelID_t id)
 功    能:  获取加速阶段步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetAccelSteps(TrapeAccelID_t id);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetdecelSteps(TrapeAccelID_t id)
 功    能:  获取减速阶段步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetdecelSteps(TrapeAccelID_t id);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetConstSteps(TrapeAccelID_t id)
 功    能:  获取匀速阶段的步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetConstSteps(TrapeAccelID_t id);

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetAccelSteps(TrapeAccelID_t id)
 功    能:  获取已运行的步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetStepCount(TrapeAccelID_t id);


/*---------------------------------------------------------------------------------------
 函数原型:  TRAPE_ACCEL_STAT_t TrapeAccel_GetStatus(TrapeAccelID_t id)
 功    能:  获取运行状态
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
TRAPE_ACCEL_STAT_t TrapeAccel_GetStatus(TrapeAccelID_t id);

/*-------------------------------------------------------------------------------------
函数原型: TRAPE_ACCEL_STAT_t  TrapeAccel_CalNextCounterVal(TrapeAccelID_t id, uint32_t * pCounterVal)
功    能:  计算下一步
输入参数:  id：梯形加速ID
输出参数:  pCounterVal：计数器的值指针
返 回 值:  电机当前状态，返回状态为STOP时，则停止计算
---------------------------------------------------------------------------------------*/
TRAPE_ACCEL_STAT_t  TrapeAccel_CalNextCounterVal(TrapeAccelID_t id, uint32_t * pCounterVal);

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87

4.4控制块增加控制句柄用于检索

我们在初始化的时候，如何知道具体初始化的是哪个控制块？
我们如何知道这个控制块有没有被使用呢？
我们如何知道定义的控制块有没有用完呢？
一个控制块使用结束了了，另一个梯形加减速想使用这个控制块又如何实现呢？

我们在控制块中增加一个变量：

    TrapeAccelID_t                  TrapeAccel_ID;     //控制块数据序号，检索使用

  • 1

这样控制块最终变为如下：

typedef struct TRAPE_ACCEL_CB_tag
{
    float                           stepsize;          //电机步长或步距角
    uint32_t                        ClkFrq;            //定时器频率
    uint32_t                        accel;             //加速段加速度
    uint32_t                        decelSpeed;        //减速段减速度
    uint32_t                        constSpeed;        //匀速运行段速度
    uint32_t                        total_step;        //电机走过的总步长或总角度，需要与stepLengh单位相同 
    uint32_t                        accelLimStep;      //理论最大步数
    uint32_t                        accelStep;         //达到匀速段速度要运行的步数 
    int32_t                         decelStep;         //减速段步数
    int32_t                         stepCount;         //用来记录各状态下的步数，共过程计算使用
    uint32_t                        prevCounterVal;    //上一个定时器寄存器的值
    int32_t                         rest;              //上次计算所得余数
    TrapeAccelID_t                  TrapeAccel_ID;     //控制块数据序号，检索使用
    TRAPE_ACCEL_STAT_t              status;            //状态
}__attribute__((packed))TRAPE_ACCEL_CB_t,*pTRAPE_ACCEL_CB_t;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17

我们在首次使用控制块之前，将所有控制块中的TrapeAccel_ID赋值为-1。
在初始化函数TrapeAccel_Init中，我们对控制块进行遍历，查找首个TrapeAccel_ID!=-1的句柄，然后对齐进行赋值初始化，将句柄进行返回，若没有TrapeAccel_ID!=-1的句柄的句柄，则表示所有控制块已用完，返回-1，表示初始化失败。
最终，初始化函数如下：

/*---------------------------------------------------------------------------------------
 函数原型:  TrapeAccelID_t TrapeAccel_Init(pTRAPE_ACCEL_PARAM_t pParam)
 功    能:  加速模块初始化
 输入参数:  pParam:梯度加速输入参数
 输出参数:	NA
 返 回 值:	如果初始化成功，返回梯度加速度ID,其他函数的调用需要使用此ID，失败，返回-1
---------------------------------------------------------------------------------------*/
TrapeAccelID_t TrapeAccel_Init(pTRAPE_ACCEL_PARAM_t this)
{
    static bool oneInit;
    
    assert(this != NULL);
   
    if(!oneInit) //首次进入时，对所有控制块句柄赋值为-1
    {
        for(uint32_t i = 0; i < MAX_TRAPE_ACCEL_COUNT; i++)
        {
            s_TrapeAccelCB[i].TrapeAccel_ID = -1;
        }
        oneInit = true;
    }

    

    for(int32_t i = 0; i < MAX_TRAPE_ACCEL_COUNT; i++)
    {
        if(s_TrapeAccelCB[i].TrapeAccel_ID == -1)
        {
            s_TrapeAccelCB[i].stepsize = this->stepsize;
            s_TrapeAccelCB[i].ClkFrq = this->ClkFrq;
            s_TrapeAccelCB[i].accel = (uint32_t)(this->accel*100); //加速度扩大100倍
            s_TrapeAccelCB[i].decel = (uint32_t)(this->decel*100);//减速度扩大100倍
            s_TrapeAccelCB[i].constSpeed = (uint32_t)(this->constSpeed*100);//速度扩大100倍
            s_TrapeAccelCB[i].totalStep = this->totalStep;
            s_TrapeAccelCB[i].TrapeAccel_ID = i;
            return i;
        }
    }
    return -1;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40

另外，我们增加一个反初始化函数，一个控制块使用完后，对其进行回收，这样另一个梯形加速电机可以重复使用此控制块。在反初始化函数中我们只需将TrapeAccel_ID赋值为-1即可。

/*---------------------------------------------------------------------------------------
 函数原型:  void TrapeAccel_Deinit(TrapeAccelID_t id)
 功    能: TrapeAccelID_t: 加速模块释放初始化
 输入参数:  id：梯度加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
void TrapeAccel_Deinit(TrapeAccelID_t id)
{
    assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    memset(&s_TrapeAccelCB[id],0,sizeof(TRAPE_ACCEL_CB_t));

    s_TrapeAccelCB[id].TrapeAccel_ID = -1;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

这样我们的控制块定义完成。

5. 函数定义

各个函数定义如下，我们不一一详述，只对其中的注意点进行描述

5.1 TrapeAccel_Start函数—启动梯形加减速计算

我们定义TrapeAccel_Start函数用于启动梯形加速算法，并返回首个定时器寄存器的值CounterVal。

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_Start(TrapeAccelID_t id)
 功    能: 启动计算梯形加减速计算
 输入参数:  id：梯度加速度ID
 输出参数:	NA
 返 回 值:	第一个Counter值
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_Start(TrapeAccelID_t id)
{
    
    pTRAPE_ACCEL_CB_t this;
    uint32_t val;

    assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    this = &s_TrapeAccelCB[id];


    TrapeAccel_CalAccelStep(this);  //计算Accel_Step

    TrapeAccel_CalAccelLimitStep(this);//计算Accel_LimitStep

    TrapeAccel_CalDecelStep(this);//计算Decel_Step

    TrapeAccel_Reset(this);//复位状态机

    val = TrapeAccel_CalFirstCounterVal(this);//计算首个CounterVal
    return val;
   
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31

TrapeAccel_Start函数内使用的各个静态函数定义如下：

5.1.1 TrapeAccel_Reset—状态机复位函数

状态机复位函数,每次使用完后或者开始时，对状态机进行复位

/*---------------------------------------------------------------------------------------
 函数原型:  static void TrapeAccel_Reset(TrapeAccelID_t id)
 功    能:  复位
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
static void TrapeAccel_Reset(pTRAPE_ACCEL_CB_t this)
{
    assert(this != NULL);

    this->status = ACCEL;
    this->rest = 0;
    this->stepCount = 0;
    this->prevCounterVal = 0;

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18

5.1.2 TrapeAccel_CalAccelStep—计算达到匀速速度所需要的加速步数

该函数中注意的一点就是，constSpeed是32位数据，最大为4294967296，是一个10位数据，我们之前已经对其放大了100倍。所以当constSpeed的值为一个较大的三位数时，其平方就会溢出，所以在下方计算中，我们对其进强制类型转化为64为整形数据

/*-------------------------------------------------------------------------------------
函数原型:  static inline void TrapeAccel_CalAccelStep(pTRAPE_ACCEL_CB_t this)
功    能:  计算加速到最大速度时的步数
输入参数:  this: 梯形加速度控制块指针
输出参数:  NA
返 回 值:  NA
注意事项：
---------------------------------------------------------------------------------------*/
static inline void TrapeAccel_CalAccelStep(pTRAPE_ACCEL_CB_t this)
{

    assert(this != NULL);
  
    this->accelStep = (uint64_t)this->constSpeed * (uint64_t)this->constSpeed 
                        / (2 * this->accel * (uint64_t)(this->stepsize * 100));
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16

5.1.3 TrapeAccel_CalAccelLimitStep—计算加速阶段的理论最大步数

同理，为防止溢出，一下函数中也进行了强制类型转化为64为非负整形数据

/*-------------------------------------------------------------------------------------
函数原型:  static inline void TrapeAccel_CalAccelLimitStep(pTRAPE_ACCEL_CB_t this)
功    能:  计算加速极限步数
输入参数:  this: 梯形加速度控制块指针
输出参数:  NA
返 回 值:  NA
注意事项：
---------------------------------------------------------------------------------------*/
static inline void TrapeAccel_CalAccelLimitStep(pTRAPE_ACCEL_CB_t this)
{

    assert(this != NULL);

    
    this->accelLimStep = (uint64_t)this->accel * (uint64_t)this->totalStep 
									/ ( this->accel + this->decel);
   
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18

5.1.4 TrapeAccel_CalDecelStep—计算减速阶段步数

同理，为防止溢出，一下函数中也进行了强制类型转化为64为非负整形数据.
另外，decelStep与其他变量不同的时，我们定义为32位整形，以负数表示。这样后续求减速阶段的CounterVal时，我们直接使用即可。


/*-------------------------------------------------------------------------------------
函数原型:  static inline void TrapeAccel_CalDecelStep(pTRAPE_ACCEL_CB_t this)
功    能:  计算减速阶段的步数
输入参数:  accelParam：计算梯形加速相关参数
输出参数:  NA
返 回 值:  NA
注意事项： 此函数必须在TrapeAccel_CalAccelStep和TrapeAccel_CalAccelStep之后调用
---------------------------------------------------------------------------------------*/
static inline void TrapeAccel_CalDecelStep(pTRAPE_ACCEL_CB_t this)
{

    assert(this != NULL);

    if(this->accelLimStep > this->accelStep)
    {
        this->decelStep = -(int32_t)((uint64_t)this->accelStep * (uint64_t)this->accel / this->decel);     
    }
    else
    {
        this->decelStep =  (int32_t)this->accelLimStep - (int32_t)this->totalStep;
    }

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

5.1.5 TrapeAccel_CalFirstCounterVal—计算首个CounnterVal值

同理，为防止溢出，一下函数中也进行了强制类型转化为64为非负整形数据.
另外，decelStep与其他变量不同的时，我们定义为32位整形，并给其增加负号。这样后续求减速阶段的CounterVal时，我们直接使用即可。

/*-------------------------------------------------------------------------------------
函数原型:  static inline uint32_t TrapeAccel_CalFirstCounterVal(pTRAPE_ACCEL_CB_t this)
功    能:  计算电机加速运行时的初始计数器值
输入参数:  accelParam：计算梯形加速相关参数
输出参数:  NA
返 回 值:  计数器值
注意事项：
---------------------------------------------------------------------------------------*/
static inline uint32_t TrapeAccel_CalFirstCounterVal(pTRAPE_ACCEL_CB_t this)
{
    uint32_t T1_FQEQ;
    uint64_t  A_SQ;
    uint32_t counerVal;

    assert(this != NULL);

    T1_FQEQ = (uint32_t)(0.00676 * this->ClkFrq);

    A_SQ = (uint64_t)(20000000000LLU * (double)this->stepsize );

	
    counerVal = (uint32_t)(T1_FQEQ * sqrt(A_SQ /(uint64_t)this->accel)*0.01);
    this->prevCounterVal = counerVal;
    return counerVal;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25

5.2 TrapeAccel_CalNextCounterVal—计算下个周期内的计数器值

该函数返回状态值，其定义如下：

typedef enum
{
    ACCEL = 0,
    CONSTANT,
    DECEL,
    STOP,
} TRAPE_ACCEL_STAT_t;

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

当减速完成后，会返回状态STOP，调用发收到STOP后可以停止定时器PWM输出，来停止电机调用示例如下(在定时器17中断函数中调用)：

void TIM17_IRQHandler(void)
{
   if(LL_TIM_IsActiveFlag_CC1(TIM17))
   {
        LL_TIM_ClearFlag_CC1(TIM17); 
       
        uint32_t counerVal;
        TRAPE_ACCEL_STAT_t stat;
       
        
        stat = TrapeAccel_CalNextCounterVal(0, &counerVal);
       
        if(stat == STOP)
        {
            TimerDisable(object->pParams);//停止定时器
        }
        else
        {
            SetPWMFrq(object->pParams,counerVal);//设置ARR寄存器的值
        }
   }
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
/*-------------------------------------------------------------------------------------
函数原型: TRAPE_ACCEL_STAT_t  TrapeAccel_CalNextCounterVal(TrapeAccelID_t id, uint32_t * pCounterVal)
功    能:  计算下一步
输入参数:  id：梯形加速ID
输出参数:  pCounterVal：计数器的值指针
返 回 值:  电机当前状态，返回状态为STOP时，则停止计算
---------------------------------------------------------------------------------------*/
TRAPE_ACCEL_STAT_t  TrapeAccel_CalNextCounterVal(TrapeAccelID_t id, uint32_t * pCounterVal)
{
    
    pTRAPE_ACCEL_CB_t this;

    assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);
    assert(pCounterVal != NULL);


    this = &s_TrapeAccelCB[id];
    this->stepCount++;
    switch(this->status)
    {

        case ACCEL:
            if((uint32_t)this->stepCount > this->accelLimStep)
            {
                this->status = DECEL;
                this->stepCount = this->decelStep;
                break;
            }
            else if((uint32_t)this->stepCount > this->accelStep)
            {
                this->status = CONSTANT;
                break;
            }
            *pCounterVal = TrapeAccel_CalCounterValAndRest(this);
            break;
        case CONSTANT:
            if((uint32_t)this->stepCount > (uint32_t)((int32_t)this->totalStep + this->decelStep))//因为this->decelStep本身带负号，所以用+
            {
                this->status = DECEL;
                this->stepCount = this->decelStep;
                break;
            }

            break;
        case DECEL:
            if(this->stepCount == 0)
            {
                this->status = STOP;
                break;
            }
            *pCounterVal = TrapeAccel_CalCounterValAndRest(this);

            break;
        case STOP:
            *pCounterVal = 0;
            TrapeAccel_Reset(this); 
            break;
        
    }
    return this->status;
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61

5.2.1 TrapeAccel_CalCounterValAndRest—计算下个周期内的计数器值以及余数

/*-------------------------------------------------------------------------------------
函数原型:  static inline uint32_t TrapeAccel_CalCounterValAndRest(pTRAPE_ACCEL_CB_t this)
功    能:  计算电机下个周期内的计数器值以及余数
输入参数:  accelParam：计算梯形加速相关参数
输出参数:  NA
返 回 值:  计数器值
注意事项：
---------------------------------------------------------------------------------------*/
static inline uint32_t TrapeAccel_CalCounterValAndRest(pTRAPE_ACCEL_CB_t this)
{
    uint32_t temp;

    assert(this != NULL);

    
    temp = (uint32_t)((int32_t)this->prevCounterVal -
			(2 * (int32_t)this->prevCounterVal + this->rest) / (4*this->stepCount + 1));

    this->rest = (2 * (int32_t)this->prevCounterVal + this->rest) % (4*this->stepCount + 1);
    
    this->prevCounterVal = temp;

    return temp; 
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

5.3 其他反馈函数

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetAccelSteps(TrapeAccelID_t id)
 功    能:  获取加速阶段步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetAccelSteps(TrapeAccelID_t id)
{
    
	pTRAPE_ACCEL_CB_t this;
	
	assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    this = &s_TrapeAccelCB[id];
    if(this->accelStep > this->accelLimStep)
    {
        return this->accelLimStep;
    }
    else
    {
        return this->accelStep;
    }
}

/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetdecelSteps(TrapeAccelID_t id)
 功    能:  获取减速阶段步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetdecelSteps(TrapeAccelID_t id)
{
    
	pTRAPE_ACCEL_CB_t this;
	
	assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    this = &s_TrapeAccelCB[id];

   return (uint32_t)(-this->decelStep);

}


/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetConstSteps(TrapeAccelID_t id)
 功    能:  获取匀速阶段的步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetConstSteps(TrapeAccelID_t id)
{
    
	pTRAPE_ACCEL_CB_t this;
	
	assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    this = &s_TrapeAccelCB[id];

   return (uint32_t)((int32_t)this->totalStep-(int32_t)this->accelStep+this->decelStep);

}
/*---------------------------------------------------------------------------------------
 函数原型:  uint32_t TrapeAccel_GetAccelSteps(TrapeAccelID_t id)
 功    能:  获取已运行的步数
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
uint32_t TrapeAccel_GetStepCount(TrapeAccelID_t id)
{
    
	pTRAPE_ACCEL_CB_t this;
	
	assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    this = &s_TrapeAccelCB[id];
    if(this->status == DECEL)
    {
        return (uint32_t)((int32_t)this->totalStep + this->stepCount+1);
    }
    else
    {
        return (uint32_t)this->stepCount;
    }

    

}

/*---------------------------------------------------------------------------------------
 函数原型:  TRAPE_ACCEL_STAT_t TrapeAccel_GetStatus(TrapeAccelID_t id)
 功    能:  获取运行状态
 输入参数:  id: 梯形加速度ID
 输出参数:	NA
 返 回 值:	NA
 注意事项：	
---------------------------------------------------------------------------------------*/
TRAPE_ACCEL_STAT_t TrapeAccel_GetStatus(TrapeAccelID_t id)
{
    
	pTRAPE_ACCEL_CB_t this;
	
	assert(id >=0 || id<MAX_TRAPE_ACCEL_COUNT);

    this = &s_TrapeAccelCB[id];
  
    return this->status;

}

/*-------------------------------------------------------------------------------------
函数原型:  static inline void TrapeAccel_CalAccelStep(pTRAPE_ACCEL_CB_t this)
功    能:  计算加速到最大速度时的步数
输入参数:  this: 梯形加速度控制块指针
输出参数:  NA
返 回 值:  NA
注意事项：
---------------------------------------------------------------------------------------*/
static inline void TrapeAccel_CalAccelStep(pTRAPE_ACCEL_CB_t this)
{

    assert(this != NULL);
  
    this->accelStep = (uint64_t)this->constSpeed * (uint64_t)this->constSpeed 
                        / (2 * this->accel * (uint32_t)(this->stepsize * 100));
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97
  • 98
  • 99
  • 100
  • 101
  • 102
  • 103
  • 104
  • 105
  • 106
  • 107
  • 108
  • 109
  • 110
  • 111
  • 112
  • 113
  • 114
  • 115
  • 116
  • 117
  • 118
  • 119
  • 120
  • 121
  • 122
  • 123
  • 124
  • 125
  • 126
  • 127
  • 128
  • 129
  • 130
  • 131
  • 132
  • 133
  • 134

6.例程

我使用的MCU是STM32H743。
使用定时器17来控制生成PWM。定时器17分频前频率为240MHZ，分频值为479，则分频后定时器始终频率为500kHZ。
定时器17的初始化代码如下：


/**
  * @brief TIM17 Initialization Function
  * @param None
  * @retval None
  */
void MX_TIM17_Init(void)
{

    /* USER CODE BEGIN TIM17_Init 0 */

    /* USER CODE END TIM17_Init 0 */

    TIM_OC_InitTypeDef sConfigOC = {0};
    TIM_BreakDeadTimeConfigTypeDef sBreakDeadTimeConfig = {0};

    /* USER CODE BEGIN TIM17_Init 1 */

    /* USER CODE END TIM17_Init 1 */
    htim17.Instance = TIM17;
    htim17.Init.Prescaler = 479;
    htim17.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim17.Init.Period = 9999;
    htim17.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim17.Init.RepetitionCounter = 0;
    htim17.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
    if (HAL_TIM_Base_Init(&htim17) != HAL_OK)
    {
        Error_Handler();
    }
    if (HAL_TIM_PWM_Init(&htim17) != HAL_OK)
    {
        Error_Handler();
    }
    sConfigOC.OCMode = TIM_OCMODE_PWM1;
    sConfigOC.Pulse = 4999;
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCNPolarity = TIM_OCNPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_ENABLE;
    sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET;
    sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET;
    if (HAL_TIM_PWM_ConfigChannel(&htim17, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
    {
        Error_Handler();
    }
    sBreakDeadTimeConfig.OffStateRunMode = TIM_OSSR_DISABLE;
    sBreakDeadTimeConfig.OffStateIDLEMode = TIM_OSSI_DISABLE;
    sBreakDeadTimeConfig.LockLevel = TIM_LOCKLEVEL_OFF;
    sBreakDeadTimeConfig.DeadTime = 0;
    sBreakDeadTimeConfig.BreakState = TIM_BREAK_DISABLE;
    sBreakDeadTimeConfig.BreakPolarity = TIM_BREAKPOLARITY_HIGH;
    sBreakDeadTimeConfig.BreakFilter = 0;
    sBreakDeadTimeConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE;
    if (HAL_TIMEx_ConfigBreakDeadTime(&htim17, &sBreakDeadTimeConfig) != HAL_OK)
    {
        Error_Handler();
    }
    /* USER CODE BEGIN TIM17_Init 2 */

    /* USER CODE END TIM17_Init 2 */
    HAL_TIM_MspPostInit(&htim17);

}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63

定义控制块句柄：

static TrapeAccelID_t TrapeAccelId;

  • 1

在函数中调用加减速模块相关函数：

/*---------------------------------------------------------------------------------------
 函数原型: void SYSM_Start(void const *arg)
 功    能: 初始化并启动提醒加速度
 输入参数: NA
 输出参数: NA
 返 回 值: NA
---------------------------------------------------------------------------------------*/
__attribute__((noreturn)) static void SYSM_Satrt(void *arg)
{
    uint32_t counter;

    TRAPE_ACCEL_PARAM_t AccelParam =
    {
        .stepsize = 1.8f,
        .ClkFrq = 500000,
        .accelSpeed = 5400,
        .decelSpeed = 5400,
        .constSpeed =  32400,
        .total_step = 1000000,
    };

    (void)arg;   


    
    TrapeAccelId = TrapeAccel_Init(&AccelParam);

    counter = TrapeAccel_Start(TrapeAccelId);

    LL_TIM_SetAutoReload(TIM17, frq);
    LL_TIM_OC_SetCompareCH1(TIM17, frq/2);
    HAL_TIM_PWM_Start_IT(&htim17,TIM_CHANNEL_1);

    while(1)
    {
        osDelay(500);
    }
}

void TIM17_IRQHandler(void)
{
   if(LL_TIM_IsActiveFlag_CC1(TIM17))
   {
        LL_TIM_ClearFlag_CC1(TIM17); 
       
        uint32_t counerVal;
        TRAPE_ACCEL_STAT_t stat;
       
        
        stat = TrapeAccel_CalNextCounterVal(TrapeAccelId, &counerVal);
       
        if(stat == STOP)
        {
           HAL_TIM_PWM_Stop_IT(&htim17,TIM_CHANNEL_1);
        }
        else
        {
            LL_TIM_SetAutoReload(TIM17, frq);
            LL_TIM_OC_SetCompareCH1(TIM17, frq/2);
        }
   }
}

  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/繁依Fanyi0/article/detail/782848
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号