1 什么是固定时间运动控制算法

Marlin新版本release-2.1.3-beta开始，引入了固定时间运动(Fixed Time Motion)步进驱动框架替代原来一直沿用的变步长步进驱动框架。变步长驱动是在要发一个脉冲的时候，计算这个脉冲的速度，根据这个速度调整定时器的周期。FTM驱动方案是按照固定的时间周期（典型的比如5ms）将一段直线运动分段，比如一段直线运动用时2s，那就可以分成400段EVT，对于每一段计算这一段内电机要走的脉冲步数，然后交给定时器中断服务程序ISR去发波。ISR的周期也是固定的，比如10us一个周期，利用bresemham算法计算出这个周期要发波了，然后ISR就发一个脉冲出来。

可以看到FTM相对旧的变步长驱动方案的优点：

（1）FTM的EVT都是在主程序里面计算出来的，中断ISR只负责取出EVT并发波，中断服务程序负担很轻，所以支持更高的脉冲频率。

（2）更精准的脉冲定时和轨迹控制。预先计算脉冲频率，中断负担轻，避免了变步长中断延时导致的脉冲抖动，运动更加平稳。

（3）FTM能够很好的兼容输入整形(ZV,ZVD等），可以更好的抑制机械振动。

2 FTM固定时间运动控制算法实现

上面已经说过了FTM的运动实现思路，这个固定时间包括来个固定，一个是粗分段周期EVT_Ts，一个是发波中断周期ISR_Ts，EVT_Ts典型的可以取5ms，IST_Ts可以取10us。一段直线运动，先规划出它运动总时间Tm,然后可以分出Tm/EVT_Ts个运动段EVT，计算出每个EVT要走的步数，然后在ISR中断里面去发波。

下面是XY双轴走一条斜线的FTM算法的简单实现。

//EVT结构体
typedef struct {
    u64 rate[NUM_AXIS];
    u32 steps[NUM_AXIS];
    s32 remain[NUM_AXIS];
    u8 direction_bits;
    u8 signZ;
    u32 block_cnt;
} FtmEvt_S;

FtmEvt_S ftmEvtArr[EVT_BUFF_SIZE];

float isr_time = 0.00001f;
s32 denominator = 1<<16;
u8 moveFlag = 0;

//FTM运动规划，放到主循环中
void ftm_plan_line_xy(float dx,float dy,float speed)
{
    if(dx>0)
    {
        SET_X_DIR(1);
    }
    else
    {
        dx = -dx;
        SET_X_DIR(0);
    }

    if(dy>0)
    {
        SET_Y_DIR(1);
    }
    else
    {
        dy = -dy;
        SET_Y_DIR(0);
    }

    float len = sqrt(dx*dx+dy*dy);
    float ratioX = (float)dx/len;
    float ratioY = (float)dy/len;

    float acc_len = speed*speed/(2*move_acc);
    float dec_len = acc_len;
    float cruise_len = len-acc_len-dec_len;
    float move_max_rate = speed;

    if(cruise_len < 0)
    {
        acc_len = len/2;
        move_max_rate = sqrt(2*move_acc*acc_len);
        cruise_len = 0;
    }

    acc_time = move_max_rate/move_acc;
    dec_time = acc_time;
    cruise_time = cruise_len/move_max_rate;
    move_time = acc_time+dec_time+cruise_time;

    float curr_time = 0;
    float remiderX = 0;
    float remiderY = 0;

    while(curr_time < move_time)
    {
        float t_start = curr_time;
        float t_end = t_start+move_ts;
        if(t_end>move_time)
            t_end = move_time;
        float dt=t_end-t_start;

        float v_start,v_end;
        if(t_start < acc_time)
            v_start = move_acc*t_start;
        else if(t_start < acc_time+cruise_time)
            v_start = move_max_rate;
        else 
            v_start = move_max_rate-move_acc*(t_start-acc_time-cruise_time);

        if(t_end < acc_time)
            v_end = move_acc*t_end;
        else if(t_end < acc_time+cruise_time)
            v_end = move_max_rate;
        else 
            v_end = move_max_rate-move_acc*(t_end-acc_time-cruise_time);

        float v_avg = (v_start+v_end)/2;
        float delta_len = v_avg*dt;
        float delta_x = delta_len*ratioX;
        float delta_y = delta_len*ratioY;
        
        float delta_steps_fra_x = delta_x*X_SCALE+remiderX;
        long delta_steps_x = floor(delta_steps_fra_x);
        remiderX = delta_steps_fra_x - delta_steps_x;

        float delta_steps_fra_y = delta_y*Y_SCALE+remiderY;
        long delta_steps_y = floor(delta_steps_fra_y);
        remiderY = delta_steps_fra_y - delta_steps_y;
        
        int num_isr_in_ts = dt/isr_time;
        u64 rate_x = ((u64)delta_steps_x<<16)/num_isr_in_ts;
        ftmEvtArr[ftm_index_head].rate[X_AXIS] = rate_x;
        ftmEvtArr[ftm_index_head].steps[X_AXIS] = delta_steps_x;
        ftmEvtArr[ftm_index_head].remain[X_AXIS] = delta_steps_x;

        u64 rate_y = ((u64)delta_steps_y<<16)/num_isr_in_ts;
        ftmEvtArr[ftm_index_head].rate[Y_AXIS] = rate_y;
        ftmEvtArr[ftm_index_head].steps[Y_AXIS] = delta_steps_y;
        ftmEvtArr[ftm_index_head].remain[Y_AXIS] = delta_steps_y;

        ftm_index_head++;
        curr_time += move_ts;
    }

    moveFlag = 1;
    ftm_run_time = 0;
    ftm_run_cnt = 0;
    SET_DRIVER_ENABLE();
    Timer7Enable(ENABLE);
    while(moveFlag){}
    Timer7Enable(DISABLE);
    SET_DRIVER_DISABLE();

}

//中断发波函数，放到TIM7定时中断函数中
void FTM_ISR_proc(void)
{
    if(curr_ftm_block_xy == NULL)
    {
        if(ftm_index_tail < ftm_index_head)
        {
            curr_ftm_block_xy = &ftmEvtArr[ftm_index_tail++];
        }
        else
        {
            moveFlag = 0;
        }
    }

    if(curr_ftm_block_xy != NULL)
    {
        accumulator_x += curr_ftm_block_xy->rate[X_AXIS];
        if(accumulator_x >= denominator)
        {
            SET_X_PULSE(1);
            FTM_Delay(2);
            SET_X_PULSE(0);
            accumulator_x -= denominator;
            curr_ftm_block_xy->remain[X_AXIS]--;
                
        }

        accumulator_y += curr_ftm_block_xy->rate[Y_AXIS];
        if(accumulator_y >= denominator)
        {
            SET_Y_PULSE(1);
            FTM_Delay(2);
            SET_Y_PULSE(0);
            accumulator_y -= denominator;
            curr_ftm_block_xy->remain[Y_AXIS]--;
        }

        if(curr_ftm_block_xy->remain[X_AXIS] <= 0 && curr_ftm_block_xy->remain[Y_AXIS] <= 0)
        {
            curr_ftm_block_xy = NULL;
        }   
    }

    ftm_run_time += isr_time;
    ftm_run_cnt++;
}

传统变步长画圆的效果

FTM算法画圆效果