|
本人小白一个,如果有错误的地方请大侠更正下。由于我电机板子还没搞好,以前一块别人的板子上 脉冲管脚是接中断的,没接在计数器上,所以一直没能调试好。 我是用2个码盘电机做小车的。个人觉得最好上UCOS2,因为2个电机那实时性能要保证。加上我是2电机+4舵机的控制 所以。。。。我是上UCOS2的。
typedef struct PID
{
// signed int vi_Ref; // 设置的速度 这两个参数由于是动态变化的所以。。。
// signed int vi_FeedBack; // 传感器读回来的速度 用形式参数
long int vi_PreError; // E-1 error
long int vi_PreDerror; // E-2 error
unsigned int v_Kp; // P
unsigned int v_Ki; // I three parameter
unsigned int v_Kd; // D
long int vl_PreU; //所得电压
}PID;
PID sPID1[2];
PID *pp1 =&sPID1[0];
PID *pp2 =&sPID1[1]; // PID Control Structure
long int v_PIDCalc1( signed short set_point,signed short next_point)
{
signed long error,d_error,dd_error;
error = (signed long)(set_point - next_point);
d_error = error - pp1->vi_PreError;
dd_error = d_error - pp1->vi_PreDerror;
pp1->vi_PreError = error; pp1->vi_PreDerror = d_error;
if( ( error < VV_DEADLINE ) && ( error > -VV_DEADLINE ) );
else
{
pp1->vl_PreU +=(signed long)( pp1 -> v_Kp * d_error + pp1 -> v_Ki * error + pp1->v_Kd*dd_error);
}
if( pp1->vl_PreU >= VV_MAX )
{
pp1->vl_PreU = VV_MAX;
}
else if( pp1->vl_PreU <= VV_MIN )
{
pp1->vl_PreU = 0;
}
else if(error>20)
{
pp1->vl_PreU=VV_MAX;
}
return (pp1->vl_PreU);
}
long int v_PIDCalc2( signed short set_point,signed short next_point)
{
signed long error,d_error,dd_error;
error = (signed long)(set_point - next_point);
d_error = error - pp2->vi_PreError;
dd_error = d_error - pp2->vi_PreDerror;
pp2->vi_PreError = error; pp2->vi_PreDerror = d_error;
if( ( error < VV_DEADLINE ) && ( error > -VV_DEADLINE ) );
else
{
pp2->vl_PreU +=(signed long)( pp2 -> v_Kp * d_error + pp2 -> v_Ki * error + pp2->v_Kd*dd_error);
}
if( pp2->vl_PreU >= VV_MAX )
{
pp2->vl_PreU = VV_MAX;
}
else if( pp2->vl_PreU <= VV_MIN )
{
pp2->vl_PreU = 0;
}
else if(error>20)
{
pp2->vl_PreU=VV_MAX;
}
return (pp2->vl_PreU);
}
void PIDInit ()
{
// sPID1.vi_Ref = 0 ;
// sPID1.vi_FeedBack = 0 ;
pp1->vi_PreError = 0 ;
pp1->vi_PreDerror = 0 ;
pp1->v_Kp = VV_KPVALUE;
pp1->v_Ki = VV_KIVALUE;
pp1->v_Kd = VV_KDVALUE;
pp1->vl_PreU = 0;
pp2->vi_PreError = 0 ;
pp2->vi_PreDerror = 0 ;
pp2->v_Kp = VV_KPVALUE;
pp2->v_Ki = VV_KIVALUE;
pp2->v_Kd = VV_KDVALUE;
pp2->vl_PreU = 0;
}
起初我调试PID时 思路是这么想的,用定数器采集码盘脉冲的数量,实现测速的功能。同时开启定时器的中断设定周期,每隔一定的周期产生一次中断去读取脉冲数,然后将此脉冲数清零。此时测出的速度值与你设定的速度值想减,经过PID算法。然后输出速度值,输出的速度一路用于反馈,一路用于输出相应的PWM值。。。。。。 |
|