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