public static ConPort getInstance()
{
if (conPort == null)
{
conPort = new ConPort();
}
return(conPort);
}
/// <summary>
/// 初始化串口信息
/// </summary>
public static void initPort()
{
conPort = ConPort.getInstance();
drPort = DrPort.getInstance();
urgPort = UrgPort.getInstance();
ports = new IPort[] {
conPort, drPort, urgPort
};
}
public void Start(ConPort conPort, UrgPort urgPort, KeyPoint keyPoint)
{
while (getCurrentKB(urgPort).UrgB == 0)
{
;
}
config.TimeForControl = 100;
config.xError = 20;
config.aError = 2;
Adjust_A(conPort, urgPort, keyPoint);
Adjust_X(conPort, urgPort, keyPoint);
config.xError = 5;
config.aError = 0.5;
Adjust_A(conPort, urgPort, keyPoint);
Adjust_X(conPort, urgPort, keyPoint);
}
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);
}
}
