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); } }