步进电机--S 曲线的C算法

最新推荐文章于 2024-02-05 23:51:42 发布

brantyu123

最新推荐文章于 2024-02-05 23:51:42 发布

阅读量5.5k

点赞数 4

分类专栏：电机控制类

本文链接：https://blog.csdn.net/brantyu123/article/details/52042009

版权

电机控制类专栏收录该内容

1 篇文章 0 订阅

订阅专栏

S 曲线生成函数

static void Calc_S_Curve(S_MOTOR *pmotor,u8 model)
{
/* 
S型 曲线生成 公式

1.变量说明
Jmax: 最大加加速度。
Amax:最大的加速度。
Fstart:启动速度
T:阶段时间（三个阶段时间相等）

2.基本换算公式
Jmax=Amax/T
Faa=Fstart+0.5*Jmax*t*t
Fua=Faa+Amax*t
Fra=Fmax-0.5*Jmax*(T-t)*(T-t) 

2.已知 Fstart，T，Fmax 计算其他分量

Fra(min)=Fua(max)
Fmax-0.5*Jmax*T*T=Amax*T+Fstart+0.5*Jmax*T*T
Fmax-0.5*Amax*T=Amax*T+Fstart+0.5*Amax*T

Amax=(Fmax-Fstart)/(2T)
JMAX=Amax/T=(Fmax-Fstart)/(2T*T)

*/

float f;
u32 i;
float   STEP_FACTOR;
float Amax;
float T=pmotor->s.T;
float Jmax;
float Fstart =pmotor->s.Fstart;
float Fmax   =pmotor->s.Fmax;
float val;


STEP_FACTOR=DEFAULT_STEP_FACTOR;    


/* 已知 Fstart，T，Fmax 计算其他分量 */

if(model==3)//3/3
{

}
else if(model=2)//2/3
{
T=T*2/3;
Fmax=Fmax*2/3;
}
else//1/3
{
 T=T*1/3;
 Fmax=Fmax*1/3;
}


Amax=(Fmax-Fstart)/(2*T);
Jmax=Amax/T;
pmotor->s.Amax=Amax;
pmotor->s.Jmax=Jmax;    




   /*  加加速度阶段 */
    for(i=0;i<DIVERGENCE_CNT;i++)
    {
        f=Fstart+0.5*Jmax*(i*T/DIVERGENCE_CNT)*(i*T/DIVERGENCE_CNT);
        pmotor->s.atime[i]=FREQ_2_TIME/f;
        pmotor->s.astep[i]=f*(T/DIVERGENCE_CNT)/STEP_FACTOR;
    }

   /* 恒速加速阶段 */
    Fstart=f;
      for(i=0;i<DIVERGENCE_CNT;i++)
    {
        f=Fstart+Amax*(i*T/DIVERGENCE_CNT);
        pmotor->s.atime[i+DIVERGENCE_CNT]=FREQ_2_TIME/f;
        pmotor->s.astep[i+DIVERGENCE_CNT]=f*(T/DIVERGENCE_CNT)/STEP_FACTOR;
    }
    /* 减加速阶段 */
     for(i=0;i<DIVERGENCE_CNT;i++)
    {
        f=Fmax-0.5*Jmax*(T-i*T/DIVERGENCE_CNT)*(T-i*T/DIVERGENCE_CNT);
        pmotor->s.atime[i+DIVERGENCE_CNT*2]=FREQ_2_TIME/f;
        pmotor->s.astep[i+DIVERGENCE_CNT*2]=f*(T/DIVERGENCE_CNT)/STEP_FACTOR;
    }

    //计算 S曲线形成需要的最少步数,上升和下降 完全对称
    pmotor->s.s_steps=0;
    for(i=0;i<(DIVERGENCE_CNT*3);i++)
    {
    pmotor->s.s_steps+=pmotor->s.astep[i];
    }

}