private void timer1_Tick(object sender, EventArgs e) { if (net != null) { net.GetNewImage("False"); //net.Restart("False"); } PaintEventArgs p = new PaintEventArgs(pictureBox1.CreateGraphics(), pictureBox1.Bounds); //Компонент на котором нужно рисовать и область на которой нужно рисовать pictureBox1_Paint(sender, p); if (ra is VrepAdapter) { var vrep = ra as VrepAdapter; string Lidar = VRepFunctions.GetStringSignal(vrep.clientID, "Lidar"); //str-данные с ледара string RobPos = VRepFunctions.GetStringSignal(vrep.clientID, "RobPos"); //получение координат робота на сцене Врепа vrep.ReceiveLedData(Lidar); vrep.ReceiveOdomData(RobPos); } if (ra != null) { ra.Send(RobDrive); //ra.SendVrep(2f, -2f); } if (RobDrive != null & ra != null && SQ != null)//отправка одометрии в экземпляр класса drive { if (map.Val == null) { map.ImgDistance(); // map.FillNewPerm(); } map.graph = net.GetGraph(ra.RobotOdomData, map.graph); //map.GlobListToGraph(map.GlobalMapList, map.RobOdData); float GoalPointX = Convert.ToSingle(textBox8.Text); float GoalPointY = Convert.ToSingle(textBox9.Text); Point start = new Point((int)(ra.RobotOdomData[0] * 10 + map.Xmax / 2), (int)(ra.RobotOdomData[1] * 10 + map.Ymax / 2)); Point goal = new Point((int)GoalPointX * 10 + map.Xmax / 2, (int)GoalPointY * 10 + map.Ymax / 2); ListPoints = SiG.FindPath(map.graph, start, goal); if (ListPoints != null) { SQ.GetNextPoint(ListPoints, ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], map.Xmax, map.Ymax); RobDrive.GetDrive(ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], SQ.CurrentPointX, SQ.CurrentPointY, map.Xmax, map.Ymax); ra.Send(RobDrive); //ra.SendVrep(2f, -2f); //ra.Sendmaf("LUA_Base(0, 0, 0.1)"); } //if (net.PythonIsReady()) //{ // net.GetNewImage("True", map.GlobalValString, map.GlobalOdomString);// отправляем глобальные координаты для всех точек // map.GlobalOdomString = ""; // map.FillNewVal(net.GetNewVal());// обновляем массив Val новыми значениями проходимости // net.GetNewImage("False"); //} //map.LedDataToList(map.Val, ra.RobotOdomData, ind++); // обновление массива GlobalMapList } if (ra != null & RobDrive != null)//вывод переменных из Робот Адаптера на форму { string OutOdomData = ""; for (int i = 0; i < ra.RobotOdomData.Length; i++) { OutOdomData = OutOdomData + ra.RobotOdomData[i] + "; "; } richTextBox2.Text = OutOdomData; if (RobDrive != null) { textBox2.Text = RobDrive.Phi.ToString(); textBox2.Invalidate(); textBox3.Text = RobDrive.RobotDirection.ToString(); textBox3.Invalidate(); textBox4.Text = RobDrive.TargetDirection.ToString(); textBox4.Invalidate(); textBox5.Text = RobDrive.DistToTarget.ToString(); textBox5.Invalidate(); } } }
public List <Point> ListPointsTemp = new List <Point>(); //для работы Астара мо змейкой private void timer1_Tick(object sender, EventArgs e) { //pictureBox1.Invalidate(); PaintEventArgs p = new PaintEventArgs(pictureBox1.CreateGraphics(), pictureBox1.Bounds); //Компонент на котором нужно рисовать и область на которой нужно рисовать pictureBox1_Paint(sender, p); // pictureBox1_Paint(); // this.Invalidate(); f2.Invalidate(); if (ra is VrepAdapter) { var vrep = ra as VrepAdapter; string Lidar = VRepFunctions.GetStringSignal(vrep.clientID, "Lidar"); //str-данные с ледара string RobPos = VRepFunctions.GetStringSignal(vrep.clientID, "RobPos"); //получение координат робота на сцене Врепа vrep.ReceiveLedData(Lidar); vrep.ReceiveOdomData(RobPos); } if (ra != null) { ra.Send(RobDrive); } if (RobDrive != null & ra != null && SQ != null)//отправка одометрии в экземпляр класса drive { map.GlobListToGraph(map.GlobalMapList, map.RobOdData); float GoalPointX = Convert.ToSingle(textBox8.Text); float GoalPointY = Convert.ToSingle(textBox9.Text); Point start = new Point((int)(ra.RobotOdomData[0] * 10 + map.Xmax / 2), (int)(ra.RobotOdomData[1] * 10 + map.Ymax / 2)); Point goal = new Point((int)GoalPointX * 10 + map.Xmax / 2, (int)GoalPointY * 10 + map.Ymax / 2); // float[] test = OFC.getCamOdom();/////////////////////////////////////////////////////удалить эту строчку и объект класса OFC // ListPoints = null; //1 шаг if (key == 0) { goal.X = 100; goal.Y = 100; ListPoints = SiG.FindPath(map.graph, start, goal); //SearchInGraph.FindPath(map.graph, start, goal); key = 1; ListPointsTemp = ListPoints; } //блок первый, выбираем левую нижнюю точку для поиска по карте. PointF H = new PointF(0, 0); RM.NextRoom(out H, out flag, ra.RobotOdomData, flag); //Реализуем движение змейкой //2 шаг flag = 2; if (flag == 2) { PointF snakePoint = SP.ExpertSystem(out flag, ra.RobotOdomData, ListPointsTemp); // получаем следующую точку змейки Point snakeP = new Point((int)snakePoint.X + map.Xmax / 2, (int)snakePoint.Y + map.Ymax / 2); //точка подготовленная для масштабов карты ListPoints = SiG.FindPath(map.graph, start, Point.Round(snakeP)); //отправляем в А* эти точки } //if (snakePoint.X == 0 & snakePoint.Y == 0) //{ // //здесь взять из класса RoomMap следующюю комнату и поехать туда. Из RoomMap точка должна прийти в неподготовленном виде как в классе snakePoint //} Point driveH = new Point((int)H.X + map.Xmax / 2, (int)H.Y + map.Ymax / 2);//точка для движения к неисследованному кармуну на карте if (flag == 1) { ListPoints = SiG.FindPath(map.graph, start, Point.Round(driveH));//отправляем в А* эти точки } ListPointsTemp = ListPoints;//запоминаем результат А* для следующего цикла if (ListPoints != null) { SQ.GetNextPoint(ListPoints, ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], map.Xmax, map.Ymax); //RobDrive.GetDrive(ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], SQ.CurrentPointX, SQ.CurrentPointY, map.Xmax,map.Ymax);//закоментили 12.01.18 чтобы отправлять туда точки от движения змейкой RobDrive.GetDrive(ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], ListPoints[ListPoints.Count - 1].X, ListPoints[ListPoints.Count - 1].Y, map.Xmax, map.Ymax); ra.Send(RobDrive); } map.LedDataToList(ra.RobotLedData, ra.RobotOdomData); } if (ra != null & RobDrive != null)//вывод переменных из Робот Адаптера на форму { string OutLedData = ""; string OutOdomData = ""; for (int i = 0; i < ra.RobotLedData.Length; i++) { OutLedData = OutLedData + ra.RobotLedData[i] + "; "; } for (int i = 0; i < ra.RobotOdomData.Length; i++) { OutOdomData = OutOdomData + ra.RobotOdomData[i] + "; "; } // richTextBox1.Text = OutLedData;//закоменчен вывод данных одометрии richTextBox2.Text = OutOdomData; if (RobDrive != null) { textBox2.Text = RobDrive.Phi.ToString(); textBox2.Invalidate(); textBox3.Text = RobDrive.RobotDirection.ToString(); textBox3.Invalidate(); textBox4.Text = RobDrive.TargetDirection.ToString(); textBox4.Invalidate(); textBox5.Text = RobDrive.DistToTarget.ToString(); textBox5.Invalidate(); if (ra is YoubotAdapter) { //richTextBox3.Text = OFC.CamOdomData[0].ToString(); //richTextBox3.Text = OFC.CamOdomData[1].ToString(); //richTextBox3.Text = OFC.CamOdomData[2].ToString(); //richTextBox3.Invalidate(); // richTextBox3.Text = OFC.txb_text.ToString(); // richTextBox3.Invalidate(); } } } }
private void timer1_Tick(object sender, EventArgs e) { PaintEventArgs p = new PaintEventArgs(pictureBox1.CreateGraphics(), pictureBox1.Bounds); //Компонент на котором нужно рисовать и область на которой нужно рисовать pictureBox1_Paint(sender, p); f2.Invalidate(); if (ra is VrepAdapter) { var vrep = ra as VrepAdapter; string Lidar = VRepFunctions.GetStringSignal(vrep.clientID, "Lidar"); //str-данные с ледара string RobPos = VRepFunctions.GetStringSignal(vrep.clientID, "RobPos"); //получение координат робота на сцене Врепа vrep.ReceiveLedData(Lidar); vrep.ReceiveOdomData(RobPos); } if (ra != null) { ra.Send(RobDrive); } if (RobDrive != null & ra != null && SQ != null)//отправка одометрии в экземпляр класса drive { map.GlobListToGraph(map.GlobalMapList, map.RobOdData); float GoalPointX = Convert.ToSingle(textBox8.Text); float GoalPointY = Convert.ToSingle(textBox9.Text); Point start = new Point((int)(ra.RobotOdomData[0] * 10 + map.Xmax / 2), (int)(ra.RobotOdomData[1] * 10 + map.Ymax / 2)); Point goal = new Point((int)GoalPointX * 10 + map.Xmax / 2, (int)GoalPointY * 10 + map.Ymax / 2); ListPoints = SiG.FindPath(map.graph, start, goal); if (ListPoints != null) { SQ.GetNextPoint(ListPoints, ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], map.Xmax, map.Ymax); RobDrive.GetDrive(ra.RobotOdomData[0], ra.RobotOdomData[1], ra.RobotOdomData[2], SQ.CurrentPointX, SQ.CurrentPointY, map.Xmax, map.Ymax); ra.Send(RobDrive); } map.LedDataToList(ra.RobotLedData, ra.RobotOdomData); } if (ra != null & RobDrive != null)//вывод переменных из Робот Адаптера на форму { string OutLedData = ""; string OutOdomData = ""; for (int i = 0; i < ra.RobotLedData.Length; i++) { OutLedData = OutLedData + ra.RobotLedData[i] + "; "; } for (int i = 0; i < ra.RobotOdomData.Length; i++) { OutOdomData = OutOdomData + ra.RobotOdomData[i] + "; "; } richTextBox2.Text = OutOdomData; if (RobDrive != null) { textBox2.Text = RobDrive.Phi.ToString(); textBox2.Invalidate(); textBox3.Text = RobDrive.RobotDirection.ToString(); textBox3.Invalidate(); textBox4.Text = RobDrive.TargetDirection.ToString(); textBox4.Invalidate(); textBox5.Text = RobDrive.DistToTarget.ToString(); textBox5.Invalidate(); } } }