コード例 #1
0
ファイル: GoWithSonic.cs プロジェクト: miphas/SmartCarB
        private void GetBackInfo(ref int shiftSpeed, ref int rotatSpeed, IConPort myconport)
        {
            SonicModel sm    = myconport.Measure_Sonic();
            int        right = sm.S[4];
            int        left  = sm.S[7];
            //if (right < 80) right = 230;
            //if (left < 80) left = 230;
            double angle;
            double dis;
            double length = right + left + 400;//左右距离+车宽

            if (right > 350 && left > 350)
            {
                rotatSpeed = 0;
                shiftSpeed = 0;
            }
            else if (right > 350)
            {
                shiftSpeed = (int)(0.2 * (200 - left));
                rotatSpeed = (int)(3 * (200 - left));
            }
            else if (left > 350)
            {
                shiftSpeed = (int)(0.2 * (right - 200));
                rotatSpeed = (int)(3 * (right - 200));
            }
            else
            {
                if (length < 800)
                {
                    angle = 0;
                }
                else
                {
                    angle = Math.Acos(800 / length);
                }
                if (right >= left)
                {
                    rotatSpeed = (int)(5 * (angle * 180 / 3.14159));
                }
                else if (right < left)
                {
                    rotatSpeed = -(int)(5 * (angle * 180 / 3.14159));
                }
                if (length > 800)
                {
                    shiftSpeed = (int)(0.2 * (right - left));
                }
            }
        }
コード例 #2
0
ファイル: ConPort.cs プロジェクト: miphas/SmartCarB
        public SonicModel Measure_Sonic()
        {
            // 等待设置完毕
            while (config.IsSetting)
            {
                ;
            }
            config.IsGetting = true;

            // 填充数据
            int[] temp = new int[8];
            for (int i = 0; i < 8; i++)
            {
                temp[i] = config.UltraSonic[i];
            }
            SonicModel sonicData = new SonicModel(temp);

            // 关闭等待
            config.IsGetting = false;
            return(sonicData);
        }
コード例 #3
0
ファイル: Forward.cs プロジェクト: miphas/SmartCarB
        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);
        }
コード例 #4
0
ファイル: Forward.cs プロジェクト: miphas/SmartCarB
        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);
        }