1 背景

在2021年接觸到步進(jìn)電機(jī)，當(dāng)時(shí)是用來(lái)驅(qū)動(dòng)熱敏打印機(jī)，沒(méi)有用到加減速算法，速度時(shí)間表好像是日本客戶那邊提供過(guò)來(lái)的，這次調(diào)試加減速算法，遇到了不少問(wèn)題，在這里記錄一下，希望能幫到未來(lái)對(duì)此有困惑的自己，如果能幫到其他人也算是有幸。

2 步進(jìn)電機(jī)基本參數(shù)

2.1 驅(qū)動(dòng)器

2.2 電機(jī)

2.3 接線

2.4 細(xì)分及電流配置

2.5 驅(qū)動(dòng)方式

一般有3種：定時(shí)器中斷+GPIO、GPIO+延時(shí)、PWM比較輸出，這里使用的是PWM比較輸出模式，在比較中斷中不斷地更新PWM周期來(lái)達(dá)到改變速度的目的。

3 梯形加減速算法

3.1 推導(dǎo)過(guò)程

3.1.1 組成部分

步進(jìn)電機(jī)的動(dòng)作過(guò)程是由加速、勻速、減速三個(gè)部分組成，轉(zhuǎn)速的單位是rpm，每分鐘多少轉(zhuǎn)，最終需要rpm轉(zhuǎn)換成每秒的脈沖數(shù)也就是頻率，速度越快頻率越大，對(duì)應(yīng)的脈沖周期T就越小。

3.1.2 速度和脈沖周期的關(guān)系

第1個(gè)脈沖周期t=t1-t0，第2個(gè)脈沖周期t=t2-t1，那么t=計(jì)數(shù)值*計(jì)數(shù)時(shí)間，例如72MHz的情況下，定時(shí)器預(yù)分配為72-1,計(jì)數(shù)值設(shè)置為0xFFFF，脈沖周期t=65535*(1/1MHz)，使用PWM的翻轉(zhuǎn)模式，翻轉(zhuǎn)模式下，2次翻轉(zhuǎn)為一個(gè)完整脈沖，那么設(shè)置比較值為0xFFFF/2，那么t=65.535ms，速度pps=15.2Hz，也就是1秒鐘15.2個(gè)脈沖。

3.1.3 加速度和距離的關(guān)系

s=vot + (1/2)at2，由于初速度為0，那么S=(1/2)*a*t2

s=alpha(步距角)*n(脈沖數(shù)),那么(1/2)at2=alpha*n

整個(gè)加速時(shí)間t=tn-t0，t0=0，那么tn=t，那么tn=√(2*n *alpha/a)

tn=cn*tt(計(jì)數(shù)時(shí)間)

3.1.4 下一個(gè)脈沖計(jì)數(shù)

cn*tt=t(n+1)-tn=√(2*(n-1) *alpha/a)-√(2*(n) *alpha/a)

cn=1/tt (√(n+1)-√n)√(2*alpha/a)

c0=1/tt √(2alpha/a)

cn=c0*(√(n+1)-√n)

根據(jù)麥克勞林公式

√(n+-1) = 1+-1/2n-1/8*n2+O(1/n3)

cn/cn-1 = c0(√(n+1)-√n)/c0(√(n)-√n-n)

最終化簡(jiǎn)后得到

cn=cn-1 - (2cn-1 / 4n+1)

3.1.5 誤差和放大倍數(shù)

當(dāng)n=1時(shí)有0.4485的偏差,將C0乘以0.676來(lái)解決這個(gè)誤差

假設(shè)主頻為72M預(yù)分頻系數(shù)為72-1，那么

C0 = 10000000.676√(2alpha/a)

為了保證計(jì)算的精度，需要對(duì)相關(guān)變量進(jìn)行放大處理，這里對(duì)加速度放大10倍

C0=10000000.676 √(2alpha/a10)

那么對(duì)2*alpha乘以100000，

根號(hào)中2alpha100000/a10=2alpha*10000/a

100*√(2*alpha/a)

對(duì)1000000*0.676除以10倍，然后再對(duì)整個(gè)結(jié)果除以10，那么

