private void Adjust_X(ConPort conPort, UrgPort urgPort, KeyPoint targetPoint)
{
KeyPoint currentPoint = getCurrentKB(urgPort);
while (true)
{
// 比较精度
double current = currentPoint.UrgB;
double target = targetPoint.UrgB;
if (Math.Abs(current - target) < config.xError)
{
return;
}
// 获得输出
double adjustX = (current - target) * 40;
int xSpeed = (int)(adjustX / config.TimeForControl);
if (xSpeed == 0)
{
return;
}
// 限幅
if (xSpeed > 300)
{
xSpeed = 300;
}
if (xSpeed < -300)
{
xSpeed = -300;
}
// 控制
conPort.Control_Move_By_Speed(xSpeed, 0, 0);
// 更新数据
System.Threading.Thread.Sleep(config.TimeForControl);
currentPoint = getCurrentKB(urgPort);
}
}
private void Adjust_A(ConPort conPort, UrgPort urgPort, KeyPoint targetPoint)
{
KeyPoint currentPoint = getCurrentKB(urgPort);
while (true)
{
// 比较精度
double current = currentPoint.UrgK;
double target = targetPoint.UrgK;
if (Math.Abs(current - target) <= config.aError)
{
return;
}
// 获取控制速度
double adjustA = (current - target) * 150;
int aSpeed = (int)(adjustA / config.TimeForControl);
if (aSpeed == 0)
{
return;
}
// 限幅
if (aSpeed > 25)
{
aSpeed = 25;
}
if (aSpeed < -25)
{
aSpeed = -25;
}
// 控制
conPort.Control_Move_By_Speed(0, 0, aSpeed);
// 更新数据
System.Threading.Thread.Sleep(config.TimeForControl);
currentPoint = getCurrentKB(urgPort);
}
}