static public Frame DemandeCapteurCouleur(Board board, SensorColorID capteur) { byte[] tab = new byte[3]; tab[0] = (byte)board; tab[1] = (byte)UdpFrameFunction.DemandeCapteurCouleur; tab[2] = (byte)capteur; return(new Frame(tab)); }
/// <summary> /// Retourne le nom usuel d'un capteur couleur /// </summary> /// <param name="sensor">Capteur couleur à nommer</param> /// <returns>Nom du capteur couleur</returns> public static String GetName(SensorColorID sensor) { switch (sensor) { case SensorColorID.BuoyRight: return("bouée droite"); case SensorColorID.BuoyLeft: return("bouée gauche"); default: return(sensor.ToString()); } }
public override Color ReadSensorColor(SensorColorID sensor, bool wait = true) { ColorPlus c = Color.White; int i = DateTime.Now.Millisecond + DateTime.Now.Second * 1000 + DateTime.Now.Minute * 60 * 1000 + DateTime.Now.Hour * 60 * 60 * 1000; int max = 999 + 59 * 1000 + 59 * 60 * 1000 + 23 * 60 * 60 * 1000; i %= (15 * 1000); max %= (15 * 1000); int steps = 10; int maxColor = 255 * steps; i = (int)Math.Floor(i / (max / (float)maxColor)); switch (i / 255) { case 0: c = ColorPlus.FromRgb(255, i % 255, 0); break; case 1: c = ColorPlus.FromRgb(255 - i % 255, 255, 0); break; case 2: c = ColorPlus.FromRgb(0, 255, i % 255); break; case 3: c = ColorPlus.FromRgb(0, 255 - i % 255, 255); break; case 4: c = ColorPlus.FromRgb(i % 255, 0, 255); break; case 5: c = ColorPlus.FromRgb(255, 0, 255 - i % 255); break; case 6: c = ColorPlus.FromRgb(255 - i % 255, 0, 0); break; case 7: c = ColorPlus.FromRgb(i % 255, i % 255, i % 255); break; case 8: c = ColorPlus.FromRgb(255 - i % 255 / 2, 255 - i % 255 / 2, 255 - i % 255 / 2); break; case 9: c = ColorPlus.FromHsl(0, i % 255 / 255f, 0.5); break; } OnSensorColorChanged(sensor, c); return(SensorsColorValue[sensor]); }
void GrosRobot_SensorColorChanged(SensorColorID sensor, Color color) { this.InvokeAuto(() => { switch (sensor) { case SensorColorID.BuoyRight: picColorRight.SetColor(color); break; case SensorColorID.BuoyLeft: picColorLeft.SetColor(color); break; } }); }
public override Color ReadSensorColor(SensorColorID sensor, bool wait = true) { if (wait) { _lockSensorColor[sensor] = new Semaphore(0, int.MaxValue); } Frame t = UdpFrameFactory.DemandeCapteurCouleur(_boardSensorColor[sensor], sensor); Connections.UDPBoardConnection[_boardSensorColor[sensor]].SendMessage(t); if (wait) { _lockSensorColor[sensor].WaitOne(100); } return(SensorsColorValue[sensor]); }
public abstract Color ReadSensorColor(SensorColorID sensor, bool waitEnd = true);
protected void OnSensorColorChanged(SensorColorID sensor, Color color) { SensorsColorValue[sensor] = color; SensorColorChanged?.Invoke(sensor, color); }
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; } }