Beispiel #1
0
        /////////////////////////////////////////////// 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);
        }
Beispiel #2
0
 public static UrgPort getInstance()
 {
     if (urgPort == null)
     {
         urgPort = new UrgPort();
     }
     return(urgPort);
 }
Beispiel #3
0
 /// <summary>
 /// 初始化串口信息
 /// </summary>
 public static void initPort()
 {
     conPort = ConPort.getInstance();
     drPort  = DrPort.getInstance();
     urgPort = UrgPort.getInstance();
     ports   = new IPort[] {
         conPort, drPort, urgPort
     };
 }
Beispiel #4
0
        /////////////////////////////////////////////// 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);
        }
Beispiel #5
0
        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);
        }
Beispiel #6
0
        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);
            }
        }
Beispiel #7
0
        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);
            }
        }