C0=(10000000.676 /10√(2*alpha/a))/10

3.1.6 相關(guān)宏定義

#define FSPR 200

#define MICRO_STEP 8

#define SPR (FSPR*MICRO_STEP)

#define ALPHA 2*3.14159/SPR

#define GAIN_X 10

#define FREQUENCY_1 1000000*0.676/GAIN_X

#define ALPHA_2_GAIN_100000 2ALPHA100000

3.2 單位轉(zhuǎn)換

通常情況下加減速時(shí)間(ms)、最大速度(mm/s)、距離(mm)是已知的，我們需要將轉(zhuǎn)速mm/s轉(zhuǎn)換成角速度(rad/s)，將加減速時(shí)間轉(zhuǎn)換成加速度(rad/sec^2)

已知公式角速度=轉(zhuǎn)速*2π/60

假設(shè)轉(zhuǎn)速有9.55rpm，那么角速度=9.5523.14159/60=1(rad/s)

3.2.1 單位轉(zhuǎn)換相關(guān)宏定義

假設(shè)軸直徑是5mm，默認(rèn)情況下1轉(zhuǎn)的距離是(5*3.1415)

#define CONFIG_ROUND_MM (5*3.1415)

#define MMPS_TO_RPM(SPEED) (SPEED*60/CONFIG_ROUND_MM)

#define RPM_TO_RADPS(SPEED) (SPEED*2π/60)

#define MMPS_TO_RAD_PS(SPEED) (SPEED*2π/CONFIG_ROUND_MM)

#define MMPS_TO_RAD_0_1_PS(SPEED) (SPEED102π/CONFIG_ROUND_MM)

3.3 代碼部分

3.3.1 初始化

PA1作為PWM，PA2作為DIR

TIM_HandleTypeDef   htim2;
void TIM_PWM_Init(u16 arr,u16 psc)
{  
    TIM_OC_InitTypeDef sConfigOC = {0};
    htim2.Instance = TIM2;
    htim2.Init.Prescaler = psc;
    htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim2.Init.Period = arr;
    htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    htim2.Init.RepetitionCounter = 0; 
    HAL_TIM_OC_Init(&htim2);

    sConfigOC.OCMode = TIM_OCMODE_TOGGLE;;
    sConfigOC.Pulse = arr/2;
    sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
    sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
    HAL_TIM_OC_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2);
  HAL_TIM_Base_Start(&htim2); 
}
void HAL_TIM_OC_MspInit(TIM_HandleTypeDef *htim)
{
    if (htim->Instance == TIM2)
    {
    GPIO_InitTypeDef GPIO_Initure;
    __HAL_RCC_TIM2_CLK_ENABLE(); 
    __HAL_AFIO_REMAP_TIM2_PARTIAL_2();  
    __HAL_RCC_GPIOA_CLK_ENABLE(); 
    GPIO_Initure.Pin=GPIO_PIN_1; //PB5
    GPIO_Initure.Mode=GPIO_MODE_AF_PP;  
    GPIO_Initure.Pull=GPIO_PULLUP; 
    GPIO_Initure.Speed=GPIO_SPEED_FREQ_HIGH; 
      HAL_GPIO_Init(GPIOA,&GPIO_Initure);
        HAL_NVIC_SetPriority(TIM2_IRQn, 2, 2);
        HAL_NVIC_EnableIRQ(TIM2_IRQn);
    }
}

3.3.2 加減速算法初始化

輸入?yún)?shù)是總步數(shù)、加減速時(shí)間ms、最大速度mm/s

