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);
}