public void goWithSonic(KeyPoint res, KeyPoint des, IConPort myConPort, IDrPort myDrPort)
{
int goSpeed = -100;
double toMove = 0;
bool flagX = false;
bool flagY = false;
int shiftSpeed = 0;
int rotatSpeed = 0;
KeyPoint start = myDrPort.getPosition();
KeyPoint now = myDrPort.getPosition();
if (Math.Abs(des.x - res.x) < Math.Abs(des.y - res.y))
{
toMove = des.y - res.y;
flagY = true;
}
else
{
toMove = des.x - res.x;
flagX = true;
}
while ((flagX && Math.Abs(toMove - (now.x - start.x)) > 0.1) ||
(flagY && Math.Abs(toMove - (now.y - start.y)) > 0.1))
{
GetBackInfo(ref shiftSpeed, ref rotatSpeed, myConPort);
//if (myUrgPort.CanGo())
//{
myConPort.Control_Move_By_Speed(goSpeed, -shiftSpeed, rotatSpeed / 60);
//}
System.Threading.Thread.Sleep(100);
now = myDrPort.getPosition();
}
}
/// <summary>
/// 按照地图上点的顺序开始巡检
/// </summary>
/// <param name="map"></param>
/// <param name="conPort"></param>
/// <param name="drPort"></param>
/// <param name="urgPort"></param>
private static void Route(MapModel map, IConPort conPort, IDrPort drPort, IUrgPort urgPort)
{
drPort.setPosition(0, 0, 0);
for (int i = 0;
ControlMethod.curState == ControlMethod.ctrlItem.GoMap && i < map.Points.Count - 1;
++i)
{
if (map.Points[i + 1].type == 0) // map.Points[i].type == 0 &&
{
KeyPoint keyDes = new KeyPoint(map.Points[i + 1]);
P2P(map.Points[i], keyDes, conPort, drPort);
}
else
{
int keepLeft = (map.Points[i + 1].wayType == 0) ? 0 :
(map.Points[i + 1].wayType == 1) ? 10 * InfoManager.wayIF.WayLeft : -10 * InfoManager.wayIF.WayLeft;
Forward forward = new Forward();
forward.Start(map.Points[i + 1], keepLeft, conPort, urgPort, drPort);
// 双路径时置点
if (map.Points[i + 1].moveBack)
{
++i;
if (map.Points.Count > i + 1)
{
PortManager.drPort.setPosition(map.Points[i + 1]);
}
}
else
{
PortManager.drPort.setPosition(map.Points[i + 1]);
}
}
}
}
public void CorrPos(KeyPoint targetPoint, IConPort conPort, IDrPort drPort)
{
while (true)
{
double current = drPort.getPosition().x;
double target = targetPoint.x;
int xSpeed = (int)(150 * (current - target));
double error1 = Math.Abs(current - target);
current = drPort.getPosition().y;
target = targetPoint.y;
int ySpeed = (int)(-150 * (current - target));
double error2 = Math.Abs(current - target);
current = drPort.getPosition().w;
target = targetPoint.w;
int wSpeed = (int)(-100 * (current - target));
double error3 = Math.Abs(current - target);
if (error1 < 0.04 && error2 < 0.02 && error3 < 0.05)
{
break;
}
if (xSpeed < -50)
{
xSpeed = -50;
}
if (xSpeed > 50)
{
xSpeed = 50;
}
if (ySpeed < -100)
{
ySpeed = -100;
}
if (ySpeed > 100)
{
ySpeed = 100;
}
if (wSpeed < -10)
{
wSpeed = -10;
}
if (wSpeed > 10)
{
wSpeed = 10;
}
conPort.Control_Move_By_Speed(ySpeed, xSpeed, wSpeed);
System.Threading.Thread.Sleep(100);
}
}
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);
}
public void Start(KeyPoint targetPoint, double keepLeft, IConPort conPort, IUrgPort urgPort, IDrPort drPort)
{
if (ControlMethod.curState == ControlMethod.ctrlItem.ExpMap)
{
int setLeft = InfoManager.wayIF.WayLeft * 10; //10 * int.Parse(DataArea.infoModel.Data[(int)FileInfo.paramE.WayLeft]);
int setRigh = InfoManager.wayIF.WayRight * 10; //10 * int.Parse(DataArea.infoModel.Data[(int)FileInfo.paramE.WayRight]);
keepLeft = (Form_Path.wayType == 0) ? 0 :
(Form_Path.wayType == 1) ? setLeft : -setRigh;
}
// 记录距离，自己决定开启
AlignAisle align = new AlignAisle();
double record = align.recordDistance();
align.Start();
Backward backward = new Backward();
backward.clear();
config.PreviousPos = drPort.getPosition();
#region 找到通道入口
while (true)
{
// 获取速度
int ySpeed = getForwardSpeed(config.MaxForwardSpeed, targetPoint, urgPort, drPort);
int xSpeed = 0;
int wSpeed = 0;
// 退出条件
List <CONFIG.URG_POINT> pointsL = getUrgPoint(160, 180, urgPort);
List <CONFIG.URG_POINT> pointsR = getUrgPoint(0, 20, urgPort);
double minL = double.MaxValue, minR = double.MaxValue;
for (int i = 0; i < pointsL.Count; i++)
{
double x = Math.Abs(pointsL[i].x);
if (x < minL)
{
minL = x;
}
}
for (int i = 0; i < pointsR.Count; i++)
{
double x = Math.Abs(pointsR[i].x);
if (x < minR)
{
minR = x;
}
}
if (minL < 1000 || minR < 1000)
{
break;
}
// 控制
conPort.Control_Move_By_Speed(ySpeed, xSpeed, wSpeed);
// 比较之前与现在的位置
KeyPoint currentPos = drPort.getPosition();
bool recored = currentPos.x != config.PreviousPos.x ||
currentPos.y != config.PreviousPos.y ||
currentPos.w != config.PreviousPos.w;
config.PreviousPos = currentPos;
if (!PortManager.conPort.IsStop && recored)
{
Backward.COMMAND command = new Backward.COMMAND();
command.ForwardSpeed = ySpeed;
command.LeftSpeed = xSpeed;
command.RotateSpeed = wSpeed;
backward.set(command);
}
System.Threading.Thread.Sleep(100);
}
#endregion
//backward.startpoint = drPort.getPosition();
#region 通道内行走
if (keepLeft < 0)
{
keepLeft -= 225;
}
if (keepLeft > 0)
{
keepLeft += 225;
}
while (!config.AchieveTarget)
{
int ForwardSpeed = getForwardSpeed(config.MaxForwardSpeed, targetPoint, urgPort, drPort);
int TranslateSpeed = -getTranslateSpeed(keepLeft, conPort, urgPort, drPort);
int RotateSpeed = getRotateSpeed(conPort, urgPort, drPort);
// 距离限速
List <CONFIG.URG_POINT> pointsH = getUrgPoint(85, 95, urgPort);
double minH = double.MaxValue;
for (int i = 0; i < pointsH.Count; i++)
{
if (minH > pointsH[i].y)
{
minH = pointsH[i].y;
}
}
if (minH < 1200)
{
TranslateSpeed = 0;
//RotateSpeed = 0;
}
double current = drPort.getPosition().y;
while (Math.Abs(current - targetPoint.y) < 0.02)
{
break;
}
conPort.Control_Move_By_Speed(ForwardSpeed, TranslateSpeed, RotateSpeed);
// 比较之前与现在的位置
KeyPoint currentPos = drPort.getPosition();
bool recored = currentPos.x != config.PreviousPos.x ||
currentPos.y != config.PreviousPos.y ||
currentPos.w != config.PreviousPos.w;
config.PreviousPos = currentPos;
if (!PortManager.conPort.IsStop && recored)
{
Backward.COMMAND command = new Backward.COMMAND();
command.ForwardSpeed = ForwardSpeed;
command.LeftSpeed = TranslateSpeed;
command.RotateSpeed = RotateSpeed;
backward.set(command);
}
System.Threading.Thread.Sleep(100);
}
#endregion
if (ControlMethod.curState == ControlMethod.ctrlItem.ExpMap)
{
ProcessNewMap.markKeyPoint(1, record);
}
// 校准方式1
//CorrPos(targetPoint, conPort, drPort);
// 校准方式2
//CorrectPosition corrp = new CorrectPosition();
//corrp.Start(PortManager.conPort, PortManager.urgPort, targetPoint);
//PortManager.drPort.setPosition(targetPoint);
// 单一路径返回
if (ControlMethod.curState == ControlMethod.ctrlItem.ExpMap && !Form_Path.wayBack)
{
return;
}
if (ControlMethod.curState == ControlMethod.ctrlItem.GoMap && !targetPoint.moveBack)
{
return;
}
// 后退
conPort.Control_Move_By_Speed(0, 0, 0);
System.Threading.Thread.Sleep(1000);
backward.Start();
// 调整距离，自己决定开启
if (ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
// 大于10mm开启调整
if (targetPoint.disWay > 10)
{
align.adjustDistance(targetPoint.disWay);
}
}
else if (ControlMethod.curState == ControlMethod.ctrlItem.ExpMap)
{
align.adjustDistance(record);
}
}
private int getTranslateSpeed(double keepLeft, IConPort conPort, IUrgPort urgPort, IDrPort drPort)
{
// 取数据
SonicModel sonic = conPort.Measure_Sonic();
double distanceL = 0, distanceR = 0;
if (sonic.S[0] < 1000)
{
distanceL = sonic.S[0];
}
if (sonic.S[7] < 1000)
{
if (distanceL == 0)
{
distanceL = sonic.S[7];
}
else
{
distanceL = Math.Min(distanceL, sonic.S[7]);
}
}
if (sonic.S[3] < 1000)
{
distanceR = sonic.S[3];
}
if (sonic.S[4] < 1000)
{
if (distanceR == 0)
{
distanceR = sonic.S[4];
}
else
{
distanceR = Math.Min(distanceR, sonic.S[4]);
}
}
if (distanceL != 0)
{
distanceL += 225;
}
if (distanceR != 0)
{
distanceR += 225;
}
if (distanceL == 0)
{
distanceL = double.MaxValue;
}
if (distanceR == 0)
{
distanceR = double.MaxValue;
}
// 不适用超声波数据
distanceL = double.MaxValue;
distanceR = double.MaxValue;
// 加点前瞻
List <CONFIG.URG_POINT> pointsL = getUrgPoint(120, 180, urgPort);
List <CONFIG.URG_POINT> pointsR = getUrgPoint(0, 60, urgPort);
double minL = double.MaxValue, minR = double.MaxValue;
for (int i = 0; i < pointsL.Count; i++)
{
double x = Math.Abs(pointsL[i].x);
if (x < minL)
{
minL = x;
}
}
for (int i = 0; i < pointsR.Count; i++)
{
double x = Math.Abs(pointsR[i].x);
if (x < minR)
{
minR = x;
}
}
distanceL = Math.Min(distanceL, minL);
distanceR = Math.Min(distanceR, minR);
if (distanceL == 0 && distanceR == 0)
{
return(0);
}
// 判断通道宽度
double AisleWidth = 800;
// 获取控制
int TranslateSpeed = 0;
// 最短距离模式
double acceptDistance = 320;
bool acceptL = distanceL < acceptDistance;
bool acceptR = distanceR < acceptDistance;
if (acceptL && acceptR)
{
double current = distanceL;
double target = (distanceL + distanceR) / 2;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (acceptL && !acceptR)
{
double current = distanceL;
double target = (distanceL < AisleWidth) ? (distanceL + distanceR) : 180; //Math.Abs(keepLeft);
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (!acceptL && acceptR)
{
double current = distanceR;
double target = (distanceL < AisleWidth) ? (distanceL + distanceR) : 180; //Math.Abs(keepLeft);
TranslateSpeed = -(int)PDcontroller(current, target, ref config.PD_T);
}
// 限速
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
if (acceptL || acceptR)
{
return(TranslateSpeed);
}
// 数据能否使用
if (0 < distanceL && distanceL < AisleWidth)
{
acceptL = true;
}
if (0 < distanceR && distanceR < AisleWidth)
{
acceptR = true;
}
// 数据无效模式
if (!acceptL && !acceptR)
{
return(0);
}
if (!acceptL)
{
double current = distanceR;
double target = AisleWidth / 2;
TranslateSpeed = -(int)PDcontroller(current, target, ref config.PD_T);
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
return(TranslateSpeed);
}
if (!acceptR)
{
double current = distanceL;
double target = AisleWidth / 2;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
return(TranslateSpeed);
}
// 左中右模式
if (keepLeft == 0)
{
double current = distanceL;
double target = (distanceL + distanceR) / 2;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (keepLeft > 0)
{
double current = distanceL;
double target = keepLeft;
TranslateSpeed = (int)PDcontroller(current, target, ref config.PD_T);
}
if (keepLeft < 0)
{
double current = distanceR;
double target = -keepLeft;
TranslateSpeed = -(int)PDcontroller(current, target, ref config.PD_T);
}
// 限速
if (TranslateSpeed > config.MaxTranslateSpeed)
{
return(config.MaxTranslateSpeed);
}
if (TranslateSpeed < -config.MaxTranslateSpeed)
{
return(-config.MaxTranslateSpeed);
}
return(TranslateSpeed);
}
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);
}
/// <summary>
/// 执行点到点编码器导航程序
/// </summary>
/// <param name="res"></param>
/// <param name="des"></param>
/// <param name="conPort"></param>
/// <param name="drPort"></param>
private static void P2P(KeyPoint res, KeyPoint des, IConPort conPort, IDrPort drPort)
{
// 设置角度、距离误差范围
double angRound = 0.008;
double disRound = 0.0075;
// 开始调整的角度大小
double angAdjust = 0.2;
// 当前位置点信息
KeyPoint nowPos = drPort.getPosition();
// 按照相对坐标来走
des.x = nowPos.x + des.x - res.x;
des.y = nowPos.y + des.y - res.y;
des.w = nowPos.w + des.w - res.w;
//
// 1.旋转AGV对准方向
//
int rotSpeed = 0;
if (Math.Abs(nowPos.w - des.w) > angAdjust)
{
while (Math.Abs(nowPos.w - des.w) > angRound &&
ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
Navigation.getRotSpeed(des, nowPos, ref rotSpeed);
// 执行转弯
conPort.Control_Move_By_Speed(0, 0, rotSpeed);
Thread.Sleep(sleepTime);
nowPos = drPort.getPosition();
}
}
//
// 2.前进至目标位置
//
int goSpeed = 0;
int shSpeed = 0;
if (Math.Abs(res.y - des.y) < Math.Abs(res.x - des.x))
{
while (Math.Abs(des.x - nowPos.x) > disRound && ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
Navigation.getDefaultSpeed(res, des, nowPos, nowPos.w + Math.PI / 2, ref goSpeed, ref shSpeed);
// 执行行进控制
conPort.Control_Move_By_Speed(goSpeed, shSpeed, 0);
Thread.Sleep(sleepTime);
nowPos = drPort.getPosition();
Console.WriteLine(nowPos.x);
}
}
else
{
while (Math.Abs(des.y - nowPos.y) > disRound && ControlMethod.curState == ControlMethod.ctrlItem.GoMap)
{
Navigation.getDefaultSpeed(res, des, nowPos, nowPos.w + Math.PI / 2, ref goSpeed, ref shSpeed);
// 执行行进控制
conPort.Control_Move_By_Speed(goSpeed, shSpeed, 0);
Thread.Sleep(sleepTime);
nowPos = drPort.getPosition();
}
}
}