|
|
我本人是从事单片机程序开发的，以前从来没有接触过运动控制这一块领域。目前，公司有一个项目要用到伺服电机，驱动器的电路和代码都有，领导让我从代码里 研究一下驱动器的算法，看了几天，看到头晕晕的，还没有一个大概的概念。被一个函数卡了好几天还是没想明白，我贴一下代码出来，希望多多指教。
/**
* @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周年了！感谢大家的支持与爱护！！
曾经有一段真挚的爱情摆在我的面前，我没有珍惜，现在想起来，还好我没有珍惜……
|
[复制链接]
发表于 2016-9-19 18:03:57
是DSP 程序么？？？
楼主
点赞一个，厉害
