private void LogPosition() { _linkLogPositions.RegisterName(); lock (PositionsHistorical) { PositionsHistorical.Add(new Position(Position.Angle, new RealPoint(Position.Coordinates.X, Position.Coordinates.Y))); while (PositionsHistorical.Count > 1200) { PositionsHistorical.RemoveAt(0); } } }
public void ReceptionUdpMessage(Frame frame) { // Analyser la trame reçue //Console.WriteLine(trameRecue.ToString()); switch ((UdpFrameFunction)frame[1]) { case UdpFrameFunction.RetourTension: //BatterieVoltage = (frame[2] * 256 + frame[3]) / 100f; break; case UdpFrameFunction.MoteurFin: _lockMotor[(MotorID)frame[2]]?.Release(); break; case UdpFrameFunction.MoteurBlocage: // Idem avec bip _lockMotor[(MotorID)frame[2]]?.Release(); //TODO2020AllDevices.RecGoBot.Buzz(".."); break; case UdpFrameFunction.Blocage: ThreadManager.CreateThread(AsyncAsserEnable).StartThread(); break; case UdpFrameFunction.FinDeplacement: case UdpFrameFunction.FinRecallage: // Idem Thread.Sleep(40); // TODO2018 ceci est une tempo ajoutée au pif de pwet parce qu'on avant envie alors voilà IsInLineMove = false; _lockFrame[UdpFrameFunction.FinDeplacement]?.Release(); break; case UdpFrameFunction.AsserRetourPositionXYTeta: // Réception de la position mesurée par l'asservissement try { double y = (double)((short)(frame[2] << 8 | frame[3]) / 10.0); double x = (double)((short)(frame[4] << 8 | frame[5]) / 10.0); double teta = (frame[6] << 8 | frame[7]) / 100.0 - 180; teta = (-teta); y = -y; x = -x; Position nouvellePosition = new Position(teta, new RealPoint(x, y)); if (Position.Coordinates.Distance(nouvellePosition.Coordinates) < 300 || !_positionReceived) { // On reçoit la position du robot // On la prend si elle est pas très éloignée de la position précédente ou si je n'ai jamais reçu de position Position = nouvellePosition; } else { // On pense avoir une meilleure position à lui redonner parce que la position reçue est loin de celle qu'on connait alors qu'on l'avait reçue du robot SetAsservOffset(Position); } _positionReceived = true; _lockFrame[UdpFrameFunction.AsserDemandePositionXYTeta]?.Release(); lock (PositionsHistorical) { PositionsHistorical.Add(new Position(teta, new RealPoint(x, y))); while (PositionsHistorical.Count > 1200) { PositionsHistorical.RemoveAt(0); } } OnPositionChanged(Position); } catch (Exception) { Console.WriteLine("Erreur dans le retour de position asservissement."); } break; case UdpFrameFunction.AsserRetourPositionCodeurs: int nbPositions = frame[2]; for (int i = 0; i < nbPositions; i++) { // TODO2018 peut mieux faire, décaller les bits int gauche1 = frame[3 + i * 8]; int gauche2 = frame[4 + i * 8]; int gauche3 = frame[5 + i * 8]; int gauche4 = frame[6 + i * 8]; int codeurGauche = gauche1 * 256 * 256 * 256 + gauche2 * 256 * 256 + gauche3 * 256 + gauche4; int droite1 = frame[7 + i * 8]; int droite2 = frame[8 + i * 8]; int droite3 = frame[9 + i * 8]; int droite4 = frame[10 + i * 8]; int codeurDroit = droite1 * 256 * 256 * 256 + droite2 * 256 * 256 + droite3 * 256 + droite4; _lastPidTest[0].Add(codeurGauche); _lastPidTest[1].Add(codeurDroit); } break; case UdpFrameFunction.RetourChargeCPU_PWM: int valsCount = frame[2]; for (int i = 0; i < valsCount; i++) { double cpuLoad = (frame[3 + i * 6] * 256 + frame[4 + i * 6]) / 5000.0; double pwmLeft = frame[5 + i * 6] * 256 + frame[6 + i * 6] - 4000; double pwmRight = frame[7 + i * 6] * 256 + frame[8 + i * 6] - 4000; _lastRecMoveLoad.Add(cpuLoad); _lastPwmLeft.Add(pwmLeft); _lastPwmRight.Add(pwmRight); } break; case UdpFrameFunction.RetourCapteurCouleur: //TODO2018 : multiplier par 2 pour obtenir de belles couleurs ? SensorColorID sensorColor = (SensorColorID)frame[2]; Color newColor = Color.FromArgb(Math.Min(255, frame[3] * 1), Math.Min(255, frame[4] * 1), Math.Min(255, frame[5] * 1)); if (newColor != SensorsColorValue[sensorColor]) { OnSensorColorChanged(sensorColor, newColor); } _lockSensorColor[(SensorColorID)frame[2]]?.Release(); break; case UdpFrameFunction.RetourCapteurOnOff: SensorOnOffID sensorOnOff = (SensorOnOffID)frame[2]; bool newState = frame[3] > 0 ? true : false; if (sensorOnOff == SensorOnOffID.StartTrigger) { SetStartTrigger(newState); } if (sensorOnOff == SensorOnOffID.PresenceBuoyRight && newState && Actionneur.ElevatorRight.Armed) { Actionneur.ElevatorRight.Armed = false; Actionneur.ElevatorRight.DoSequencePickupColorThread(Buoy.Red); } if (newState != SensorsOnOffValue[sensorOnOff]) { OnSensorOnOffChanged(sensorOnOff, newState); } _lockSensorOnOff[sensorOnOff]?.Release(); break; case UdpFrameFunction.RetourValeursNumeriques: Board numericBoard = (Board)frame[0]; lock (NumericPinsValue) { NumericPinsValue[numericBoard][0] = (Byte)frame[2]; NumericPinsValue[numericBoard][1] = (Byte)frame[3]; NumericPinsValue[numericBoard][2] = (Byte)frame[4]; NumericPinsValue[numericBoard][3] = (Byte)frame[5]; NumericPinsValue[numericBoard][4] = (Byte)frame[6]; NumericPinsValue[numericBoard][5] = (Byte)frame[7]; } _lockFrame[UdpFrameFunction.RetourValeursNumeriques]?.Release(); break; case UdpFrameFunction.RetourValeursAnalogiques: Board analogBoard = (Board)frame[0]; const double toVolts = 0.0008056640625; List <double> values = new List <double>(); AnalogicPinsValue[analogBoard][0] = ((frame[2] * 256 + frame[3]) * toVolts); AnalogicPinsValue[analogBoard][1] = ((frame[4] * 256 + frame[5]) * toVolts); AnalogicPinsValue[analogBoard][2] = ((frame[6] * 256 + frame[7]) * toVolts); AnalogicPinsValue[analogBoard][3] = ((frame[8] * 256 + frame[9]) * toVolts); AnalogicPinsValue[analogBoard][4] = ((frame[10] * 256 + frame[11]) * toVolts); AnalogicPinsValue[analogBoard][5] = ((frame[12] * 256 + frame[13]) * toVolts); AnalogicPinsValue[analogBoard][6] = ((frame[14] * 256 + frame[15]) * toVolts); AnalogicPinsValue[analogBoard][7] = ((frame[16] * 256 + frame[17]) * toVolts); AnalogicPinsValue[analogBoard][8] = ((frame[18] * 256 + frame[19]) * toVolts); _lockFrame[UdpFrameFunction.RetourValeursAnalogiques]?.Release(); break; case UdpFrameFunction.ReponseLidar: int lidarID = frame[2]; if (_lastLidarMeasure == null) { _lastLidarMeasure = ""; } string mess = ""; int decallageEntete = 3; if (_lastLidarMeasure.Length == 0) { // C'est le début de la trame à recomposer, et au début y'a la position de la prise de mesure à lire ! double y = (double)((short)(frame[3] << 8 | frame[4]) / 10.0); double x = (double)((short)(frame[5] << 8 | frame[6]) / 10.0); double teta = (frame[7] << 8 | frame[8]) / 100.0 - 180; _lastLidarPosition = new Position(-teta, new RealPoint(-x, -y)); decallageEntete += 6; } for (int i = decallageEntete; i < frame.Length; i++) { _lastLidarMeasure += (char)frame[i]; mess += (char)frame[i]; } if (Regex.Matches(_lastLidarMeasure, "\n\n").Count == 2) { _lockFrame[UdpFrameFunction.ReponseLidar]?.Release(); } break; case UdpFrameFunction.RetourCouleurEquipe: GameBoard.MyColor = frame[2] == 0 ? GameBoard.ColorLeftBlue : GameBoard.ColorRightYellow; if (Config.CurrentConfig.IsMiniRobot) { StartTriggerEnable = true; } break; } }