__IO uint16_t tim_count;        /* 達(dá)到最大速度時(shí)的步數(shù)*/
    __IO uint32_t max_s_lim;        /* 必須要開(kāi)始減速的步數(shù)（如果加速?zèng)]有達(dá)到最大速度）*/
    __IO uint32_t accel_lim;

    if(g_motion_sta != STOP)        /* 只允許步進(jìn)電機(jī)在停止的時(shí)候才繼續(xù)*/
        return;

  __IO int32_t accel = (speed*1000)/accel_time_ms;

  __IO int32_t decel = accel;


    if(step < 0)                    /* 步數(shù)為負(fù)數(shù) */
    {   
        g_srd.dir = CCW;            /* 逆時(shí)針?lè)较蛐D(zhuǎn) */ 
        step = -step;               /* 獲取步數(shù)絕對(duì)值 */
    }
    else
    {
        g_srd.dir = CW;             /* 順時(shí)針?lè)较蛐D(zhuǎn) */ 
    }


    if(step == 1)                   
    {
        g_srd.accel_count = -1;     
        g_srd.run_state = DECEL;     
        g_srd.step_delay = 1000;     
    }
    else if(step != 0)              /* 如果目標(biāo)運(yùn)動(dòng)步數(shù)不為0*/
    { 
        g_srd.min_delay = (int32_t)(A_T_x10 /speed); //勻速運(yùn)行時(shí)的計(jì)數(shù)值 

        g_srd.step_delay = (int32_t)((T1_FREQ_148 * sqrt(A_SQ / accel))/10); /* c0 */


        max_s_lim = (uint32_t)(speed*speed / (A_x200*accel/10));/* 計(jì)算多少步之后達(dá)到最大速度的限制 max_s_lim = speed^2 / (2*alpha*accel) */


        if(max_s_lim == 0)                                      /* 如果達(dá)到最大速度小于0.5步，我們將四舍五入為0,但實(shí)際我們必須移動(dòng)至少一步才能達(dá)到想要的速度 */
        {
            max_s_lim = 1;
        }
        accel_lim = (uint32_t)(step*decel/(accel+decel));       /* 這里不限制最大速度 計(jì)算多少步之后我們必須開(kāi)始減速 n1 = (n1+n2)decel / (accel + decel) */


        if(accel_lim == 0)                                      /* 不足一步 按一步處理*/
        {
            accel_lim = 1;
        }
        if(accel_lim <= max_s_lim)                              /* 加速階段到不了最大速度就得減速。。。使用限制條件我們可以計(jì)算出減速階段步數(shù) */
        {
            g_srd.decel_val = accel_lim - step;                 /* 減速段的步數(shù) */
        }
        else
        {
            g_srd.decel_val = -(max_s_lim*accel/decel);         /* 減速段的步數(shù) */
        }
        if(g_srd.decel_val == 0)                                /* 不足一步 按一步處理 */
        {
            g_srd.decel_val = -1;
        }
        g_srd.decel_start = step + g_srd.decel_val;             /* 計(jì)算開(kāi)始減速時(shí)的步數(shù) */
        
        if(g_srd.step_delay <= g_srd.min_delay)                 /* 如果一開(kāi)始c0的速度比勻速段速度還大，就不需要進(jìn)行加速運(yùn)動(dòng)，直接進(jìn)入勻速 */
        {
            g_srd.step_delay = g_srd.min_delay;
            g_srd.run_state = RUN;
        }
        else  
        {
            g_srd.run_state = ACCEL;
        }
        g_srd.accel_count = 0;                                  /* 復(fù)位加減速計(jì)數(shù)值 */
    }
    g_motion_sta = 1;                                           /* 電機(jī)為運(yùn)動(dòng)狀態(tài) */
    tim_count=__HAL_TIM_GET_COUNTER(&htimx_STEPMOTOR);
    __HAL_TIM_SET_COMPARE(&htimx_STEPMOTOR,STEPMOTOR_TIM_CHANNELn,tim_count+g_srd.step_delay/2);  /* 設(shè)置定時(shí)器比較值 */

3.3.4 中斷處理

void TIM2_IRQHandler(void)
{
    HAL_TIM_IRQHandler(&htim2);
}


