小车站立，测速代码问题

FOR628655

电磁的速度控制能不能消除站立时的抖动问题呢？调PD还是在抖，但也能站很久···对于速度控制，不知道这段代码有什么问题么？请高手指教。
err_chesu=3.35*shu;        //shu为脉冲个数，车速定义为车轮每秒转的圈数 3.35=1000/300 1ms=1/1000秒 300线码盘···
Last_err_chesu=err_chesu-err_chesu_1;
  fP=err_chesu*0.01;
  fI=err_chesu_1*0;
  fD=Last_err_chesu*0.02;
  err_chesu=err_chesu_1;
  Mspeed=fP+fD;