|
我本人是从事单片机程序开发的,以前从来没有接触过运动控制这一块领域。目前,公司有一个项目要用到伺服电机,驱动器的电路和代码都有,领导让我从代码里 研究一下驱动器的算法,看了几天,看到头晕晕的,还没有一个大概的概念。被一个函数卡了好几天还是没想明白,我贴一下代码出来,希望多多指教。
/**
* @param dis: the distant to move.
* @param t: the given time to move.
* @param acc: accerlation
* @retval 0 for no error. 1 for time to short or acc to small;
*/
u8 setTPath(int64_t dis,int64_t t,int acc)
{
int64_t temp,t_destin,treferencepos;
int dec;
vel=AV*10;//AV为两次电机转过的角度差(脉冲数量表示)
treferencepos=getPosition();//电机实际转过的角度,也是脉冲数量表示
t_destin=dis;
dis=1000*(dis-treferencepos);
t=10*t;
if(vel>0)
{
dec=-acc;
if(vel*t-vel*vel/acc/2>dis)
{
if(vel*vel/acc/2>dis)
dec=acc;
acc=-acc;
}
}
else
{
dec=acc;
if(vel*t+vel*vel/acc/2>dis)
{
acc=-acc;
}
else
{
if(vel*vel/acc/2<dis)
dec=-acc;
}
}
if(dec/acc==-1)
{
temp=t*t+2*vel*t/acc-4*dis/acc+3*vel*vel/acc/acc;
if(temp<0)
return 1;
temp=sqrt(temp);
acctime=(t-vel/acc-temp)/2;
dectime=t-(acctime+vel/acc);
if(acctime>0&&dectime>acctime&&dectime<t)
;
else
{
acctime=(t-vel/acc+temp)/2;
dectime=t-(acctime+vel/acc);
if(acctime>0&&dectime>acctime&&dectime<t)
;
else
return 2;
}
}
else
{
acctime=(dis-vel*t-vel*vel/2/acc)/(vel+acc*t);
dectime=t+vel/acc+acctime;
}
destin=t_destin;
accelerate=acc;
decelerate=dec;
maxvel=vel+acc*acctime;
endtime=t;
distant=dis;
referencepos=treferencepos;
return 0;
}
该算法的核心应该就是位置控制算法,代码中的公式temp=t*t+2*vel*t/acc-4*dis/acc+3*vel*vel/acc/acc;代表什么意义,怎样推导出来的,算来算去都不行,梯形算法还是S型算法我都推导不出来,麻烦了解的大侠给我指导一下 |
阿莫论坛20周年了!感谢大家的支持与爱护!!
曾经有一段真挚的爱情摆在我的面前,我没有珍惜,现在想起来,还好我没有珍惜……
|