//绘制合速度图像
        void ResultantVelCurveDrawing()
        {
            //数据读取
            StreamReader pathFile = File.OpenText(VelInf);

            List <string> tempString = new List <string>();

            while (!pathFile.EndOfStream)
            {
                tempString.Add(pathFile.ReadLine());
            }

            pathFile.Close();


            //如果点数小于2,不能规划出轨迹
            if (tempString.Count() < 2)
            {
                return;
            }

            Graphics myGra = VelDisplay.CreateGraphics();

            Pen myPen = new Pen(Color.Red, 1);

            PointF[] velWheel1       = new PointF[tempString.Count()];
            PointF[] velWheel2       = new PointF[tempString.Count()];
            PointF[] velWheel3       = new PointF[tempString.Count()];
            PointF[] targetVelWheel1 = new PointF[tempString.Count()];
            PointF[] targetVelWheel2 = new PointF[tempString.Count()];
            PointF[] targetVelWheel3 = new PointF[tempString.Count()];

            float[] zAngle = new float[tempString.Count()];
            int     i      = 0;

            foreach (string str in tempString)
            {
                string[] sArray = str.Split(',');

                velWheel1[i].X = velWheel2[i].X = velWheel3[i].X
                                                      = targetVelWheel1[i].X = targetVelWheel2[i].X = targetVelWheel3[i].X
                                                                                                          = float.Parse(sArray[0]);

                velWheel1[i].Y = float.Parse(sArray[1]);
                velWheel2[i].Y = float.Parse(sArray[2]);
                velWheel3[i].Y = float.Parse(sArray[3]);

                targetVelWheel1[i].Y = float.Parse(sArray[4]);
                targetVelWheel2[i].Y = float.Parse(sArray[5]);
                targetVelWheel3[i].Y = float.Parse(sArray[6]);
                i++;
            }

            PointF[] trueVel         = new PointF[tempString.Count()];
            PointF[] targetVel       = new PointF[tempString.Count()];
            PointF[] trueDir         = new PointF[tempString.Count()];
            PointF[] targetDir       = new PointF[tempString.Count()];
            PointF[] trueRotateVel   = new PointF[tempString.Count()];
            PointF[] targetRotateVel = new PointF[tempString.Count()];
            PointF[] DirectionErr    = new PointF[tempString.Count()];
            PointF[] binaryzation    = new PointF[tempString.Count()];



            //由三个轮子的速度计算合成速度
            for (int j = 0; j < targetVelWheel1.Count(); j++)
            {
                Calculate.TriWheelVel2_t tempVel;
                tempVel = Calculate.TriWheelVel2ResultantVel(velWheel1[j].Y, velWheel2[j].Y, velWheel3[j].Y, zAngle[j]);

                trueVel[j].Y       = tempVel.speed;
                trueVel[j].X       = velWheel1[j].X;
                trueDir[j].Y       = tempVel.direction;
                trueDir[j].X       = velWheel1[j].X;
                trueRotateVel[j].Y = tempVel.rotationVel;
                trueRotateVel[j].X = velWheel1[j].X;


                tempVel              = Calculate.TriWheelVel2ResultantVel(targetVelWheel1[j].Y, targetVelWheel2[j].Y, targetVelWheel3[j].Y, zAngle[j]);
                targetVel[j].Y       = tempVel.speed;
                targetVel[j].X       = targetVelWheel1[j].X;
                targetDir[j].Y       = tempVel.direction;
                targetDir[j].X       = targetVelWheel1[j].X;
                targetRotateVel[j].Y = tempVel.rotationVel;
                targetRotateVel[j].X = targetVelWheel1[j].X;
            }


            pathMax = FindMaxAbs(targetDir).X;
            zoom    = (float)(VelDisplay.Bottom / 2) / FindMaxAbs(targetDir).Y;


            //设置滚动条最大值
            hScrollBar1.Maximum = (int)(PERCENT * pathMax / (VelDisplay.Right / zoomL)) + 1;



            for (int j = 0; j < targetVelWheel1.Count(); j++)
            {
                DirectionErr[j].X = targetDir[j].X;
                DirectionErr[j].Y = Math.Abs(targetDir[j].Y - trueDir[j].Y);
                if (DirectionErr[j].Y > 180)
                {
                    DirectionErr[j].Y = 360 - DirectionErr[j].Y;
                }
                else if (DirectionErr[j].Y < -180)
                {
                    DirectionErr[j].Y = 360 + DirectionErr[j].Y;
                }
            }


            //进行滤波处理
            for (int m = 0; m < 4; m++)
            {
                for (int j = 1; j < DirectionErr.Count() - 1; j++)
                {
                    DirectionErr[j].Y = (DirectionErr[j - 1].Y + DirectionErr[j + 1].Y) / 2;
                }
            }



            //进行阈值判断,二值化
            for (int j = 0; j < DirectionErr.Count(); j++)
            {
                if (DirectionErr[j].Y < 8)
                {
                    binaryzation[j].Y = 0;
                    binaryzation[j].X = DirectionErr[j].X;
                }
                else
                {
                    binaryzation[j].Y = DirectionErr[j].Y;
                    binaryzation[j].X = DirectionErr[j].X;
                }
            }



            //将二值化的文件存入txt文档中
            StreamWriter SpeedFile = new StreamWriter(SpeedReplanInfo, false);

            for (int m = 0; m < binaryzation.Count(); m++)
            {
                SpeedFile.WriteLine("{0:f0},{1:f0}", binaryzation[m].X, binaryzation[m].Y);
            }

            SpeedFile.Close();


            //进行阈值判断,二值化
            for (int j = 0; j < DirectionErr.Count(); j++)
            {
                binaryzation[j] = GetPanelAxes(binaryzation[j].X, binaryzation[j].Y);
            }



            myPen.Color = Color.Black;
            myGra.DrawLines(myPen, binaryzation);
        }
