飞思直立,方向控制到底是咋样啊!
void Dir_Control()
{
int lnDeltaValue;
int DirErr,DirErr_his;
int nP,nD;
DirErr = 0;
DirErr_his = 0;
//-------------------------------
DirErr_his = DirErr;
DirErr = (int)CENTER - center; // 正-->左转 负-->右转
//-------------------------------
nP = DirErr;
nP = (int)(nP * CAR_DIR_P);
nD = DirErr - DirErr_his;
nD = (int)(nD * CAR_DIR_D);
Dir_Speed = (int)(nP + nD);
}
|