/// <summary>
/// 保存当前路径
/// </summary>
private void btnSave_Click(object sender, EventArgs e)
{
if (this.boxModel.SelectedIndex == -1)
{
MessageBox.Show("请选择模式！", "提示", MessageBoxButtons.OK, MessageBoxIcon.Warning);
return;
}
if (this.txtName.Text.Length == 0)
{
MessageBox.Show("请填写路径名！", "提示", MessageBoxButtons.OK, MessageBoxIcon.Warning);
return;
}
// 新建路径模式下
if (this.boxModel.SelectedIndex == 0)
{
FolderBrowserDialog dia = new FolderBrowserDialog();
dia.ShowDialog();
if (dia.SelectedPath == null || dia.SelectedPath.Length == 0)
{
MessageBox.Show("请选择路径！", "提示", MessageBoxButtons.OK, MessageBoxIcon.Warning);
return;
}
ProcessNewMap.stopRefreshMap();
String path = dia.SelectedPath + (dia.SelectedPath.EndsWith("\\") ? "" : "\\") + txtName.Text + ".xml";
MapFile mf = new MapFile(path);
MapModel tm = ProcessNewMap.getMapModel();
mf.writeNodeData(tm);
}
}
// 执行控制线程事项
public static void runMethod()
{
while (!Form1.closing)
{
// 什么都不做
if (curState == ctrlItem.DoNothing)
{
// do nothing
}
// 沿地图行进
else if (curState == ctrlItem.GoMap)
{
ProcessRoute.routing();
curState = ctrlItem.DoNothing;
}
// 沿通道创建地图
else if (curState == ctrlItem.ExpMap)
{
// 沿通道行进倒退
new Forward().Start(new KeyPoint(), 0, PortManager.conPort, PortManager.urgPort, PortManager.drPort);
// 重设关键点(仅双路径时重设)
if (Form_Path.wayBack)
{
PortManager.drPort.setPosition(DataArea.mapModel.Points[DataArea.mapModel.Points.Count - 2]);
ProcessNewMap.markKeyPoint(0);
}
curState = ctrlItem.DoNothing;
}
}
System.Threading.Thread.Sleep(refreshTime);
}
/// <summary>
/// 记录普通关键点
/// </summary>
private void rioPT_Click(object sender, EventArgs e)
{
if (this.boxModel.Text.Length == 0)
{
MessageBox.Show("请先选择模式!", "提示", MessageBoxButtons.OK, MessageBoxIcon.Warning);
return;
}
ProcessNewMap.markKeyPoint(0);
}
/// <summary>
/// 选择模式旋钮
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void boxModel_SelectedIndexChanged(object sender, EventArgs e)
{
// 选择生成地图选项
if (boxModel.SelectedIndex == 0)
{
// 初始化串口信息
PortManager.initPort();
PortManager.openAllPort();
ProcessNewMap.init();
ProcessNewMap.startRefreshMap();
}
}
// 执行界面更新线程事项
public static void runMethod()
{
while (!Form1.closing)
{
// 什么都不做
if (curState == displayItem.DoNothing)
{
// do nothing
}
// 更新引导界面
else if (curState == displayItem.RefGuide)
{
ProcessCurMap.mapping();
}
// 更新路径生成界面
else if (curState == displayItem.RefPath)
{
ProcessNewMap.mapping();
}
System.Threading.Thread.Sleep(refreshTime);
}
System.Threading.Thread.Sleep(refreshTime);
}
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);
}
}
