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