private void btnLaunch_Click(object sender, EventArgs e)
{
if (_linkPolling == null)
{
btnLaunch.Text = "Stopper";
btnLaunch.Image = Properties.Resources.Pause16;
_linkPolling = ThreadManager.CreateThread(link => Measure());
_linkPolling.StartInfiniteLoop(new TimeSpan(0));
_linkDrawing = ThreadManager.CreateThread(link => DrawMeasures());
_linkDrawing.StartInfiniteLoop(new TimeSpan(0, 0, 0, 0, 50));
}
else
{
btnLaunch.Text = "Lancer";
btnLaunch.Image = Properties.Resources.Play16;
_linkPolling.Cancel();
_linkDrawing.Cancel();
_linkPolling.WaitEnd();
_linkDrawing.WaitEnd();
_linkPolling = null;
_linkDrawing = null;
}
}
private void btnAffichage_Click(object sender, EventArgs e)
{
if (btnAffichage.Text == "Lancer l'affichage")
{
_linkDisplay = ThreadManager.CreateThread(link => DisplayInfos());
_linkDisplay.Name = "Affichage des données";
_linkDisplay.StartInfiniteLoop(new TimeSpan(0, 0, 0, 0, 100));
Dessinateur.Start();
btnAffichage.Text = "Stopper l'affichage";
btnAffichage.Image = GoBot.Properties.Resources.Pause16;
}
else
{
_linkDisplay?.Cancel();
_linkDisplay?.WaitEnd();
_linkDisplay = null;
Dessinateur.Stop();
btnAffichage.Text = "Lancer l'affichage";
btnAffichage.Image = GoBot.Properties.Resources.Play16;
}
}
public override void DeInit()
{
_linkAsserv.Cancel();
_linkLogPositions.Cancel();
_linkAsserv.WaitEnd();
_linkLogPositions.WaitEnd();
}
private void btnLateral_Click(object sender, EventArgs e)
{
_isOnLeft = !_isOnLeft;
if (_linkAnimation != null)
{
_linkAnimation.Cancel();
_linkAnimation.WaitEnd();
}
if (!_isOnLeft)
{
Actionneur.ElevatorRight.DoPushInside();
btnLateral.Image = Properties.Resources.BigArrow;
}
else
{
Actionneur.ElevatorRight.DoPushOutside();
Bitmap bmp = new Bitmap(Properties.Resources.BigArrow);
bmp.RotateFlip(RotateFlipType.RotateNoneFlipX);
btnLateral.Image = bmp;
}
_linkAnimation = ThreadManager.CreateThread(link => ThreadLateral(!_isOnLeft ? -4 : 4));
_linkAnimation.StartInfiniteLoop(15);
picStorage.Focus();
}
protected override void StopLoop()
{
if (_feedLink != null)
{
_manager.CloseChannelUDP();
_feedLink.Cancel();
_feedLink.WaitEnd();
_feedLink = null;
}
}
private void btnFingerRight_Click(object sender, EventArgs e)
{
if (_linkFingerRight == null)
{
_linkFingerRight = Threading.ThreadManager.CreateThread(link => Actionneurs.Actionneur.FingerRight.DoDemoGrab(link));
_linkFingerRight.StartThread();
}
else
{
_linkFingerRight.Cancel();
_linkFingerRight.WaitEnd();
_linkFingerRight = null;
}
}
private void switchBouton_ValueChanged(object sender, bool value)
{
if (value)
{
trackBar.Min = _currentPositionnable.Minimum;
trackBar.Max = _currentPositionnable.Maximum;
_linkPolling = ThreadManager.CreateThread(link => PollingLoop());
_linkPolling.StartThread();
}
else
{
_linkPolling.Cancel();
}
}
private void btnColorLeft_ValueChanged(object sender, bool value)
{
if (value)
{
Robots.MainRobot.SetActuatorOnOffValue(ActuatorOnOffID.PowerSensorColorBuoyLeft, true);
Robots.MainRobot.SensorColorChanged += GrosRobot_SensorColorChanged;
ThreadManager.CreateThread(link => PollingColorLeft(link)).StartInfiniteLoop(50);
}
else
{
Robots.MainRobot.SetActuatorOnOffValue(ActuatorOnOffID.PowerSensorColorBuoyLeft, false);
_linkColorLeft.Cancel();
_linkColorLeft.WaitEnd();
_linkColorLeft = null;
}
}
private void switchButtonPort_ValueChanged(object sender, bool value)
{
if ((switchButtonPortA.Value || switchButtonPortB.Value || switchButtonPortC.Value) && _link == null)
{
_link = ThreadManager.CreateThread(link => AskValues());
_link.Name = "Ports numériques " + _board.ToString();
_link.StartInfiniteLoop(50);
}
if (!switchButtonPortA.Value & !switchButtonPortB.Value & !switchButtonPortC.Value & _link != null)
{
_link.Cancel();
_link.WaitEnd();
_link = null;
}
}
private void btnOnOff_ValueChanged(object sender, bool value)
{
if (value)
{
ledState.Color = Robots.MainRobot.ReadSensorOnOff(_sensor) ? Color.LimeGreen : Color.Red;
_linkPolling = ThreadManager.CreateThread(link => PollingSensor());
_linkPolling.RegisterName(nameof(PollingSensor) + " " + lblName.Text);
_linkPolling.StartInfiniteLoop(50);
}
else
{
_linkPolling.Cancel();
_linkPolling.WaitEnd();
_linkPolling = null;
ledState.Color = Color.Gray;
}
}
private void MainForm_KeyUp(object sender, KeyEventArgs e)
{
if (e.KeyCode == Keys.Down || e.KeyCode == Keys.Up)
{
_moveVertical = 0;
}
else if (e.KeyCode == Keys.Left || e.KeyCode == Keys.Right)
{
_moveHorizontal = 0;
}
if (_moveVertical == 0 && _moveHorizontal == 0 && _moveLoop != null)
{
_moveLoop.Cancel();
_moveLoop.WaitEnd();
_moveLoop = null;
}
}
private void boxTorque_ValueChanged(object sender, bool value)
{
if (value)
{
_linkPolling = ThreadManager.CreateThread(link => GetServoInfos());
_linkPolling.StartInfiniteLoop(new TimeSpan(0, 0, 0, 0, 20));
_linkDrawing = ThreadManager.CreateThread(link => DrawTorqueCurve());
_linkDrawing.StartInfiniteLoop(new TimeSpan(0, 0, 0, 0, 100));
}
else
{
_linkPolling.Cancel();
_linkDrawing.Cancel();
_linkPolling.WaitEnd();
_linkDrawing.WaitEnd();
_linkPolling = null;
_linkDrawing = null;
}
}
private void switchBouton_ValueChanged(object sender, bool value)
{
if (value)
{
_linkPolling = ThreadManager.CreateThread(link => AskValues());
_linkPolling.Name = "Ports analogiques " + Carte.ToString();
_linkPolling.StartInfiniteLoop(50);
_linkDraw = ThreadManager.CreateThread(link => ctrlGraphique.DrawCurves());
_linkDraw.Name = "Graph ports analogiques " + Carte.ToString();
_linkDraw.StartInfiniteLoop(100);
}
else
{
_linkPolling.Cancel();
_linkPolling.WaitEnd();
_linkPolling = null;
_linkDraw.Cancel();
_linkDraw.WaitEnd();
_linkDraw = null;
}
}
private void btnStay_Click(object sender, EventArgs e)
{
_link.Cancel();
DialogResult = DialogResult.No;
Close();
}
public static Trajectory ChercheTrajectoire(Graph graph, IEnumerable <IShape> obstacles, IEnumerable <IShape> opponents, Position startPos, Position endPos, double rayonSecurite, double distanceSecuriteCote)
{
Stopwatch sw = Stopwatch.StartNew();
bool pathFound = false;
Trajectory output = DirectTrajectory(obstacles.Concat(opponents), startPos, endPos, rayonSecurite);
if (output != null)
{
pathFound = true;
}
else
{
_linkResetRadius = null;
double distance;
bool raccordable = false;
output = new Trajectory();
output.StartAngle = startPos.Angle;
output.EndAngle = endPos.Angle;
PointsTrouves = new List <RealPoint>();
PointsTrouves.Add(new RealPoint(startPos.Coordinates));
output.AddPoint(new RealPoint(startPos.Coordinates));
lock (graph)
{
Console.WriteLine("Cherche trajectoire début");
int nbPointsDepart = 0;
int nbPointsArrivee = 0;
Node debutNode = null, finNode = null;
Node nodeProche = graph.ClosestNode(startPos.Coordinates, out distance);
if (distance != 0)
{
debutNode = new Node(startPos.Coordinates);
nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite);
}
else
{
debutNode = nodeProche;
nbPointsDepart = debutNode.OutgoingArcs.Count;
}
//TODO2018 phase d'approche finale pourrie, l'angle final peut se faire à la fin au lieu de à la fin du path finding avant l'approche finale
if (nbPointsDepart == 0)
{
// On ne peut pas partir de là où on est
Position positionTestee = new Position(startPos);
bool franchissable = true;
// Boucle jusqu'à trouver un point qui se connecte au graph jusqu'à 1m devant
int i;
for (i = 0; i < 100 && !raccordable; i += 1)
{
positionTestee.Move(10);
debutNode = new Node(positionTestee.Coordinates);
raccordable = graph.Raccordable(debutNode, obstacles, rayonSecurite);
}
// Le point à i*10 mm devant nous est reliable au graph, on cherche à l'atteindre
if (raccordable)
{
Segment segmentTest = new Segment(new RealPoint(positionTestee.Coordinates), new RealPoint(startPos.Coordinates));
// Test des obstacles
foreach (IShape obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
{
franchissable = false;
// Si l'obstacle génant est un adversaire, on diminue petit à petit son rayon pour pouvoir s'échapper au bout d'un moment
if (opponents.Contains(obstacle) && GameBoard.OpponentRadius > 50)
{
Robots.MainRobot.Historique.Log("Adversaire au contact, impossible de s'enfuir, réduction du périmètre adverse", TypeLog.PathFinding);
GameBoard.OpponentRadius -= 10;
_linkResetRadius?.Cancel();
}
}
}
// Si le semgent entre notre position et le graph relié au graph est parcourable on y va !
if (franchissable)
{
PointsTrouves.Add(new RealPoint(positionTestee.Coordinates));
output.AddPoint(new RealPoint(positionTestee.Coordinates));
debutNode = new Node(positionTestee.Coordinates);
nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite);
}
}
else
{
franchissable = false;
}
// Si toujours pas, on teste en marche arrière
if (!franchissable)
{
franchissable = true;
raccordable = false;
nbPointsDepart = 0;
positionTestee = new Position(startPos);
for (i = 0; i > -100 && !raccordable; i--)
{
positionTestee.Move(-10);
debutNode = new Node(positionTestee.Coordinates);
raccordable = graph.Raccordable(debutNode, obstacles, rayonSecurite);
}
// Le point à i*10 mm derrière nous est reliable au graph, on cherche à l'atteindre
if (raccordable)
{
Segment segmentTest = new Segment(new RealPoint(positionTestee.Coordinates), new RealPoint(startPos.Coordinates));
// Test des obstacles
foreach (IShape obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
{
franchissable = false;
// Si l'obstacle génant est un adversaire, on diminue petit à petit son rayon pour pouvoir s'échapper au bout d'un moment
if (opponents.Contains(obstacle) && GameBoard.OpponentRadius > 50)
{
Robots.MainRobot.Historique.Log("Adversaire au contact, impossible de s'enfuir, réduction du périmètre adverse", TypeLog.PathFinding);
GameBoard.OpponentRadius -= 10;
_linkResetRadius?.Cancel();
}
}
}
// Si le semgent entre notre position et le graph relié au graph est parcourable on y va !
if (franchissable)
{
PointsTrouves.Add(new RealPoint(positionTestee.Coordinates));
output.AddPoint(new RealPoint(positionTestee.Coordinates));
debutNode = new Node(positionTestee.Coordinates);
nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite);
}
}
}
}
if (nbPointsDepart > 0)
{
finNode = graph.ClosestNode(endPos.Coordinates, out distance);
if (distance != 0)
{
finNode = new Node(endPos.Coordinates);
nbPointsArrivee = graph.AddNode(finNode, obstacles, rayonSecurite);
}
else
{
nbPointsArrivee = 1;
}
if (nbPointsArrivee == 0)
{
Console.WriteLine("Blocage arrivée");
// On ne peut pas arriver là où on souhaite aller
// On teste si on peut faire une approche en ligne
// teta ne doit pas être nul sinon c'est qu'on ne maitrise pas l'angle d'arrivée et on ne connait pas l'angle d'approche
Position positionTestee = new Position(endPos);
bool franchissable = true;
raccordable = false;
// Boucle jusqu'à trouver un point qui se connecte au graph jusqu'à 1m devant
int i;
for (i = 0; i < 100 && !raccordable; i++)
{
positionTestee.Move(10);
raccordable = graph.Raccordable(new Node(positionTestee.Coordinates), obstacles, rayonSecurite);
}
// Le point à i*10 mm devant nous est reliable au graph, on cherche à l'atteindre
if (raccordable)
{
Segment segmentTest = new Segment(new RealPoint(positionTestee.Coordinates), new RealPoint(endPos.Coordinates));
// Test des obstacles
foreach (IShape obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
{
franchissable = false;
}
}
}
else
{
franchissable = false;
}
// Si toujours pas, on teste en marche arrière
if (!franchissable)
{
positionTestee = new Position(endPos);
nbPointsArrivee = 0;
for (i = 0; i > -100 && !raccordable; i--)
{
positionTestee.Move(-10);
raccordable = graph.Raccordable(new Node(positionTestee.Coordinates), obstacles, rayonSecurite);
}
if (raccordable)
{
franchissable = true;
Segment segmentTest = new Segment(new RealPoint(positionTestee.Coordinates), new RealPoint(endPos.Coordinates));
// Test des obstacles
foreach (IShape obstacle in obstacles)
{
if (obstacle.Distance(segmentTest) < distanceSecuriteCote)
{
franchissable = false;
}
}
}
}
// Si le semgent entre notre position et le node relié au graph est parcourable on y va !
if (franchissable)
{
finNode = new Node(positionTestee.Coordinates);
nbPointsArrivee = graph.AddNode(finNode, obstacles, rayonSecurite);
}
}
}
if (nbPointsDepart > 0 && nbPointsArrivee > 0)
{
// Teste s'il est possible d'aller directement à la fin sans passer par le graph
bool toutDroit = true;
Segment segment = new Segment(debutNode.Position, finNode.Position);
foreach (IShape forme in obstacles)
{
if (segment.Distance(forme) < rayonSecurite)
{
toutDroit = false;
break;
}
}
if (toutDroit)
{
Robots.MainRobot.Historique.Log("Chemin trouvé : ligne droite", TypeLog.PathFinding);
pathFound = true;
PointsTrouves.Add(finNode.Position);
output.AddPoint(finNode.Position);
if (endPos.Coordinates.Distance(finNode.Position) > 1)
{
PointsTrouves.Add(new RealPoint(endPos.Coordinates));
output.AddPoint(new RealPoint(endPos.Coordinates));
}
}
// Sinon on passe par le graph
else
{
AStar aStar = new AStar(graph);
aStar.DijkstraHeuristicBalance = 0;
//Console.WriteLine("Avant pathFinding : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
if (aStar.SearchPath(debutNode, finNode))
{
//Console.WriteLine("PathFinding trouvé : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
List <Node> nodes = aStar.PathByNodes.ToList <Node>();
Robots.MainRobot.Historique.Log("Chemin trouvé : " + (nodes.Count - 2) + " noeud(s) intermédiaire(s)", TypeLog.PathFinding);
CheminEnCoursNoeuds = new List <Node>();
CheminEnCoursArcs = new List <Arc>();
//Console.WriteLine("Début simplification : " + (DateTime.Now - debut).TotalMilliseconds + "ms");
// Simplification du chemin
// On part du début et on essaie d'aller au point du plus éloigné au moins éloigné en testant si le passage est possible
// Si c'est possible on zappe tous les points entre les deux
for (int iNodeDepart = 0; iNodeDepart < nodes.Count - 1; iNodeDepart++)
{
if (iNodeDepart != 0)
{
PointsTrouves.Add(nodes[iNodeDepart].Position);
output.AddPoint(nodes[iNodeDepart].Position);
}
CheminEnCoursNoeuds.Add(nodes[iNodeDepart]);
bool raccourciPossible = true;
for (int iNodeArrivee = nodes.Count - 1; iNodeArrivee > iNodeDepart; iNodeArrivee--)
{
raccourciPossible = true;
Segment racourci = new Segment(nodes[iNodeDepart].Position, nodes[iNodeArrivee].Position);
//Arc arcRacourci = new Arc(nodes[iNodeDepart], nodes[iNodeArrivee]);
CheminTest = racourci;
//arcRacourci.Passable = false;
for (int i = obstacles.Count() - 1; i >= 4; i--) // > 4 pour ne pas tester les bordures
{
IShape forme = obstacles.ElementAt(i);
ObstacleTeste = forme;
ObstacleProbleme = null;
if (racourci.Distance(forme) < rayonSecurite)
{
ObstacleProbleme = forme;
raccourciPossible = false;
break;
}
//else if(Config.CurrentConfig.AfficheDetailTraj > 0)
// Thread.Sleep(Config.CurrentConfig.AfficheDetailTraj);
}
if (Config.CurrentConfig.AfficheDetailTraj > 0)
{
Thread.Sleep(Config.CurrentConfig.AfficheDetailTraj);
}
ObstacleTeste = null;
if (raccourciPossible)
{
//CheminEnCoursArcs.Add(arcRacourci);
iNodeDepart = iNodeArrivee - 1;
break;
}
}
CheminTest = null;
if (!raccourciPossible)
{
//Arc arc = new Arc(nodes[iNodeDepart], nodes[iNodeDepart + 1]);
//CheminEnCoursArcs.Add(arc);
}
}
CheminEnCoursNoeuds.Add(nodes[nodes.Count - 1]);
PointsTrouves.Add(nodes[nodes.Count - 1].Position);
output.AddPoint(nodes[nodes.Count - 1].Position);
Robots.MainRobot.Historique.Log("Chemin optimisé : " + (CheminEnCoursNoeuds.Count - 2) + " noeud(s) intermédiaire(s)", TypeLog.PathFinding);
pathFound = true;
if (endPos.Coordinates.Distance(finNode.Position) > 1)
{
PointsTrouves.Add(new RealPoint(endPos.Coordinates));
output.AddPoint(new RealPoint(endPos.Coordinates));
}
}
else
{
CheminEnCoursNoeuds = null;
CheminEnCoursArcs = null;
pathFound = false;
}
}
}
graph.CleanNodesArcsAdd();
}
}
Dessinateur.CurrentTrack = null;
//CheminEnCoursNoeuds = null;
Console.WriteLine("Cherche trajectoire fin : " + sw.ElapsedMilliseconds + "ms");
PointsTrouves = null;
ObstacleProbleme = null;
ObstacleTeste = null;
//Console.WriteLine("PathFinding en " + (DateTime.Now - debut).TotalMilliseconds + " ms");
if (!pathFound)
{
Robots.MainRobot.Historique.Log("Chemin non trouvé", TypeLog.PathFinding);
return(null);
}
else
{
if (GameBoard.OpponentRadius < GameBoard.OpponentRadiusInitial)
{
_linkResetRadius = ThreadManager.CreateThread(link => ResetOpponentRadiusLoop());
_linkResetRadius.StartThread();
}
if (output.Lines.Count > 1)
{
output = ReduceLines(output, obstacles.Concat(opponents), rayonSecurite);
}
return(output);
}
}
public static void Stop()
{
_linkDisplay?.Cancel();
_linkDisplay?.WaitEnd();
_linkDisplay = null;
}
protected override void StopLoop()
{
_linkMeasures.Cancel();
}
