public override void Send(Drive RobDrive)
{ /*youbot_connection.send(ToString(data));*/
if (RobDrive != null)
{
right = RobDrive.right * (-5f);//20.02 удалет тип var//*(-2.5f);
left = RobDrive.left * (-5f);
//right = 0;//для проверки
// left = 0;
}
if (VRepFunctions.GetConnectionId(clientID) == -1)
{
return;
}
Byte sensorTrigger = (Byte)0;
int simulationTime = VRepFunctions.GetLastCmdTime(clientID);//??????
if (simulationTime - driveBackStartTime < 3000)
{
driveBackStartTime = simulationTime;
}
{
VRepFunctions.SetJointTargetVelocity(clientID, leftMotorHandle, left);
VRepFunctions.SetJointTargetVelocity(clientID, leftMotorHandleA, left);
VRepFunctions.SetJointTargetVelocity(clientID, rightMotorHandle, right);
VRepFunctions.SetJointTargetVelocity(clientID, rightMotorHandleA, right);//right);
}
}
public override void Init()
{
clientID = VRepFunctions.Start("127.0.0.1", 7777);
if (clientID != -1)
{
VRepFunctions.GetObjectHandle(clientID, "rollingJoint_fl", out leftMotorHandle);
VRepFunctions.GetObjectHandle(clientID, "rollingJoint_rl", out leftMotorHandleA);
VRepFunctions.GetObjectHandle(clientID, "rollingJoint_rr", out rightMotorHandle);
VRepFunctions.GetObjectHandle(clientID, "rollingJoint_fr", out rightMotorHandleA);
VRepFunctions.GetObjectHandle(clientID, "Proximity_sensor", out sensorHandle);
}
}
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();
}
}
}
}
public override void Deactivate()
{
VRepFunctions.Finish(clientID);
}
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();
}
}
}
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();
}
}
}
