PID控制小车视觉跟踪移动目标

利用PID控制，使目标图像点移动到图像中心。

目的：视觉跟踪物体。小车能够跟踪目标。利用质心法，求出物体的质心，然后控制（PID）小车运动，使物体的质心在图像平面中心位置。

目标点为实际物体所在位置。图像中心是理想位置。

//m_posX , m_posY为物体质心在像平面的位置。

//(320,240)是图像平面中心。理想位置。

//ex, ey为误差

//初始ex0, ey0为0；

ex=320-m_posX;

ey=240-m_posY;

vx=KP*ex+KD*(ex-ex0);   //得到2个电机的速度，控制。利用电机速度不相等来转弯。

vy=KP*ey+KD*(ey-ey0); 

ex0=ex;

ey0=ey;

//控制,运动控制卡库函数

GT_PrflV(2); //将指定控制轴设置为速度追踪模式

GT_SetVel(2,-vx);//设置指定控制轴的目标速度

GT_SetAcc(2,ACC_1);//设置指定控制轴的加速度       

GT_PrflV(1);

GT_SetVel(1,vy);

GT_SetAcc(1,ACC_1);

GT_MltiUpdt(0x03); //两个轴同时刷新，同时运动