/////////////////////////////////////////////// public method ////////////////////////////////////////////
public KeyPoint getCurrentKB(UrgPort urgPort)
{
KeyPoint keyPoint = new KeyPoint();
keyPoint.Can_Adj = 0;
if (!urgPort.IsOpen)
{
return(keyPoint);
}
// 取得数据
getUrgPoint(urgPort);
// 分割成组
config.urgGroups = new List <List <URG_POINT> >();
CutGroup_UrgPoint(config.pointsH);
// 挑选出正前方数据，进行拟合
List <URG_POINT> linePoints = GetHeadGroup_UrgPoint();
if (linePoints.Count <= 3)
{
return(keyPoint);
}
double[] KB = Fit_UrgPoint(linePoints);
keyPoint.UrgK = KB[0];
keyPoint.UrgB = KB[1];
if (keyPoint.UrgB < 1200)
{
keyPoint.Can_Adj = 1;
}
return(keyPoint);
}
public static UrgPort getInstance()
{
if (urgPort == null)
{
urgPort = new UrgPort();
}
return(urgPort);
}
/// <summary>
/// 初始化串口信息
/// </summary>
public static void initPort()
{
conPort = ConPort.getInstance();
drPort = DrPort.getInstance();
urgPort = UrgPort.getInstance();
ports = new IPort[] {
conPort, drPort, urgPort
};
}
/////////////////////////////////////////////// private attribute ////////////////////////////////////////////
private bool getUrgPoint(UrgPort urgPort)
{
config.pointsH = new List <URG_POINT>();
if (!urgPort.IsOpen)
{
return(false);
}
UrgModel urgModel = urgPort.getUrgData();
if (urgModel.Distance == null)
{
return(false);
}
int BG = (int)((75 - urgPort.AngleStart) / urgPort.AnglePace);
int ED = (int)((105 - urgPort.AngleStart) / urgPort.AnglePace);
if (urgModel.Distance.Count < ED)
{
return(false);
}
config.pointsH = new List <URG_POINT>();
for (int i = BG; i < ED; i++)
{
if (urgModel.Distance[i] == 0)
{
continue;
}
URG_POINT point = new URG_POINT();
point.d = urgModel.Distance[i];
double angle = urgPort.AngleStart + i * urgPort.AnglePace;
point.a = angle;
point.x = point.d * Math.Cos(angle * Math.PI / 180);
point.y = point.d * Math.Sin(angle * Math.PI / 180);
config.pointsH.Add(point);
}
return(true);
}
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);
}
}
