private static List <CONFIG.URG_POINT> SortPoints(List <CONFIG.URG_POINT> points)
{
// 点的数量不够，直接返回。
if (points.Count <= 3)
{
return(points);
}
// 距离排序
for (int i = 1; i < points.Count; i++)
{
for (int j = 0; j < points.Count - i; j++)
{
if (points[j].y <= points[j + 1].y)
{
continue;
}
CONFIG.URG_POINT temp = new CONFIG.URG_POINT();
temp = points[j];
points[j] = points[j + 1];
points[j + 1] = temp;
}
}
// 选取距离
int indexofcut = points.Count;
for (int i = 0; i < points.Count - 1; i++)
{
if (points[i + 1].y - points[i].y > 200)
{
indexofcut = i + 1; break;
}
}
points.RemoveRange(indexofcut, points.Count - indexofcut);
// 距离跨度要求
double xMax = double.MinValue, xMin = double.MaxValue;
for (int i = 0; i < points.Count; i++)
{
double x = points[i].x;
if (x > xMax)
{
xMax = x;
}
if (x < xMin)
{
xMin = x;
}
}
if (xMax - xMin < 200)
{
return(new List <CONFIG.URG_POINT>());
}
return(points);
}
private int getForwardSpeed(int keepSpeed, KeyPoint targetPoint, IUrgPort urgPort, IDrPort drPort)
{
// 取点
List <CONFIG.URG_POINT> pointsH = getUrgPoint(85, 95, urgPort);
// 数量不够
if (pointsH.Count == 0)
{
return(keepSpeed);
}
// 换成绝对距离
for (int i = 0; i < pointsH.Count; i++)
{
CONFIG.URG_POINT point = pointsH[i]; point.y = Math.Abs(point.y); pointsH[i] = point;
}
// 距离限速
double minH = double.MaxValue;
for (int i = 0; i < pointsH.Count; i++)
{
if (minH > pointsH[i].y)
{
minH = pointsH[i].y;
}
}
if (minH < 200)
{
config.AchieveTarget = true; return(0);
}
// 退出条件有变
//double current = drPort.getPosition().y * 1000;
//double target = targetPoint.y * 1000;
//if (Math.Abs(current - target) < 10) { config.AchieveTarget = true; }
//int ForwardSpeed = (int)PDcontroller(current, target, ref config.PD_F);
double current = minH;
double target = 330;
if (Math.Abs(current - target) < 20)
{
config.AchieveTarget = true; return(0);
}
int ForwardSpeed = -(int)PDcontroller(current, target, ref config.PD_F);
// 限速
if (ForwardSpeed > config.MaxForwardSpeed)
{
return(config.MaxForwardSpeed);
}
if (ForwardSpeed < -config.MaxForwardSpeed)
{
return(-config.MaxForwardSpeed);
}
return(ForwardSpeed);
}
private List <CONFIG.URG_POINT> getUrgPoint(double angleBG, double angleED)
{
List <CONFIG.URG_POINT> points = new List <CONFIG.URG_POINT>();
if (!PortManager.urgPort.IsOpen)
{
return(points);
}
UrgModel urgModel = PortManager.urgPort.getUrgData();
while (urgModel.Distance == null || urgModel.Distance.Count == 0)
{
urgModel = PortManager.urgPort.getUrgData();
}
int BG = (int)((angleBG - -30) / (360.0 / 1024.0));
int ED = (int)((angleED - -30) / (360.0 / 1024.0));
if (urgModel.Distance.Count < ED)
{
return(points);
}
for (int i = BG; i < ED; i++)
{
if (urgModel.Distance[i] == 0)
{
continue;
}
CONFIG.URG_POINT point = new CONFIG.URG_POINT();
point.d = urgModel.Distance[i];
double angle = -30.0 + i * 360.0 / 1024.0;
point.a = angle;
point.x = point.d * Math.Cos(angle * Math.PI / 180);
point.y = point.d * Math.Sin(angle * Math.PI / 180);
points.Add(point);
}
return(points);
}
private int getRotateSpeed(IConPort conPort, IUrgPort urgPort, IDrPort drPort)
{
List <CONFIG.URG_POINT> pointsH = getUrgPoint(85, 95, urgPort);
double minH = double.MaxValue;
for (int i = 0; i < pointsH.Count; i++)
{
double y = pointsH[i].y;
if (y < minH)
{
minH = y;
}
}
if (minH < 1000)
{
return(0);
}
// 取点
List <CONFIG.URG_POINT> pointsL = getUrgPoint(120, 180, urgPort);
List <CONFIG.URG_POINT> pointsR = getUrgPoint(0, 60, urgPort);
// 交换并取绝对坐标
for (int i = 0; i < pointsL.Count; i++)
{
CONFIG.URG_POINT point = pointsL[i];
double tempx = Math.Abs(point.x);
double tempy = Math.Abs(point.y);
point.x = tempy; point.y = tempx; pointsL[i] = point;
}
for (int i = 0; i < pointsR.Count; i++)
{
CONFIG.URG_POINT point = pointsR[i];
double tempx = Math.Abs(point.x);
double tempy = Math.Abs(point.y);
point.x = tempy; point.y = tempx; pointsR[i] = point;
}
// 一米之内
for (int i = pointsL.Count - 1; i >= 0; i--)
{
if (pointsL[i].y > 1000)
{
pointsL.RemoveAt(i);
}
}
for (int i = pointsR.Count - 1; i >= 0; i--)
{
if (pointsR[i].y > 1000)
{
pointsR.RemoveAt(i);
}
}
// 拟合左右两边障碍物信息
pointsL = SortPoints(pointsL);
pointsR = SortPoints(pointsR);
pointsL = getFitPoints(pointsL);
pointsR = getFitPoints(pointsR);
double[] KAB_L = getFitLine(pointsL);
double[] KAB_R = getFitLine(pointsR);
// 点数量不够
bool acceptL = pointsL.Count > 10;
bool acceptR = pointsR.Count > 10;
if (!acceptL && !acceptR)
{
return(0);
}
// 控制策略
int RotateSpeed = 0;
if (acceptL && acceptR)
{
double current = 0;
double target = (KAB_L[1] - KAB_R[1]) / 2;
RotateSpeed = (int)PDcontroller(current, target, ref config.PD_R);
}
if (acceptL && !acceptR)
{
double current = 0;
double target = KAB_L[1];
RotateSpeed = (int)PDcontroller(current, target, ref config.PD_R);
}
if (!acceptL && acceptR)
{
double current = 0;
double target = KAB_R[1];
RotateSpeed = -(int)PDcontroller(current, target, ref config.PD_R);
}
// 判断是否允许旋转
double permitRotateDistance = 100; //100
SonicModel sonic = conPort.Measure_Sonic();
//foreach (int s in sonic.S) { if (s < permitRotateDistance) { return 0; } }
// 限速
if (RotateSpeed > config.MaxRotateSpeed)
{
return(config.MaxRotateSpeed);
}
if (RotateSpeed < -config.MaxRotateSpeed)
{
return(-config.MaxRotateSpeed);
}
return(RotateSpeed);
}
/// <summary>
/// 获取平移速度
/// </summary>
/// <param name="keepSpeed">维持该速度去寻找通道口</param>
/// <param name="Finished">寻找完成</param>
/// <returns></returns>
private int getTranslateSpeed(int keepSpeed, ref bool Finished)
{
// 取点
List <CONFIG.URG_POINT> pointsH = getUrgPoint(45, 135);
// 去掉 Y 方向跨度过大的点
double acceptDis = 1500;
for (int i = pointsH.Count - 1; i >= 0; i--)
{
if (pointsH[i].d > acceptDis)
{
pointsH.RemoveAt(i);
}
}
// 点数量不够
if (pointsH.Count <= 3)
{
return(0);
}
// 对 X 坐标从小到大排序
for (int i = 1; i < pointsH.Count; i++)
{
for (int j = 0; j < pointsH.Count - i; j++)
{
if (pointsH[j].x <= pointsH[j + 1].x)
{
continue;
}
CONFIG.URG_POINT point = pointsH[j];
pointsH[j] = pointsH[j + 1];
pointsH[j + 1] = point;
}
}
// 获取最大间隙
double MaxGap = double.MinValue;
int indexL = 0, indexR = 0;
for (int i = 0; i < pointsH.Count - 1; i++)
{
double gap = Math.Abs(pointsH[i].x - pointsH[i + 1].x);
if (gap > MaxGap)
{
MaxGap = gap; indexL = i; indexR = i + 1;
}
}
double disL = Math.Abs(pointsH[indexL].x);
double disR = Math.Abs(pointsH[indexR].x);
// 判断间隙是否足够大
if (MaxGap < 500)
{
return(keepSpeed);
}
// 是否已经满足退出条件
double current = disL;
double target = (disL + disR) / 2;
if (Math.Abs(current - target) < 20)
{
Finished = true;
}
// 获取控制
double Kp = 0.5;
int xSpeed = (int)(Kp * (current - target));
// 限速
if (xSpeed > 100)
{
xSpeed = 100;
}
if (xSpeed < -100)
{
xSpeed = -100;
}
return(xSpeed);
}