void HAL_TIM_OC_DelayElapsedCallbackstepper_ctl_t *p_ctl,(TIM_HandleTypeDef *htim)
{                                                                               
    __IO static uint8_t i = 0;                            
    uint32_t capture = 0;

    if(htim==p_ctl->htim)
    { 
        capture = tmr_channel_value_get(p_ctl->tmr_x, p_ctl->timer_channel);
        tmr_channel_value_set(p_ctl->tmr_x, p_ctl->timer_channel, capture + p_ctl->step_delay/2);

        if (STOP_MONITOR_LEVEL == rt_pin_read(STOP_PIN))
        {
            stepper_ctl_stop(p_ctl);
        }


        i++;

        if (i == 2)                                       
        {
            i = 0;        

            if (IRON_ACTION_RUN == p_ctl->action_type)
            {
                switch(p_ctl->run_state)                        
                {
                    case STOP:
                    {
                        stepper_ctl_stop(p_ctl);
                        p_ctl->step_count = 0;                             
                        p_ctl->rest_delay = 0;       
                        p_ctl->last_accel_delay = 0;
                        p_ctl->new_step_delay = 0;
                        i = 0;
                    }
                    break;

                    case ACCEL:
                    {
                        p_ctl->step_count++;    

                        if (p_ctl->dir == CW)
                        {
                            p_ctl->step_position++;                      
                        }
                        else
                        {
                            p_ctl->step_position--;                     
                        }

                        p_ctl->accel_count++;                       

                        p_ctl->new_step_delay = p_ctl->step_delay - (((2 *p_ctl->step_delay) + p_ctl->rest_delay)/(4 * p_ctl->accel_count + 1));

                        p_ctl->rest_delay = ((2 * p_ctl->step_delay)+p_ctl->rest_delay)%(4 * p_ctl->accel_count + 1);        

                        if (p_ctl->step_count >= p_ctl->decel_start)        
                        {
                            p_ctl->accel_count = p_ctl->decel_val;    
                            p_ctl->run_state = DECEL;        
                            get_acc_real=p_ctl->step_count;
                        }
                        else if (p_ctl->new_step_delay <= p_ctl->min_delay) 
                        {
                            p_ctl->last_accel_delay = p_ctl->new_step_delay;    
                            p_ctl->new_step_delay = p_ctl->min_delay;      
                            p_ctl->rest_delay = 0;                            
                            p_ctl->run_state = RUN;                  
                            get_acc_real=p_ctl->step_count;
                        }
                    }
                    break;

                    case RUN:
                    {
                        p_ctl->step_count++;        

                        if (p_ctl->dir == CW)
                        {
                            p_ctl->step_position++;                     
                        }
                        else
                        {
                            p_ctl->step_position--;                      
                        }

                        p_ctl->new_step_delay = p_ctl->min_delay;        

                        if (p_ctl->step_count >= p_ctl->decel_start)        
                        {
                            p_ctl->accel_count = p_ctl->decel_val;    
                            p_ctl->new_step_delay = p_ctl->last_accel_delay;     
                            p_ctl->run_state = DECEL;               
                        }
                    }
                    break;

                    case DECEL:
                    {
                        p_ctl->step_count++;         

                        if (p_ctl->dir == CW)
                        {
                            p_ctl->step_position++;                    
                        }
                        else
                        {
                            p_ctl->step_position--;                
                        }

                        p_ctl->accel_count++;
                        p_ctl->new_step_delay = p_ctl->step_delay - (((2 * p_ctl->step_delay) + p_ctl->rest_delay)/(4 * p_ctl->accel_count + 1)); 
                        p_ctl->rest_delay = ((2 * p_ctl->step_delay)+p_ctl->rest_delay)%(4 * p_ctl->accel_count + 1);                                  

                        if (p_ctl->accel_count >= 0)                  
                        {
                            p_ctl->run_state = STOP;
                        }
                    }
                    break;

                    default:
                    {

                    }
                    break;
                }


                p_ctl->step_delay = p_ctl->new_step_delay;
            }
            else if (IRON_ACTION_MANUAL == p_ctl->action_type)
            {
                p_ctl->step_count++;

                if (p_ctl->step_count >= p_ctl->manual_limit_steps)
                {
                    stepper_ctl_stop(p_ctl);

                    p_ctl->step_count = 0;
                }
            }
            else if (IRON_ACTION_RESET == p_ctl->action_type)
            {
                 p_ctl->step_count++;

                 if (p_ctl->step_count >= p_ctl->manual_limit_steps)
                 {
                     stepper_ctl_stop(p_ctl);

                     p_ctl->step_count = 0;
                 }


                 if (ZERO_MONITOR_LEVEL == rt_pin_read(ZERO_PIN))
                 {
                     stepper_ctl_stop(p_ctl);
                 }
            }
        }
    }
}

