void Stats_NouvelleDonnee(TimeSpan temps, int pwm, DetectionBalise detection1, DetectionBalise detection2) { ctrlGraphiqueAngle1.AjouterPoint("Angle 1", detection1.AngleCentral, Color.Green); ctrlGraphiqueDistance1.AjouterPoint("Distance 1", detection1.Distance, Color.Red); ctrlGraphiqueAngle2.AjouterPoint("Angle 2", detection2.AngleCentral, Color.Green); ctrlGraphiqueDistance2.AjouterPoint("Distance 2", detection2.Distance, Color.Red); ctrlGraphiqueTemps.AjouterPoint("Temps(ms)", temps.TotalMilliseconds, Color.Blue); ctrlGraphiquePWM.AjouterPoint("PWM", pwm, Color.DarkSalmon); this.Invoke(eventGraphiques); }
/// <summary> /// Réception d'un message envoyé par la carte de la balise /// </summary> /// <param name="trame">Message reçu</param> public void connexion_NouvelleTrame(Trame trame) { if (trame == null) return; try { // On ne traite que les messages qui nous sont adressés if (trame[0] != (byte)0xB1) return; if (trame[1] == (byte)FonctionBalise.DetectionRapide) { if (Position != null) { int capteur = trame[2]; double debut = 360 - ((trame[3] * 256 + trame[4]) / 100.0) + Config.CurrentConfig.GetOffsetBalise(capteur); double fin = 360 - ((trame[5] * 256 + trame[6]) / 100.0) + Config.CurrentConfig.GetOffsetBalise(capteur); debut = debut + Position.Angle; fin = fin + Position.Angle; if (DetectionsRapides == null) DetectionsRapides = new List<DetectionBalise>(); DetectionBalise detect = new DetectionBalise(this, debut, fin); bool recalcul = false; if (capteur == 2 && detect.Distance > 1250) { recalcul = true; detect.Distance = 1250; } else if (capteur == 2 && detect.Distance < 480) { recalcul = true; detect.Distance = 480; } else if (capteur == 3 && detect.Distance > 600) { recalcul = true; detect.Distance = 600; } else if (capteur == 3 && detect.Distance < 300) { recalcul = true; detect.Distance = 300; } if (recalcul) { // Un peu de trigo pas bien compliquée double xPoint = Position.Coordonnees.X + Math.Cos(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance; double yPoint = Position.Coordonnees.Y + Math.Sin(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance; detect.Position = new PointReel(xPoint, yPoint); } DetectionsRapides.Add(detect); nbDetectionsRapides++; PositionsChange(); } } if (trame[1] == (byte)FonctionBalise.Detection) { if (Position != null) { if (DetectionsRapides != null) { // Vide les détections rapides datant de plus d'un tour for (int i = 0; i < DetectionsRapides.Count - nbDetectionsRapides; i++) DetectionsRapides.RemoveAt(0); } nbDetectionsRapides = 0; // Réception d'une mesure sur un tour de rotation // Vérification checksum // Calcul de la vitesse de rotation int nbTicks = trame[2] * 256 + trame[3]; VitesseToursSecActuelle = 1 / (nbTicks * 0.0000064); int nouvelleVitesse = 0; if (ReglageVitesse) nouvelleVitesse = AsservissementVitesse(); // Réception des données angulaires int nbMesures1 = trame[4]; int nbMesures2 = trame[5]; // Si on a un nombre impair de fronts on laisse tomber cette mesure, elle n'est pas bonne if (nbMesures1 % 2 != 0 || nbMesures2 % 2 != 0) { Console.WriteLine("Erreur de détection (fronts impairs)"); return; } nbMesures1 = nbMesures1 / 2; nbMesures2 = nbMesures2 / 2; // Vérification de la taille de la trame if (trame.Length != 6 + nbMesures1 * 4 + nbMesures2 * 4) { Console.WriteLine("Erreur de taille de trame"); return; } // Réception des mesures du capteur 1 DetectionsCapteur1.Clear(); List<int> tabAngle = new List<int>(); long verif = 0; for (int i = 0; i < nbMesures1 * 4; i += 2) { int valeur = trame[6 + i] * 256 + trame[6 + i + 1]; tabAngle.Add(valeur); verif += valeur * (i / 2 + 1); } tabAngle.Sort(); for (int i = 0; i < nbMesures1 * 2; i++) { verif -= tabAngle[i] * (i + 1); } if (verif != 0) { Console.WriteLine("Inversion détectée capteur 1"); } for (int i = 0; i < nbMesures1; i++) { double debut = 360 - (tabAngle[i * 2] / 100.0) + Config.CurrentConfig.GetOffsetBalise(1); double fin = 360 - (tabAngle[i * 2 + 1] / 100.0) + Config.CurrentConfig.GetOffsetBalise(1); debut = debut + Position.Angle; fin = fin + Position.Angle; DetectionBalise detect = new DetectionBalise(this, debut, fin); bool recalcul = false; if (detect.Distance > 600) { recalcul = true; detect.Distance = 600; } else if (detect.Distance < 300) { recalcul = true; detect.Distance = 300; } if (recalcul) { // Un peu de trigo pas bien compliquée double xPoint = Position.Coordonnees.X + Math.Cos(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance; double yPoint = Position.Coordonnees.Y + Math.Sin(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance; detect.Position = new PointReel(xPoint, yPoint); } DetectionsCapteur1.Add(detect); } // Réception des mesures du capteur 2 int offSet = nbMesures1 * 4 + 6; DetectionsCapteur2.Clear(); // tableau pour trier les angles ( correction logiciel ) tabAngle.Clear(); verif = 0; for (int i = 0; i < nbMesures2 * 4; i += 2) { int valeur = trame[offSet + i] * 256 + trame[offSet + i + 1]; tabAngle.Add(valeur); verif += valeur * (i / 2 + 1); } tabAngle.Sort(); for (int i = 0; i < nbMesures2 * 2; i++) { verif -= tabAngle[i] * (i + 1); } if (verif != 0) { Console.WriteLine("Inversion détectée capteur 2"); } for (int i = 0; i < nbMesures2; i++) { double debut = 360 - (tabAngle[i * 2] / 100.0) + Config.CurrentConfig.GetOffsetBalise(2); double fin = 360 - (tabAngle[i * 2 + 1] / 100.0) + Config.CurrentConfig.GetOffsetBalise(2); debut = debut + Position.Angle; fin = fin + Position.Angle; DetectionBalise detect = new DetectionBalise(this, debut, fin); bool recalcul = false; if (detect.Distance > 1250) { recalcul = true; detect.Distance = 1250; } else if (detect.Distance < 480) { recalcul = true; detect.Distance = 480; } if (recalcul) { // Un peu de trigo pas bien compliquée double xPoint = Position.Coordonnees.X + Math.Cos(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance; double yPoint = Position.Coordonnees.Y + Math.Sin(Maths.DegreeToRadian(detect.AngleCentral)) * detect.Distance; detect.Position = new PointReel(xPoint, yPoint); } DetectionsCapteur2.Add(detect); } // Réglage de l'offset d'angle des capteurs if (ReglageOffset) { // Si on a une mesure incorrecte (une mesure correcte demande 2 détections sur chaque balise : un reflecteur au "centre" et un autre en bas de la piste) // Le réglage est annulé /*if (DetectionsCapteur1.Count == 2 && DetectionsCapteur2.Count == 2) { // 3 cas où on passe d'abord par la balise en bas de la piste avant celle du milieu (sens de rotation anti-horaire) if (Carte == GoBot.Carte.RecBun && Plateau.NotreCouleur == Plateau.CouleurGaucheJaune || Carte == GoBot.Carte.RecBeu && Plateau.NotreCouleur == Plateau.CouleurGaucheJaune || Carte == GoBot.Carte.RecBoi && Plateau.NotreCouleur == Plateau.CouleurDroiteVert) { DetectionsCapteur1.RemoveAt(0); DetectionsCapteur2.RemoveAt(0); } if (ReglageOffset) { compteurReglageOffset--; // On ajoute les angles mesurés à l'historique anglesMesuresPourOffsetCapteur2.Add(DetectionsCapteur2[0].AngleCentral); anglesMesuresPourOffsetCapteur1.Add(DetectionsCapteur1[0].AngleCentral); } }*/ if (DetectionsCapteur1.Count == 1 && DetectionsCapteur2.Count == 1) { compteurReglageOffset--; // On ajoute les angles mesurés à l'historique anglesMesuresPourOffsetCapteur2.Add(DetectionsCapteur2[0].AngleCentral); anglesMesuresPourOffsetCapteur1.Add(DetectionsCapteur1[0].AngleCentral); } if (ReglageOffset && compteurReglageOffset == 0) { // Compteur arrivé à la fin, on calcule l'offset double moyenne = 0; foreach (double dv in anglesMesuresPourOffsetCapteur1) moyenne += dv; moyenne /= anglesMesuresPourOffsetCapteur1.Count; // Calcul l'offset en fonction de ce qu'il est censé mesurer au centre moyenne = 0 - moyenne; // On le sauve dans la config (haut) Config.CurrentConfig.SetOffsetBalise(1, moyenne + Config.CurrentConfig.GetOffsetBalise(1)); moyenne = 0; foreach (double dv in anglesMesuresPourOffsetCapteur2) moyenne += dv; moyenne /= anglesMesuresPourOffsetCapteur2.Count; moyenne = 0 - moyenne; // On le sauve dans la config (bas) Config.CurrentConfig.SetOffsetBalise(2, moyenne + Config.CurrentConfig.GetOffsetBalise(2)); Config.Save(); // Réglage terminé ReglageOffset = false; if (CalibrationAngulaireTerminee != null) CalibrationAngulaireTerminee(); } } Detections = new List<DetectionBalise>(); Detections.AddRange(DetectionsCapteur1); Detections.AddRange(DetectionsCapteur2); // Retire les détections correspondant à la position de robots alliés if (!ReglageOffset && Plateau.ReflecteursNosRobots) { foreach (Robot robot in Robots.DicRobots.Values) { for (int i = 0; i < Detections.Count; i++) { DetectionBalise detection = Detections[i]; // Calcul du 3ème point du triangle rectangle Balise / Gros robot Droite droiteBalise0Degres = new Droite(Position.Coordonnees, new PointReel(Position.Coordonnees.X + 500, Position.Coordonnees.Y)); Droite perpendiculaire = droiteBalise0Degres.GetPerpendiculaire(robot.Position.Coordonnees); PointReel troisiemePoint = perpendiculaire.getCroisement(droiteBalise0Degres); double distanceBaliseTroisiemePoint = troisiemePoint.Distance(Position.Coordonnees); double distanceBaliseRobot = robot.Position.Coordonnees.Distance(Position.Coordonnees); double a = Math.Acos(distanceBaliseTroisiemePoint / distanceBaliseRobot); Angle angleGrosRobot = new Angle(a, AnglyeType.Radian); Angle angleDetection = new Angle(detection.AngleCentral); double marge = 4; if (Plateau.NotreCouleur == Plateau.CouleurDroiteVert) { Angle diff = new Angle(180) - (angleDetection - angleGrosRobot); if (Math.Abs((diff).AngleDegres) < marge) { Detections.RemoveAt(i); i--; } } else if (Plateau.NotreCouleur == Plateau.CouleurGaucheViolet) { Angle diff = angleGrosRobot - angleDetection; if (Math.Abs((diff).AngleDegres) < marge) { Detections.RemoveAt(i); i--; } } } } } // Génération de l'event de notification de détection PositionsChange(); Actualisation(); } } } catch (Exception) { } Connexion.ConnexionCheck.MajConnexion(); }