public void ChercheTraj(Position depart) { if (!calculTraj) { calculTraj = true; DateTime debut = DateTime.Now; List<Trajectoire> trajs = new List<Trajectoire>(); /*foreach (Mouvement m in Plateau.Enchainement.ListeMouvementsGros) { if (m.PositionProche != null && m.Score > 0) { //foreach (Position pt in m.Positions) { Trajectoire traj = PathFinder.ChercheTrajectoire(Robots.GrosRobot.Graph, Plateau.ListeObstacles, depart, m.PositionProche, Robots.GrosRobot.Rayon, 160); if (traj != null) { trajs.Add(traj); Console.WriteLine(m.ToString() + " -> " + traj.Duree + "ms"); } } } } Dessinateur.Trajectoires = trajs;*/ calculTraj = false; //Console.WriteLine("Temps calcul toutes possibilités : " + (DateTime.Now - debut).TotalMilliseconds + " ms"); } }
public Hokuyo(LidarID lidar) { //trameDetails = "VV\n00P\n"; this.lidar = lidar; semLock = new Semaphore(1, 1); switch (lidar) { case LidarID.LidarSol: model = "URG-04LX-UG01"; break; } if (model.Contains("UBG-04LX-F01"))//Hokuyo bleu { nbPoints = 725; angleMesurable = new Angle(240, AnglyeType.Degre); offsetPoints = 44; } else if (model.Contains("URG-04LX-UG01")) //Petit hokuyo { nbPoints = 725; angleMesurable = new Angle(240, AnglyeType.Degre); offsetPoints = 44; } else if (model.Contains("BTM-75LX")) // Grand hokuyo { nbPoints = 1080; angleMesurable = new Angle(270, AnglyeType.Degre); offsetPoints = 0; } position = Robots.GrosRobot.Position; Robots.GrosRobot.PositionChange += GrosRobot_PositionChange; }
/// <summary> /// Retourneles coordonnées d'un point en fonction d'une position de départ et d'une direction /// </summary> /// <param name="depart">Point de départ</param> /// <param name="direction">Direction</param> /// <returns>Coordonnées du point</returns> public static PointReel GetCoordonnees(Position depart, Direction direction) { Angle angleAdverse = direction.angle + depart.Angle; double x = depart.Coordonnees.X + Math.Cos(angleAdverse.AngleRadians) * direction.distance; double y = depart.Coordonnees.Y + Math.Sin(angleAdverse.AngleRadians) * direction.distance; PointReel positionAdv = new PointReel(x, y); return positionAdv; }
public List<IActionDuree> ToActions() { List<IActionDuree> actions = new List<IActionDuree>(); Angle angle = new Angle(AngleDepart); for (int i = 0; i < PointsPassage.Count - 1; i++) { PointReel c1 = new PointReel(PointsPassage[i].X, PointsPassage[i].Y); PointReel c2 = new PointReel(PointsPassage[i + 1].X, PointsPassage[i + 1].Y); Position p = new Position(angle, c1); Direction traj = Maths.GetDirection(p, c2); // Teste si il est plus rapide (moins d'angle à tourner) de se déplacer en marche arrière bool inverse = false; if (Math.Abs(traj.angle.AngleDegres) > 90) { inverse = true; traj.angle = new Angle(traj.angle.AngleDegres - 180); } if (traj.angle.AngleDegres < 0) { actions.Add(new ActionPivot(Robots.GrosRobot, -traj.angle, SensGD.Droite)); angle -= traj.angle; } else if (traj.angle.AngleDegres > 0) { actions.Add(new ActionPivot(Robots.GrosRobot, traj.angle, SensGD.Gauche)); angle -= traj.angle; } if (inverse) actions.Add(new ActionRecule(Robots.GrosRobot, (int)(traj.distance))); else actions.Add(new ActionAvance(Robots.GrosRobot, (int)(traj.distance))); } Angle diff = angle - AngleFinal; if (Math.Abs(diff.AngleDegres) > 0.2) { if (diff.AngleDegres < 0) actions.Add(new ActionPivot(Robots.GrosRobot, -diff, SensGD.Droite)); else actions.Add(new ActionPivot(Robots.GrosRobot, diff, SensGD.Gauche)); } return actions; }
/// <summary> /// Retourne la direction (angle et distance) à suivre pour arriver à un point donné en partant d'une position précise (coordonnées et angle) /// </summary> /// <param name="depart">Position de départ</param> /// <param name="arrivee">Coordonnées d'arrivée</param> /// <returns>Direction à prendre</returns> public static Direction GetDirection(Position depart, PointReel arrivee) { Direction result = new Direction(); double distance = depart.Coordonnees.Distance(arrivee); result.distance = distance; PointReel devantRobot = new PointReel(depart.Coordonnees.X + Math.Cos(depart.Angle.AngleRadians) * 100, depart.Coordonnees.Y + Math.Sin(depart.Angle.AngleRadians) * 100); Angle angle; double angleCalc = 0; // Deux points sur le même axe vertical : 90° ou -90° selon le point le plus haut if (arrivee.X == depart.Coordonnees.X) { angleCalc = Math.PI / 2; if (arrivee.Y > depart.Coordonnees.Y) angleCalc = -angleCalc; } // Deux points sur le même axe horizontal : 0° ou 180° selon le point le plus à gauche else if (arrivee.Y == depart.Coordonnees.Y) { angleCalc = Math.PI; if (arrivee.X > depart.Coordonnees.X) angleCalc = 0; } // Cas général : Calcul de l'angle else { angleCalc = Math.Acos((arrivee.X - depart.Coordonnees.X) / distance); if (arrivee.Y > depart.Coordonnees.Y) angleCalc = -angleCalc; } // Prendre en compte l'angle initial du robot angle = new Angle(angleCalc, AnglyeType.Radian); angle = angle + depart.Angle; result.angle = angle; return result; }
private Position PositionDepuisRobot(Position robotPosition) { return new Position(robotPosition.Angle, new PointReel(robotPosition.Coordonnees.X + offsetX, robotPosition.Coordonnees.Y + offsetY).Rotation(robotPosition.Angle, robotPosition.Coordonnees)); }
void GrosRobot_PositionChange(Position robotPosition) { position = PositionDepuisRobot(robotPosition); }
public void Copie(Position position) { Angle.Set(position.Angle); Coordonnees.Placer(position.Coordonnees.X, position.Coordonnees.Y); }
public bool PathFinding(double x, double y, Angle teta = null, int timeOut = 0, bool attendre = false) { Historique.Log("Lancement pathfinding pour aller en " + x + " : " + y, TypeLog.PathFinding); Position destination = new Position(teta, new PointReel(x, y)); Trajectoire traj = PathFinder.ChercheTrajectoire(Graph, Plateau.ListeObstacles, Position, destination, Rayon, 130); Console.WriteLine(Plateau.ListeObstacles.Count + " obstacles"); if (traj == null) return false; semTrajectoire = new Semaphore(0, int.MaxValue); threadTrajectoire = new Thread(ParcourirTrajectoire); threadTrajectoire.Start(traj); if (attendre) semTrajectoire.WaitOne(); return !TrajectoireCoupee && !TrajectoireEchouee; }
public override void PivotGauche(double angle, bool attendre = true) { base.PivotGauche(angle, attendre); angle = Math.Round(angle, 2); Historique.AjouterAction(new ActionPivot(this, angle, SensGD.Gauche)); Destination = new Position(new Angle(Position.Angle.AngleDegres - angle, AnglyeType.Degre), new PointReel(Position.Coordonnees.X, Position.Coordonnees.Y)); SensPivot = SensGD.Gauche; if (attendre) while (Position.Angle != Destination.Angle) Thread.Sleep(50); }
public override void Init() { Historique = new Historique(IDRobot); HistoriqueCoordonnees = new List<Position>(); if (this == Robots.GrosRobot) { if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet) Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(240, 1000)); else Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 240, 1000)); } else { if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet) Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(480, 1000)); else Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 480, 1000)); } PositionCible = null; }
public override void Avancer(int distance, bool attendre = true) { base.Avancer(distance, attendre); DeplacementLigne = true; if (distance > 0) { if (!RecallageEnCours) Historique.AjouterAction(new ActionAvance(this, distance)); SensDep = SensAR.Avant; } else { if (!RecallageEnCours) Historique.AjouterAction(new ActionRecule(this, -distance)); SensDep = SensAR.Arriere; } double depX = distance * Math.Cos(Position.Angle.AngleRadians); double depY = distance * Math.Sin(Position.Angle.AngleRadians); Destination = new Position(Position.Angle, new PointReel(Position.Coordonnees.X + depX, Position.Coordonnees.Y + depY)); if (attendre) while (Position.Coordonnees.X != Destination.Coordonnees.X || Position.Coordonnees.Y != Destination.Coordonnees.Y) Thread.Sleep(10); }
public void ReceptionMessage(Trame trameRecue) { // Analyser la trame reçue Console.WriteLine(trameRecue.ToString()); if ((trameRecue[0] == (byte)Carte.RecMove && this == Robots.GrosRobot)) { if (trameRecue[1] == (byte)FonctionMove.Blocage) { thActivationAsser = new Thread(ActivationAsserv); thActivationAsser.Start(); } if (trameRecue[1] == (byte)FonctionMove.FinDeplacement || trameRecue[1] == (byte)FonctionMove.FinRecallage) { Console.WriteLine("Déblocage déplacement " + DateTime.Now.Hour + ":" + DateTime.Now.Minute + ":" + DateTime.Now.Second + ":" + DateTime.Now.Millisecond); SemaphoresMove[FonctionMove.FinDeplacement].Release(); } if (trameRecue[1] == (byte)FonctionMove.RetourPositionXYTeta) { // Réception de la position mesurée par l'asservissement try { double y = (double)((short)(trameRecue[2] << 8 | trameRecue[3]) / 10.0); double x = (double)((short)(trameRecue[4] << 8 | trameRecue[5]) / 10.0); double teta = (trameRecue[6] << 8 | trameRecue[7]) / 100.0 - 180; teta = (-teta); y = -y; x = -x; Position nouvellePosition = new Position(new Angle(teta, AnglyeType.Degre), new PointReel(x, y)); if (Position.Coordonnees.Distance(nouvellePosition.Coordonnees) < 300 || !positionRecue) { Position = nouvellePosition; //Position.Coordonnees.Placer(nouvellePosition.Coordonnees.X, nouvellePosition.Coordonnees.Y); } //else // ReglerOffsetAsserv((int)Position.Coordonnees.X, (int)Position.Coordonnees.Y, -Position.Angle); positionRecue = true; DateRefreshPos = DateTime.Now; SemaphoresMove[FonctionMove.DemandePositionXYTeta].Release(); HistoriqueCoordonnees.Add(new Position(teta, new PointReel(x, y))); if (HistoriqueCoordonnees.Count > 1200) { semHistoriquePosition.WaitOne(); while (HistoriqueCoordonnees.Count > 1200) HistoriqueCoordonnees.RemoveAt(0); semHistoriquePosition.Release(); } if (Plateau.Balise != null) Plateau.Balise.Position = Position; ChangerPosition(Position); } catch (Exception) { Console.WriteLine("Erreur dans le retour de position asservissement."); } } if (trameRecue[1] == (byte)FonctionMove.RetourPositionCodeurs) { int nbPositions = trameRecue[2]; for (int i = 0; i < nbPositions; i++) { int gauche1 = trameRecue[3 + i * 8]; int gauche2 = trameRecue[4 + i * 8]; int gauche3 = trameRecue[5 + i * 8]; int gauche4 = trameRecue[6 + i * 8]; int codeurGauche = gauche1 * 256 * 256 * 256 + gauche2 * 256 * 256 + gauche3 * 256 + gauche4; int droite1 = trameRecue[7 + i * 8]; int droite2 = trameRecue[8 + i * 8]; int droite3 = trameRecue[9 + i * 8]; int droite4 = trameRecue[10 + i * 8]; int codeurDroit = droite1 * 256 * 256 * 256 + droite2 * 256 * 256 + droite3 * 256 + droite4; retourTestPid[0].Add(codeurGauche); retourTestPid[1].Add(codeurDroit); } } if (trameRecue[1] == (byte)FonctionMove.RetourDiagnostic) { int nbValeurs = trameRecue[2]; for (int i = 0; i < nbValeurs; i++) { double chargeCPU = (trameRecue[3 + i * 6] * 256 + trameRecue[4 + i * 6]) / 5000.0; double chargePWMGauche = trameRecue[5 + i * 6] * 256 + trameRecue[6 + i * 6] - 4000; double chargePWMDroite = trameRecue[7 + i * 6] * 256 + trameRecue[8 + i * 6] - 4000; retourTestCharge[0].Add(chargeCPU); retourTestCharge[1].Add(chargePWMGauche); retourTestCharge[2].Add(chargePWMDroite); } } if (trameRecue[1] == (byte)FonctionMove.RetourValeursAnalogiques) { double valeurAnalogique1 = (trameRecue[2] * 256 + trameRecue[3]); double valeurAnalogique2 = (trameRecue[4] * 256 + trameRecue[5]); double valeurAnalogique3 = (trameRecue[6] * 256 + trameRecue[7]); double valeurAnalogique4 = (trameRecue[8] * 256 + trameRecue[9]); double valeurAnalogique5 = (trameRecue[10] * 256 + trameRecue[11]); double valeurAnalogique6 = (trameRecue[12] * 256 + trameRecue[13]); double valeurAnalogique1V = valeurAnalogique1 * 0.0008056640625; double valeurAnalogique2V = valeurAnalogique2 * 0.0008056640625; double valeurAnalogique3V = valeurAnalogique3 * 0.0008056640625; double valeurAnalogique4V = valeurAnalogique4 * 0.0008056640625; double valeurAnalogique5V = valeurAnalogique5 * 0.0008056640625; double valeurAnalogique6V = valeurAnalogique6 * 0.0008056640625; /* ?? GP2 1 GP2 2 ?? ?? ?? */ ValeursAnalogiquesMove = new List<double>(); ValeursAnalogiquesMove.Add(valeurAnalogique1); ValeursAnalogiquesMove.Add(valeurAnalogique2); ValeursAnalogiquesMove.Add(valeurAnalogique3); ValeursAnalogiquesMove.Add(valeurAnalogique4); ValeursAnalogiquesMove.Add(valeurAnalogique5); ValeursAnalogiquesMove.Add(valeurAnalogique6); if (SemaphoresMove[FonctionMove.RetourValeursAnalogiques] != null) SemaphoresMove[FonctionMove.RetourValeursAnalogiques].Release(); } } else if (trameRecue[0] == (byte)Carte.RecIO) { if (trameRecue[1] == (byte)FonctionIO.ReponseLidar) { int lidarID = trameRecue[2]; if (mesureLidar == null) mesureLidar = ""; for(int i = 3; i < trameRecue.Length; i++) { mesureLidar += (char)trameRecue[i]; } if (Regex.Matches(mesureLidar, "\n\n").Count == 2) SemaphoresIO[FonctionIO.ReponseLidar].Release(); } if (trameRecue[1] == (byte)FonctionIO.RetourValeursAnalogiques) { double valeurAnalogique1 = (trameRecue[2] * 256 + trameRecue[3]); double valeurAnalogique2 = (trameRecue[4] * 256 + trameRecue[5]); double valeurAnalogique3 = (trameRecue[6] * 256 + trameRecue[7]); double valeurAnalogique4 = (trameRecue[8] * 256 + trameRecue[9]); double valeurAnalogique5 = (trameRecue[10] * 256 + trameRecue[11]); double valeurAnalogique6 = (trameRecue[12] * 256 + trameRecue[13]); double valeurAnalogique7 = (trameRecue[14] * 256 + trameRecue[15]); double valeurAnalogique8 = (trameRecue[16] * 256 + trameRecue[17]); double valeurAnalogique9 = (trameRecue[18] * 256 + trameRecue[19]); double valeurAnalogique1V = valeurAnalogique1 * 0.0008056640625; double valeurAnalogique2V = valeurAnalogique2 * 0.0008056640625; double valeurAnalogique3V = valeurAnalogique3 * 0.0008056640625; double valeurAnalogique4V = valeurAnalogique4 * 0.0008056640625; double valeurAnalogique5V = valeurAnalogique5 * 0.0008056640625; double valeurAnalogique6V = valeurAnalogique6 * 0.0008056640625; double valeurAnalogique7V = valeurAnalogique7 * 0.0008056640625; double valeurAnalogique8V = valeurAnalogique8 * 0.0008056640625; double valeurAnalogique9V = valeurAnalogique9 * 0.0008056640625; /* Codeur effet hall Ascenseur Droite VadcV2 (AN1) Codeur effet hall Ascenseur Gauche Switchs Ascenseur Gauche Switchs Ascenseur Droite Switchs Bonus */ ValeursAnalogiquesIO = new List<double>(); ValeursAnalogiquesIO.Add(valeurAnalogique1); ValeursAnalogiquesIO.Add(valeurAnalogique2); ValeursAnalogiquesIO.Add(valeurAnalogique3); ValeursAnalogiquesIO.Add(valeurAnalogique4); ValeursAnalogiquesIO.Add(valeurAnalogique5); ValeursAnalogiquesIO.Add(valeurAnalogique6); ValeursAnalogiquesIO.Add(valeurAnalogique7); ValeursAnalogiquesIO.Add(valeurAnalogique8); ValeursAnalogiquesIO.Add(valeurAnalogique9); if (SemaphoresIO[FonctionIO.RetourValeursAnalogiques] != null) SemaphoresIO[FonctionIO.RetourValeursAnalogiques].Release(); } if (trameRecue[1] == (byte)FonctionIO.RetourCapteurOnOff) { CapteurOnOffID capteur = (CapteurOnOffID)trameRecue[2]; bool nouvelEtat = trameRecue[3] > 0 ? true : false; if (nouvelEtat != CapteurActive[capteur]) { ChangerEtatCapteurOnOff(capteur, nouvelEtat); } if (SemaphoresCapteurs[capteur] != null) SemaphoresCapteurs[capteur].Release(); } if (trameRecue[1] == (byte)FonctionIO.RetourTestConnexion) { TensionPack1 = (double)(trameRecue[2] * 256 + trameRecue[3]) / 100.0; TensionPack2 = (double)(trameRecue[4] * 256 + trameRecue[5]) / 100.0; } if (trameRecue[1] == (byte)FonctionIO.ReponseJack) { jackBranche = trameRecue[2] == 1 ? true : false; if (historiqueJack) Historique.AjouterAction(new ActionCapteur(this, CapteurID.Jack, jackBranche ? "branché" : "absent")); SemaphoresIO[FonctionIO.ReponseJack].Release(); } if (trameRecue[1] == (byte)FonctionIO.DepartJack) { if (Plateau.Enchainement == null) Plateau.Enchainement = new GoBot.Enchainements.EnchainementMatch(); Plateau.Enchainement.Executer(); } if (trameRecue[1] == (byte)FonctionIO.ReponseCouleurEquipe) { if (trameRecue[2] == 0) couleurEquipe = Plateau.CouleurGaucheViolet; else if (trameRecue[2] == 1) couleurEquipe = Plateau.CouleurDroiteVert; Plateau.NotreCouleur = couleurEquipe; SemaphoresIO[FonctionIO.ReponseCouleurEquipe].Release(); } if (trameRecue[1] == (byte)FonctionIO.RetourValeurCapteur) { if (trameRecue[2] == (byte)CapteurID.Balise) { // Recomposition de la trame comme si elle venait d'une balise String message = "B1 E4 " + trameRecue.ToString().Substring(9); if (Plateau.Balise != null) Plateau.Balise.connexion_NouvelleTrame(new Trame(message)); } if (trameRecue[2] == (byte)CapteurID.BaliseRapide1) { // Recomposition de la trame comme si elle venait d'une balise String message = "B1 E5 02 " + trameRecue.ToString().Substring(9); if (Plateau.Balise != null) Plateau.Balise.connexion_NouvelleTrame(new Trame(message)); } if (trameRecue[2] == (byte)CapteurID.BaliseRapide2) { // Recomposition de la trame comme si elle venait d'une balise String message = "B1 E5 01 " + trameRecue.ToString().Substring(9); if (Plateau.Balise != null) Plateau.Balise.connexion_NouvelleTrame(new Trame(message)); } } if (trameRecue[1] == (byte)FonctionIO.FrontCapteur) { // Réception des changement d'état des capteurs } } }
public override void Init() { Couleur = Color.Purple; Historique = new Historique(IDRobot); DateRefreshPos = DateTime.Now; //Enchainement = new Enchainements.HomologationEnchainement(); Connexion.NouvelleTrameRecue += new ConnexionUDP.ReceptionDelegate(ReceptionMessage); if (this == Robots.GrosRobot) Connexions.ConnexionIO.NouvelleTrameRecue += new ConnexionUDP.ReceptionDelegate(ReceptionMessage); if (this == Robots.GrosRobot) { if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet) Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(240, 1000)); else Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 240, 1000)); } else { if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet) Position = new Calculs.Position(new Angle(0, AnglyeType.Degre), new PointReel(480, 1000)); else Position = new Calculs.Position(new Angle(180, AnglyeType.Degre), new PointReel(3000 - 480, 1000)); } PositionCible = null; HistoriqueCoordonnees = new List<Position>(); Connexion.SendMessage(TrameFactory.DemandePositionContinue(100, this)); }
public static void Dessine() { DateTime prec = DateTime.Now; PositionCurseur = new PointReel(); while (Thread.CurrentThread.IsAlive && !Config.Shutdown) { // Limitation à 30FPS int sleep = 33 - (DateTime.Now - prec).Milliseconds - 1; if (sleep > 0) Thread.Sleep(sleep); prec = DateTime.Now; try { Bitmap bmp = new Bitmap(Properties.Resources.TablePlan.Width, Properties.Resources.TablePlan.Height); { Graphics g = Graphics.FromImage(bmp); g.SmoothingMode = System.Drawing.Drawing2D.SmoothingMode.AntiAlias; if (AfficheTable) DessinePlateau(g); DessineGraph(Robots.GrosRobot, g); //DessineTrajectoire(g); if (Robots.GrosRobot != null) DessineRobot(Robots.GrosRobot, g); if (AfficheHistoriqueCoordonneesGros && Robots.GrosRobot.HistoriqueCoordonnees != null) DessineHistoriqueTrajectoire(Robots.GrosRobot, g); if (AfficheObstacles) DessineObstacles(g); //if (Robots.PetitRobot != null) // DessineRobot(Robots.PetitRobot, g); if (AfficheElementsJeu) DessineElementsJeu(g); DessinePathFinding(g); DessinePositionEnnemis(g); if (AfficheLigneDetections) DessineLignesDetection(g); if (Robots.GrosRobot.PositionCible != null) { Point p = RealToScreenPosition(Robots.GrosRobot.PositionCible); g.DrawEllipse(penRougeEpais, p.X - 5, p.Y - 5, 10, 10); } // Dessin des coûts des mouvements if (Plateau.Enchainement != null) { if (AfficheCoutsMouvementsGros) DessineCoutMouvements(Robots.GrosRobot, g); } if ((modeCourant == Mode.PositionRPCentre || modeCourant == Mode.TeleportRPCentre) && positionDepart != null) { Point positionFin = positionCurseur; Bitmap bmpGrosRobot = new Bitmap(RealToScreenDistance(Robots.GrosRobot.Taille * 3), RealToScreenDistance(Robots.GrosRobot.Taille * 3)); Graphics gGros = Graphics.FromImage(bmpGrosRobot); gGros.FillRectangle(brushTransparent, 0, 0, RealToScreenDistance(Robots.GrosRobot.Taille * 2), RealToScreenDistance(Robots.GrosRobot.Taille * 2)); Direction traj = Maths.GetDirection(positionDepart, PositionCurseurTable); gGros.FillRectangle(brushNoirTresTransparent, bmpGrosRobot.Width / 2 - RealToScreenDistance(Robots.GrosRobot.Largeur / 2), bmpGrosRobot.Height / 2 - RealToScreenDistance(Robots.GrosRobot.Longueur / 2), RealToScreenDistance(Robots.GrosRobot.Largeur), RealToScreenDistance(Robots.GrosRobot.Longueur)); gGros.DrawRectangle(Plateau.NotreCouleur == Plateau.CouleurGaucheViolet ? penCouleurJ1 : penCouleurJ2, bmpGrosRobot.Width / 2 - RealToScreenDistance(Robots.GrosRobot.Largeur / 2), bmpGrosRobot.Height / 2 - RealToScreenDistance(Robots.GrosRobot.Longueur / 2), RealToScreenDistance(Robots.GrosRobot.Largeur), RealToScreenDistance(Robots.GrosRobot.Longueur)); gGros.DrawLine(Plateau.NotreCouleur == Plateau.CouleurGaucheViolet ? penCouleurJ1 : penCouleurJ2, bmpGrosRobot.Width / 2, bmpGrosRobot.Height / 2, bmpGrosRobot.Width / 2, bmpGrosRobot.Height / 2 - RealToScreenDistance(Robots.GrosRobot.Longueur / 2)); Point pointOrigine = RealToScreenPosition(positionDepart); g.DrawImage(RotateImage(bmpGrosRobot, 360 - traj.angle.AngleDegres + 90), pointOrigine.X - bmpGrosRobot.Width / 2, pointOrigine.Y - bmpGrosRobot.Height / 2); g.DrawLine(penBlancFleche, (Point)RealToScreenPosition(positionDepart), positionFin); } else if ((modeCourant == Mode.PositionRPFace || modeCourant == Mode.TeleportRPFace) && positionDepart != null) { Point positionFin = positionCurseur; Bitmap bmpGrosRobot = new Bitmap(RealToScreenDistance(Robots.GrosRobot.Taille * 3), RealToScreenDistance(Robots.GrosRobot.Taille * 3)); Graphics gGros = Graphics.FromImage(bmpGrosRobot); gGros.FillRectangle(brushTransparent, 0, 0, RealToScreenDistance(Robots.GrosRobot.Taille * 2), RealToScreenDistance(Robots.GrosRobot.Taille * 2)); Direction traj = Maths.GetDirection(positionDepart, PositionCurseurTable); Point pointOrigine = RealToScreenPosition(positionDepart); Position departRecule = new Position(360 - traj.angle, pointOrigine); departRecule.Avancer(RealToScreenDistance(-Robots.GrosRobot.Longueur / 2)); gGros.FillRectangle(brushNoirTresTransparent, bmpGrosRobot.Width / 2 - RealToScreenDistance(Robots.GrosRobot.Largeur / 2), bmpGrosRobot.Height / 2 - RealToScreenDistance(Robots.GrosRobot.Longueur / 2), RealToScreenDistance(Robots.GrosRobot.Largeur), RealToScreenDistance(Robots.GrosRobot.Longueur)); gGros.DrawRectangle(Plateau.NotreCouleur == Plateau.CouleurGaucheViolet ? penCouleurJ1 : penCouleurJ2, bmpGrosRobot.Width / 2 - RealToScreenDistance(Robots.GrosRobot.Largeur / 2), bmpGrosRobot.Height / 2 - RealToScreenDistance(Robots.GrosRobot.Longueur / 2), RealToScreenDistance(Robots.GrosRobot.Largeur), RealToScreenDistance(Robots.GrosRobot.Longueur)); gGros.DrawLine(Plateau.NotreCouleur == Plateau.CouleurGaucheViolet ? penCouleurJ1 : penCouleurJ2, bmpGrosRobot.Width / 2, bmpGrosRobot.Height / 2, bmpGrosRobot.Width / 2, bmpGrosRobot.Height / 2 - RealToScreenDistance(Robots.GrosRobot.Longueur / 2)); g.DrawImage(RotateImage(bmpGrosRobot, 360 - traj.angle.AngleDegres + 90), (int)(departRecule.Coordonnees.X) - bmpGrosRobot.Width / 2, (int)(departRecule.Coordonnees.Y) - bmpGrosRobot.Height / 2); g.DrawLine(penBlancFleche, (Point)RealToScreenPosition(positionDepart), positionFin); } // Trajectoires foreach (Trajectoire traj in Trajectoires) { DessineTrajectoire(traj, g); } if (Robots.GrosRobot.TrajectoireEnCours != null) { Trajectoire traj = new Trajectoire(); traj.AjouterPoint(Robots.GrosRobot.Position.Coordonnees); for (int iPoint = 1; iPoint < Robots.GrosRobot.TrajectoireEnCours.PointsPassage.Count; iPoint++) traj.AjouterPoint(Robots.GrosRobot.TrajectoireEnCours.PointsPassage[iPoint]); DessineTrajectoire(traj, g); } // Trajectoire polaire //if (modeCourant == Mode.TrajectoirePolaire) { if (trajectoirePolaireScreen !=null) foreach (Point p in trajectoirePolaireScreen) g.FillEllipse(Brushes.Red, p.X - 1, p.Y - 1, 2, 2); if (pointsPolaireScreen != null) foreach (Point p in pointsPolaireScreen) g.DrawEllipse(Pens.Black, p.X - 3, p.Y - 3, 6, 6); } TableDessinee(bmp); } } catch (Exception ex) { Console.WriteLine("Erreur pendant le dessin de la table " + ex.Message); } } }
public static Direction GetDirection(PointReel depart, PointReel arrivee) { Position positionDepart = new Position(0, depart); return GetDirection(positionDepart, arrivee); }
public override void ReglerOffsetAsserv(int offsetX, int offsetY, double offsetTeta) { Position = new Position(new Angle(-offsetTeta, AnglyeType.Degre), new PointReel(offsetX, offsetY)); PositionCible = new PointReel(offsetX, offsetY); ChangerPosition(Position); }
private void pictureBoxTable_MouseUp(object sender, MouseEventArgs e) { if (pSelected != -1) pSelected = -1; if (Dessinateur.modeCourant == Dessinateur.Mode.PositionRPCentre || Dessinateur.modeCourant == Dessinateur.Mode.TeleportRPCentre) { Direction traj = Maths.GetDirection(Dessinateur.positionDepart, Dessinateur.ScreenToRealPosition(pictureBoxTable.PointToClient(MousePosition))); positionArrivee = new Position(traj.angle, Dessinateur.positionDepart); if (Dessinateur.modeCourant == Dessinateur.Mode.PositionRPCentre) { thGoToRP = new Thread(ThreadTrajectoireGros); thGoToRP.Start(); } else Robots.GrosRobot.ReglerOffsetAsserv((int)positionArrivee.Coordonnees.X, (int)positionArrivee.Coordonnees.Y, positionArrivee.Angle.AngleDegresPositif); Dessinateur.modeCourant = Dessinateur.Mode.Visualisation; } else if (Dessinateur.modeCourant == Dessinateur.Mode.PositionRPFace || Dessinateur.modeCourant == Dessinateur.Mode.TeleportRPFace) { Point positionFin = pictureBoxTable.PointToClient(MousePosition); Direction traj = Maths.GetDirection(Dessinateur.positionDepart, Dessinateur.ScreenToRealPosition(pictureBoxTable.PointToClient(MousePosition))); Point pointOrigine = Dessinateur.positionDepart; Position departRecule = new Position(360 - traj.angle, pointOrigine); departRecule.Avancer(-Robots.GrosRobot.Longueur / 2); departRecule = new Position(traj.angle, new PointReel(departRecule.Coordonnees.X, departRecule.Coordonnees.Y)); positionArrivee = departRecule; if (Dessinateur.modeCourant == Dessinateur.Mode.PositionRPFace) { thGoToRP = new Thread(ThreadTrajectoireGros); thGoToRP.Start(); } else { Robots.GrosRobot.ReglerOffsetAsserv((int)positionArrivee.Coordonnees.X, (int)positionArrivee.Coordonnees.Y, positionArrivee.Angle.AngleDegresPositif); } Dessinateur.modeCourant = Dessinateur.Mode.Visualisation; } Dessinateur.sourisClic = false; }
public override void Stop(StopMode mode) { Historique.AjouterAction(new ActionStop(this, mode)); SemDeplacement.WaitOne(); if (mode == StopMode.Smooth) { Position nouvelleDestination = new Calculs.Position(new Angle(Position.Angle.AngleDegres), new PointReel(position.Coordonnees.X, position.Coordonnees.Y)); if (DeplacementLigne) { if (SensDep == SensAR.Avant) nouvelleDestination.Avancer(DistanceFreinageActuelle); else nouvelleDestination.Avancer(-DistanceFreinageActuelle); } Destination = nouvelleDestination; } else if (mode == StopMode.Abrupt) { VitesseActuelle = 0; Destination = Position; } SemDeplacement.Release(); }
public static Trajectoire ChercheTrajectoire(Graph graph, List<IForme> obstacles, Position positionActuell, Position destination, double rayonSecurite, double distanceSecuriteCote) { DateTime debut = DateTime.Now; double distanceRaccordable = 150; double distance; bool cheminTrouve = false; bool raccordable = false; Trajectoire trajectoire = new Trajectoire(); trajectoire.AngleDepart = new Angle(positionActuell.Angle); trajectoire.AngleFinal = new Angle(destination.Angle); PointsTrouves = new List<PointReel>(); PointsTrouves.Add(new PointReel(positionActuell.Coordonnees)); trajectoire.AjouterPoint(new PointReel(positionActuell.Coordonnees)); Synchronizer.Lock(graph); int nbPointsDepart = 0; int nbPointsArrivee = 0; Node debutNode; Node nodeProche = graph.ClosestNode(positionActuell.Coordonnees.X, positionActuell.Coordonnees.Y, 0, out distance, false); if (distance != 0) { debutNode = new Node(positionActuell.Coordonnees.X, positionActuell.Coordonnees.Y, 0); nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite, distanceRaccordable); } else debutNode = nodeProche; if (nbPointsDepart == 0) { // On ne peut pas partir de là où on est Position positionTestee = new Position(positionActuell); 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.Avancer(10); debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0); raccordable = graph.Raccordable(debutNode, obstacles, rayonSecurite, distanceRaccordable); } // Le point à i*10 mm devant nous est reliable au graph, on cherche à l'atteindre if (raccordable) { Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(positionActuell.Coordonnees)); // Test des obstacles Synchronizer.Lock(Plateau.ObstaclesBalise); foreach (IForme 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 (Plateau.ObstaclesBalise.Contains(obstacle) && Plateau.RayonAdversaire > 50) { Robots.GrosRobot.Historique.Log("Adversaire au contact, impossible de s'enfuir, réduction du périmètre adverse", TypeLog.PathFinding); Plateau.RayonAdversaire -= 10; } } } Synchronizer.Unlock(Plateau.ObstaclesBalise); // Si le semgent entre notre position et le graph relié au graph est parcourable on y va ! if (franchissable) { PointsTrouves.Add(new PointReel(positionTestee.Coordonnees)); trajectoire.AjouterPoint(new PointReel(positionTestee.Coordonnees)); debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0); nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite, distanceRaccordable); } } else franchissable = false; // Si toujours pas, on teste en marche arrière if (!franchissable) { franchissable = true; nbPointsDepart = 0; positionTestee = new Position(positionActuell); for (i = 0; i > -100 && !raccordable; i--) { positionTestee.Avancer(-10); debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0); raccordable = graph.Raccordable(debutNode, obstacles, rayonSecurite, distanceRaccordable); } // Le point à i*10 mm derrière nous est reliable au graph, on cherche à l'atteindre if (raccordable) { Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(destination.Coordonnees)); Synchronizer.Lock(Plateau.ObstaclesBalise); // Test des obstacles foreach (IForme 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 (Plateau.ObstaclesBalise.Contains(obstacle) && Plateau.RayonAdversaire > 50) { Robots.GrosRobot.Historique.Log("Adversaire au contact, impossible de s'enfuir, réduction du périmètre adverse", TypeLog.PathFinding); Plateau.RayonAdversaire -= 10; } } } Synchronizer.Unlock(Plateau.ObstaclesBalise); // Si le semgent entre notre position et le graph relié au graph est parcourable on y va ! if (franchissable) { PointsTrouves.Add(new PointReel(positionTestee.Coordonnees)); trajectoire.AjouterPoint(new PointReel(positionTestee.Coordonnees)); debutNode = new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0); nbPointsDepart = graph.AddNode(debutNode, obstacles, rayonSecurite, distanceRaccordable); } } } } Node finNode = graph.ClosestNode(destination.Coordonnees.X, destination.Coordonnees.Y, 0, out distance, false); if (distance != 0) { finNode = new Node(destination.Coordonnees.X, destination.Coordonnees.Y, 0); nbPointsArrivee = graph.AddNode(finNode, obstacles, rayonSecurite, distanceRaccordable); } if (nbPointsArrivee == 0) { Console.WriteLine("Blocage arrivée : " + (DateTime.Now - debut).TotalMilliseconds + "ms"); // 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(destination); 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++) { positionTestee.Avancer(10); raccordable = graph.Raccordable(new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0), obstacles, rayonSecurite, distanceRaccordable); } // Le point à i*10 mm devant nous est reliable au graph, on cherche à l'atteindre if (raccordable) { Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(positionActuell.Coordonnees)); // Test des obstacles foreach (IForme 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(destination); nbPointsArrivee = 0; for (i = 0; i > -100 && !raccordable; i--) { positionTestee.Avancer(-10); raccordable = graph.Raccordable(new Node(positionTestee.Coordonnees.X, positionTestee.Coordonnees.Y, 0), obstacles, rayonSecurite, distanceRaccordable); } if (raccordable) { franchissable = true; Segment segmentTest = new Segment(new PointReel(positionTestee.Coordonnees), new PointReel(destination.Coordonnees)); // Test des obstacles foreach (IForme 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.Coordonnees.X, positionTestee.Coordonnees.Y, 0); nbPointsArrivee = graph.AddNode(finNode, obstacles, rayonSecurite, distanceRaccordable); } } Synchronizer.Unlock(graph); // Teste s'il est possible d'aller directement à la fin sans passer par le graph bool toutDroit = true; Segment segment = new Segment(new PointReel(debutNode.X, debutNode.Y), new PointReel(finNode.X, finNode.Y)); foreach (IForme forme in obstacles) { if (segment.Distance(forme) < rayonSecurite) { toutDroit = false; break; } } if (toutDroit) { Robots.GrosRobot.Historique.Log("Chemin trouvé : ligne droite", TypeLog.PathFinding); cheminTrouve = true; PointsTrouves.Add(new PointReel(finNode.X, finNode.Y)); trajectoire.AjouterPoint(new PointReel(finNode.X, finNode.Y)); if (destination.Coordonnees.Distance(new PointReel(finNode.X, finNode.Y)) > 1) { PointsTrouves.Add(new PointReel(destination.Coordonnees)); trajectoire.AjouterPoint(new PointReel(destination.Coordonnees)); } } // Sinon on passe par le graph else { Synchronizer.Lock(graph); AStar aStar = new AStar(graph); aStar.DijkstraHeuristicBalance = 1; //Console.WriteLine("Avant pathFinding : " + (DateTime.Now - debut).TotalMilliseconds + "ms"); if (aStar.SearchPath(debutNode, finNode)) { Synchronizer.Unlock(graph); //Console.WriteLine("PathFinding trouvé : " + (DateTime.Now - debut).TotalMilliseconds + "ms"); List<Node> nodes = aStar.PathByNodes.ToList<Node>(); List<Arc> arcs = aStar.PathByArcs.ToList<Arc>(); Robots.GrosRobot.Historique.Log("Chemin trouvé : " + (nodes.Count - 2) + " noeud(s) intermédiaire(s)", TypeLog.PathFinding); CheminEnCoursNoeuds = new List<Node>(); CheminEnCoursArcs = new List<Arc>(); CheminTrouve = new List<Arc>(arcs); NodeTrouve = new List<Node>(nodes); //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(new PointReel(nodes[iNodeDepart].X, nodes[iNodeDepart].Y)); trajectoire.AjouterPoint(new PointReel(nodes[iNodeDepart].X, nodes[iNodeDepart].Y)); } CheminEnCoursNoeuds.Add(nodes[iNodeDepart]); bool raccourciPossible = true; for (int iNodeArrivee = nodes.Count - 1; iNodeArrivee > iNodeDepart; iNodeArrivee--) { raccourciPossible = true; Segment racourci = new Segment(new PointReel(nodes[iNodeDepart].X, nodes[iNodeDepart].Y), new PointReel(nodes[iNodeArrivee].X, nodes[iNodeArrivee].Y)); //Arc arcRacourci = new Arc(nodes[iNodeDepart], nodes[iNodeArrivee]); //CheminTest = arcRacourci; //arcRacourci.Passable = false; for (int i = obstacles.Count - 1; i >= 4; i--) // > 4 pour ne pas tester les bordures { IForme forme = obstacles[i]; ObstacleTeste = forme; ObstacleProbleme = null; if (racourci.Distance(forme) < rayonSecurite) { ObstacleProbleme = forme; // Tempo pour l'affichage détaillé de la recherche de trajectoire (option) if (Config.CurrentConfig.AfficheDetailTraj > 0) Thread.Sleep(Config.CurrentConfig.AfficheDetailTraj); 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(new PointReel(nodes[nodes.Count - 1].X, nodes[nodes.Count - 1].Y)); trajectoire.AjouterPoint(new PointReel(nodes[nodes.Count - 1].X, nodes[nodes.Count - 1].Y)); Robots.GrosRobot.Historique.Log("Chemin optimisé : " + (CheminEnCoursNoeuds.Count - 2) + " noeud(s) intermédiaire(s)", TypeLog.PathFinding); cheminTrouve = true; if (destination.Coordonnees.Distance(new PointReel(finNode.X, finNode.Y)) > 1) { PointsTrouves.Add(new PointReel(destination.Coordonnees)); trajectoire.AjouterPoint(new PointReel(destination.Coordonnees)); } } else { Synchronizer.Unlock(graph); CheminEnCoursNoeuds = null; CheminEnCoursArcs = null; cheminTrouve = false; } } Synchronizer.Lock(graph); graph.CleanNodesArcsAdd(); Synchronizer.Unlock(graph); PointsTrouves = null; ObstacleProbleme = null; ObstacleTeste = null; NodeTrouve = new List<Node>(); CheminTrouve = new List<Arc>(); //Console.WriteLine("PathFinding en " + (DateTime.Now - debut).TotalMilliseconds + " ms"); if (!cheminTrouve) { Robots.GrosRobot.Historique.Log("Chemin non trouvé", TypeLog.PathFinding); return null; } else { if (Plateau.RayonAdversaire < Plateau.RayonAdversaireInitial) { threadRAZRayonAdverse = new Thread(ThreadRAZRayonAdverse); threadRAZRayonAdverse.Start(); } return trajectoire; } }
//DateTime prec = DateTime.Now; void timerDeplacement_Elapsed(object sender, ElapsedEventArgs e) { // Calcul du temps écoulé depuis la dernière mise à jour de la position double intervalle = 0; if (watch != null) { intervalle = watch.ElapsedMilliseconds; watch.Restart(); } else watch = Stopwatch.StartNew(); //prec = DateTime.Now; SemDeplacement.WaitOne(); if (pointCourantTrajPolaire >= 0) { double distanceAvantProchainPoint = Position.Coordonnees.Distance(trajectoirePolaire[pointCourantTrajPolaire]); double distanceTotaleRestante = distanceAvantProchainPoint; distanceTotaleRestante += DistanceParcours(trajectoirePolaire, pointCourantTrajPolaire, trajectoirePolaire.Count - 1); if (distanceTotaleRestante > DistanceFreinageActuelle) VitesseActuelle = Math.Min(VitesseDeplacement, VitesseActuelle + AccelerationDebutDeplacement / (1000.0 / intervalle)); else VitesseActuelle = VitesseActuelle - AccelerationDebutDeplacement / (1000.0 / intervalle); double distanceAParcourir = VitesseActuelle / (1000.0 / intervalle); double distanceTestee = distanceAvantProchainPoint; bool changePoint = false; while (distanceTestee < distanceAParcourir && pointCourantTrajPolaire < trajectoirePolaire.Count - 1) { pointCourantTrajPolaire++; distanceTestee += trajectoirePolaire[pointCourantTrajPolaire - 1].Distance(trajectoirePolaire[pointCourantTrajPolaire]); changePoint = true; } Segment seg = null; Cercle cer = null; if(changePoint) { seg = new Segment(trajectoirePolaire[pointCourantTrajPolaire - 1], trajectoirePolaire[pointCourantTrajPolaire]); cer = new Cercle(trajectoirePolaire[pointCourantTrajPolaire - 1], distanceAParcourir - (distanceTestee - trajectoirePolaire[pointCourantTrajPolaire - 1].Distance(trajectoirePolaire[pointCourantTrajPolaire]))); } else { seg = new Segment(Position.Coordonnees, trajectoirePolaire[pointCourantTrajPolaire]); cer = new Cercle(Position.Coordonnees, distanceAParcourir); } PointReel newPos = seg.Croisements(cer)[0]; Angle a = -Maths.GetDirection(newPos, trajectoirePolaire[pointCourantTrajPolaire]).angle; position = new Position(a, newPos); ChangerPosition(Position); if (pointCourantTrajPolaire == trajectoirePolaire.Count - 1) { pointCourantTrajPolaire = -1; Destination.Copie(Position); } } else { double difference = Destination.Coordonnees.Distance(Position.Coordonnees); Angle adifference = Position.Angle - Destination.Angle; if (Math.Abs(adifference.AngleDegres) > 0.01)// Math.Round(Position.Angle.AngleDegres, 2) != Math.Round(Destination.Angle.AngleDegres, 2)) { Angle diff = Destination.Angle - Position.Angle; int coeff = 1; if (SensPivot == SensGD.Gauche) coeff = -1; double distance = Math.Abs(adifference.AngleDegres) / 360.0 * Entraxe * Math.PI; if (distance > AngleFreinageActuel) VitesseActuelle = Math.Min(VitessePivot, VitesseActuelle + AccelerationPivot / (1000.0 / intervalle)); else VitesseActuelle = VitesseActuelle - AccelerationPivot / (1000.0 / intervalle); if (Math.Abs(diff.AngleDegres) >= VitesseActuelle / (1000.0 / intervalle)) Position.Angle.Tourner(coeff * VitesseActuelle / (1000.0 / intervalle)); else if (Math.Abs(diff.AngleDegres) <= -VitesseActuelle / (1000.0 / intervalle)) Position.Angle.Tourner(coeff * VitesseActuelle / (1000.0 / intervalle)); else Position.Angle.Tourner(diff.AngleDegres); ChangerPosition(Position); } else if (difference > 0) { // Phase accélération ou déccélération if (Position.Coordonnees.Distance(Destination.Coordonnees) > DistanceFreinageActuelle) VitesseActuelle = Math.Min(VitesseDeplacement, VitesseActuelle + AccelerationDebutDeplacement / (1000.0 / intervalle)); else VitesseActuelle = VitesseActuelle - AccelerationDebutDeplacement / (1000.0 / intervalle); double distance = VitesseActuelle / (1000.0 / intervalle); //VitesseActuelle += AccellerationDeplacement / (1000 / IntervalleRafraichissementPosition); if (Destination.Coordonnees.Distance(Position.Coordonnees) <= distance) { VitesseActuelle = 0; //Position = Destination;//.Avancer(Destination.Coordonnees.Distance(Position.Coordonnees)); //Position.Coordonnees.X = Destination.Coordonnees.X; //Position.Coordonnees.Y = Destination.Coordonnees.Y; //Position.Angle.Set(Destination.Angle); //Position.Coordonnees.Placer(Destination.Coordonnees.X, Destination.Coordonnees.Y); Position.Copie(Destination); DeplacementLigne = false; } else { if (SensDep == SensAR.Avant) Position.Avancer(distance); else Position.Avancer(-distance); } ChangerPosition(Position); } } SemDeplacement.Release(); }
/// <summary> /// Génère l'évènement de changement de position /// </summary> /// <param name="position">Nouvelle position</param> protected void ChangerPosition(Position position) { if (PositionChange != null) PositionChange(position); }
/// <summary> /// Constructeur par copie /// </summary> public Position(Position other) { Angle = new Angle(other.Angle.AngleDegres); Coordonnees = new PointReel(other.Coordonnees); }