3.3.5 測(cè)試

3.3.5.1 測(cè)試生成指定脈沖個(gè)數(shù)

生成10個(gè)脈沖，脈沖寬度為1*0xFFFF/1000000=0.065535S

#define MAX_TIMER_CNT    0xFFFF
TIM_PWM_Init(MAX_TIMER_CNT,TIM_PRESCALER); 
uint32_t set_cnt_pwm = 10;
uint32_t get_acc_real = 0;
uint32_t set_cnt_pwm = 10;
void HAL_TIM_OC_DelayElapsedCallback(TIM_HandleTypeDef *htim)
{
    __IO uint32_t tim_count = 0;
    __IO uint32_t tmp = 0; 
     __IO static uint8_t i = 0;             
    if(htim->Instance==TIM2)
    {
        tim_count = __HAL_TIM_GET_COUNTER(&htimx_STEPMOTOR);
        tmp = tim_count + MAX_TIMER_CNT/2;                
        __HAL_TIM_SET_COMPARE(&htimx_STEPMOTOR,STEPMOTOR_TIM_CHANNELn,tmp);


        i++;                                            
        if(i == 2)                                        
        {
            i = 0;
                if(i == 2)                 
        {
            i = 0;
            set_cnt_pwm--;

            if (0 == set_cnt_pwm)
            {
                  set_cnt_pwm = 10;

                  HAL_TIM_OC_Stop_IT(&htim2,TIM_CHANNEL_2);
            }              
        }      
        }
    }
}

實(shí)測(cè)波形

3.3.5.2 測(cè)試設(shè)置指定參數(shù)

如果一圈的脈沖數(shù)是1600，位移是5*3.14159mm，那么100mm/s就需要10185.9個(gè)脈沖每秒，1000000/10186，那么最大速度時(shí)的計(jì)數(shù)為98，脈沖周期也就是98us左右

#define CONFIG_SPEED_MMPS 100
#define MMPS_TO_RAD_0_1_PS(SPEED) ((SPEED*2*3.14159*10)/5*3.14159)
float set_speed  = MMPS_TO_RAD_0_1_PS(CONFIG_SPEED_MMPS);         
create_t_ctrl_param(CONFIG_STEPS_PER_ROUND*2, 100, set_speed);

實(shí)測(cè)波形

A1-A2的時(shí)間剛好是100ms，脈沖個(gè)數(shù)是516個(gè)，這個(gè)加速階段的步數(shù)比理論值要大，

脈沖周期是100us左右，比理論上要大2us。

3.3.5.3 測(cè)試執(zhí)行連續(xù)的動(dòng)作

假設(shè)需要完成2個(gè)動(dòng)作，每一次的距離是2mm，間隔是200ms，2mm對(duì)應(yīng)的步數(shù)=204

#define ROUND_UP_CNT_GAIN_100                 (50)
#define ROUND_UP(M,N)                        ((((M*100)/N)+ROUND_UP_CNT_GAIN_100)/100)
#define CONFIG_STEPS_PER_ROUND                         (1600)
uint32_t distance_to_steps_mm_gain(uint32_t set_mm_gain, uint32_t set_gain)
{
    uint32_t set_gain_val = CONFIG_ROUND_MM*set_gain;
    return ROUND_UP(set_mm_gain*CONFIG_STEPS_PER_ROUND,set_gain_val);
}


create_t_ctrl_param(distance_to_steps_mm_gain(200,100), 10, set_speed);
        HAL_TIM_OC_Start_IT(&htim2,TIM_CHANNEL_2); 
        while (g_motion_sta)
        {
        }
        delay_ms(200);
create_t_ctrl_param(distance_to_steps_mm_gain(200,100), 10, set_speed);
HAL_TIM_OC_Start_IT(&htim2,TIM_CHANNEL_2);

實(shí)測(cè)波形

4 實(shí)測(cè)效果

正向轉(zhuǎn)4次，每次1圈，速度是10mm/s，反向轉(zhuǎn)1次，轉(zhuǎn)1圈，速度是2mm/s