|
re
主要是基于DDS频率步进值的线性变加速处理,因为用的DDS的相位累加器是32bit的,所以可以做到很小的步进量。
然后配合电机状态和当前输入加速度,构建一个状态机,自动调整电机转速(DDS输出脉频)和转向
核心代码如下:A_INPUT是输入的加速度值,就是PID的结果;Fout为步进电机驱动频率;
while(1)
{
CA_INPUT=A_INPUT;
dF=A_to_dF(CA_INPUT);
if(CA_INPUT>0 && Fout> Fmin && DIR==1)statue=1;
if(CA_INPUT>0 && Fout> Fmin && DIR==0)statue=2;
if(CA_INPUT>0 && Fout<=Fmin && DIR==1)statue=3;
if(CA_INPUT>0 && Fout<=Fmin && DIR==0)statue=4;
if(CA_INPUT<0 && Fout> Fmin && DIR==1)statue=5;
if(CA_INPUT<0 && Fout> Fmin && DIR==0)statue=6;
if(CA_INPUT<0 && Fout<=Fmin && DIR==1)statue=7;
if(CA_INPUT<0 && Fout<=Fmin && DIR==0)statue=8;
if(CA_INPUT==0) statue=9;
switch(statue)
{
case 1:{CDIR=1;Fout+=dF;break;}
case 2:{CDIR=0;Fout-=dF;break;}
case 3:{CDIR=1;Fout+=dF;break;}
case 4:{CDIR=1;Fout+=dF;break;}
case 5:{CDIR=1;Fout-=dF;break;}
case 6:{CDIR=0;Fout+=dF;break;}
case 7:{CDIR=0;Fout+=dF;break;}
case 8:{CDIR=0;Fout+=dF;break;}
case 9: break;
default:break;
}
DIR=CDIR;
if(Fout>Fmax)Fout=0;
if(Fout
switch(CDIR)
{
case 0:{CNT_DIR_0;break;}
case 1:{CNT_DIR_1;break;}
default:break;
}
motor_ctr(CDIR,Fout);
CNT=read_cnt();
CNTS=(s32)CNT;
} |
|