//绘制合速度图像 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); }
//通过配置的轮子最大加速度进行降速 //适当比例的降速,算完后记得把最新速度数据放在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; } }