Beispiel #2
0
        //通过配置的轮子最大加速度进行降速
        //适当比例的降速,算完后记得把最新速度数据放在ringbuffer里
        //wheelOne 一号轮速度数组首地址
        //wheelTwo  二号轮速度数组首地址
        //wheelThree 三号轮速度数组首地址
        static public void DynamicalAjusting(float[] wheelOne, float[] wheelTwo, float[] wheelThree)
        {
            float time    = 0.0f;
            int   n       = PointsInfo.pnts.Count();
            float tempAcc = 0.0f;

            //每次加速度降低至上次的百分值
            float percent = 0.9f;

            //先正向削减速度
            for (int i = 2; i < n + 1; i++)
            {
                //粗略计算每两示教点之间的运动的时间
                time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
                //轮1
                //只处理速度同向的情况
                if (wheelOne[i - 1] * wheelOne[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc > MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelOne[i - 1] = wheelOne[i - 1] > 0 ? wheelOne[i - 2] + tempAcc * percent * time : wheelOne[i - 2] - tempAcc * percent * time;
                }
                //轮2
                //只处理速度同向的情况
                if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc > MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelTwo[i - 1] = wheelTwo[i - 1] > 0 ? wheelTwo[i - 2] + tempAcc * percent * time : wheelTwo[i - 2] - tempAcc * percent * time;
                }

                //轮3
                //只处理速度同向的情况
                if (wheelThree[i - 1] * wheelThree[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc > MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelThree[i - 1] = wheelThree[i - 1] > 0 ? wheelThree[i - 2] + tempAcc * percent * time : wheelThree[i - 2] - tempAcc * percent * time;
                }
            }

            //反向削减速度
            for (int i = n; i > 1; i--)
            {
                //粗略计算每两示教点之间的运动的时间
                time = (PointsInfo.pnts[i - 1].length - PointsInfo.pnts[i - 2].length) / (PointsInfo.pnts[i - 1].velMax + PointsInfo.pnts[i - 2].velMax) * 2;
                //轮1
                //只处理速度同向的情况
                if (wheelOne[i - 1] * wheelOne[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelOne[i - 1]) - Math.Abs(wheelOne[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc < -MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelOne[i - 2] = wheelOne[i - 2] > 0 ? wheelOne[i - 1] - tempAcc * percent * time : wheelOne[i - 1] + tempAcc * percent * time;
                }


                //轮2
                //只处理速度同向的情况
                if (wheelTwo[i - 1] * wheelTwo[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelTwo[i - 1]) - Math.Abs(wheelTwo[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc < -MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelTwo[i - 2] = wheelTwo[i - 2] > 0 ? wheelTwo[i - 1] - tempAcc * percent * time : wheelTwo[i - 1] + tempAcc * percent * time;
                }


                //轮3
                //只处理速度同向的情况
                if (wheelThree[i - 1] * wheelThree[i - 2] > 0)
                {
                    tempAcc = (Math.Abs(wheelThree[i - 1]) - Math.Abs(wheelThree[i - 2])) / time;
                }
                else
                {
                    tempAcc = 0.0f;
                }

                if (tempAcc < -MotionCardParameter.GetAccMax())
                {
                    //每次削减0.05的加速度
                    wheelThree[i - 2] = wheelThree[i - 2] > 0 ? wheelThree[i - 1] - tempAcc * percent * time : wheelThree[i - 1] + tempAcc * percent * time;
                }
            }



            for (int i = 0; i < n - 1; i++)
            {
                Calculate.TriWheelVel2_t tempVel2;

                tempVel2 = Calculate.TriWheelVel2ResultantVel(wheelOne[i], wheelTwo[i], wheelThree[i], PointsInfo.pnts[i].posAngle);

                //存入新计算的速度
                PointsInfo.pnts[i].velMax = tempVel2.speed;
            }
        }