步進電機的基本參數及梯形加減速算法

1 背景

在2021年接觸到步進電機，當時是用來驅動熱敏打印機，沒有用到加減速算法，速度時間表好像是日本客戶那邊提供過來的，這次調試加減速算法，遇到了不少問題，在這里記錄一下，希望能幫到未來對此有困惑的自己，如果能幫到其他人也算是有幸。

2 步進電機基本參數

2.1 驅動器

2.2 電機

2.3 接線

2.4 細分及電流配置

2.5 驅動方式

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

3 梯形加減速算法

3.1 推導過程

3.1.1 組成部分

步進電機的動作過程是由加速、勻速、減速三個部分組成，轉速的單位是rpm，每分鐘多少轉，最終需要rpm轉換成每秒的脈沖數也就是頻率，速度越快頻率越大，對應的脈沖周期T就越小。

3.1.2 速度和脈沖周期的關系

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

3.1.3 加速度和距離的關系

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

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

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

tn=cn*tt(計數時間)

3.1.4 下一個脈沖計數

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)

根據麥克勞林公式

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

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

最終化簡后得到

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

3.1.5 誤差和放大倍數

當n=1時有0.4485的偏差,將C0乘以0.676來解決這個誤差

假設主頻為72M預分頻系數為72-1，那么

C0 = 10000000.676√(2alpha/a)

為了保證計算的精度，需要對相關變量進行放大處理，這里對加速度放大10倍

C0=10000000.676 √(2alpha/a10)

那么對2*alpha乘以100000，

根號中2alpha100000/a10=2alpha*10000/a

100*√(2*alpha/a)

對1000000*0.676除以10倍，然后再對整個結果除以10，那么

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

3.1.6 相關宏定義

#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 單位轉換

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

已知公式角速度=轉速*2π/60

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

3.2.1 單位轉換相關宏定義

假設軸直徑是5mm，默認情況下1轉的距離是(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 加減速算法初始化

輸入參數是總步數、加減速時間ms、最大速度mm/s

__IO uint16_t tim_count;        /* 達到最大速度時的步數*/
    __IO uint32_t max_s_lim;        /* 必須要開始減速的步數（如果加速沒有達到最大速度）*/
    __IO uint32_t accel_lim;

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

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

  __IO int32_t decel = accel;


    if(step < 0)                    /* 步數為負數 */
    {   
        g_srd.dir = CCW;            /* 逆時針方向旋轉 */ 
        step = -step;               /* 獲取步數絕對值 */
    }
    else
    {
        g_srd.dir = CW;             /* 順時針方向旋轉 */ 
    }


    if(step == 1)                   
    {
        g_srd.accel_count = -1;     
        g_srd.run_state = DECEL;     
        g_srd.step_delay = 1000;     
    }
    else if(step != 0)              /* 如果目標運動步數不為0*/
    { 
        g_srd.min_delay = (int32_t)(A_T_x10 /speed); //勻速運行時的計數值 

        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));/* 計算多少步之后達到最大速度的限制 max_s_lim = speed^2 / (2*alpha*accel) */


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


        if(accel_lim == 0)                                      /* 不足一步 按一步處理*/
        {
            accel_lim = 1;
        }
        if(accel_lim <= max_s_lim)                              /* 加速階段到不了最大速度就得減速。。。使用限制條件我們可以計算出減速階段步數 */
        {
            g_srd.decel_val = accel_lim - step;                 /* 減速段的步數 */
        }
        else
        {
            g_srd.decel_val = -(max_s_lim*accel/decel);         /* 減速段的步數 */
        }
        if(g_srd.decel_val == 0)                                /* 不足一步 按一步處理 */
        {
            g_srd.decel_val = -1;
        }
        g_srd.decel_start = step + g_srd.decel_val;             /* 計算開始減速時的步數 */
        
        if(g_srd.step_delay <= g_srd.min_delay)                 /* 如果一開始c0的速度比勻速段速度還大，就不需要進行加速運動，直接進入勻速 */
        {
            g_srd.step_delay = g_srd.min_delay;
            g_srd.run_state = RUN;
        }
        else  
        {
            g_srd.run_state = ACCEL;
        }
        g_srd.accel_count = 0;                                  /* 復位加減速計數值 */
    }
    g_motion_sta = 1;                                           /* 電機為運動狀態(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);  /* 設置定時器比較值 */

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 測試

3.3.5.1 測試生成指定脈沖個數

生成10個脈沖，脈沖寬度為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);
            }              
        }      
        }
    }
}

實測波形

3.3.5.2 測試設置指定參數

如果一圈的脈沖數是1600，位移是5*3.14159mm，那么100mm/s就需要10185.9個脈沖每秒，1000000/10186，那么最大速度時的計數為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);

實測波形

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

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

3.3.5.3 測試執(zhí)行連續(xù)的動作

假設需要完成2個動作，每一次的距離是2mm，間隔是200ms，2mm對應的步數=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);

實測波形

4 實測效果

正向轉4次，每次1圈，速度是10mm/s，反向轉1次，轉1圈，速度是2mm/s