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();
}
}
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);
}
}
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);
}
}
/// <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();
}
}
}