/* Recuperre les données sous forme d'octets de la trame */
public byte[] MakeTrameBinaryWithEscape(TrameProtocole trame)
{
List <byte> trameBinary = new List <byte>();
trameBinary.Add((byte)ProtocoleCharEnum.PROTOCOL_START_1);
trameBinary.Add((byte)ProtocoleCharEnum.PROTOCOL_START_2);
addToTrameBinary(trameBinary, trame.src);
addToTrameBinary(trameBinary, trame.dst);
addToTrameBinary(trameBinary, (byte)(trame.num >> 8));
addToTrameBinary(trameBinary, (byte)(trame.num & 0xFF));
addToTrameBinary(trameBinary, trame.length);
foreach (byte data in trame.data)
{
addToTrameBinary(trameBinary, data);
}
addToTrameBinary(trameBinary, (byte)(trame.crc >> 8));
addToTrameBinary(trameBinary, (byte)(trame.crc & 0xFF));
trameBinary.Add((byte)ProtocoleCharEnum.PROTOCOL_STOP);
return(trameBinary.ToArray());
}
// Nouvelle trame
private void _XbeeAPI_OnNewTrameReceived(object sender, NewTrameReceivedEventArgs args)
{
Logger.GlobalLogger.debug("Données reçues ");
byte[] datas = args.trameBytes;
int i = 0;
// Parse les données
while (i < datas.Length && !_TrameDecoder.parseIncomingData(datas[i]))
{
i++;
}
Logger.GlobalLogger.debug("Parsing terminé etat : " + _TrameDecoder.bIsCompleted);
// Condition si la trame est finie ou pas
if (!_TrameDecoder.bIsCompleted) // Trame non complette on attends
{
return;
}
TrameProtocole TrameFinale = _TrameDecoder.getDecodedTrame();
/* Ajout dans la liste des trames recus */
//_ListTramesRecues.Add(TrameFinale);
PushTrameRecus(TrameFinale);
Logger.GlobalLogger.debug("Decodage et ajout en liste");
}
/* Recuperes les bytes pour le calcul du CRC */
private static byte[] getBytesCrc(TrameProtocole trame)
{
try
{
byte[] retVal = new byte[trame.length + 5];
int index = 0;
retVal[index++] = trame.src;
retVal[index++] = trame.dst;
retVal[index++] = (byte)(trame.num >> 8);
retVal[index++] = (byte)(trame.num & 0xFF);
retVal[index++] = trame.length;
foreach (byte data in trame.data)
{
retVal[index++] = data;
}
return(retVal);
}
catch (Exception e)
{
Logger.GlobalLogger.error(e.ToString());
return(new byte[] { 0 });
}
}
/* Recuperre les Bytes */
public static byte[] getBytes(TrameProtocole trame)
{
try
{
byte[] retVal = new byte[trame.length + 10];
int index = 0;
retVal[index++] = (byte)ProtocoleCharEnum.PROTOCOL_START_1;
retVal[index++] = (byte)ProtocoleCharEnum.PROTOCOL_START_2;
retVal[index++] = trame.src;
retVal[index++] = trame.dst;
retVal[index++] = (byte)(trame.num >> 8);
retVal[index++] = (byte)(trame.num & 0xFF);
retVal[index++] = trame.length;
foreach (byte data in trame.data)
{
retVal[index++] = data;
}
retVal[index++] = (byte)(trame.crc >> 8);
retVal[index++] = (byte)(trame.crc & 0xFF);
retVal[index++] = (byte)ProtocoleCharEnum.PROTOCOL_STOP;
return(retVal);
}
catch (Exception e)
{
Logger.GlobalLogger.error(e.ToString());
return(new byte[] { 0 });
}
}
// Recuperrer une trame depuis la liste et la supprime //
public TrameProtocole PopTrameAEnvoyer()
{
TrameProtocole t = default(TrameProtocole);
if (_ListTramesToSend.Count > 0)
{
t = _ListTramesToSend[0];
_ListTramesToSend.Remove(t);
}
return(t);
}
// Recuperrer une trame depuis la liste et la supprime //
public TrameProtocole PopTrameRecus()
{
TrameProtocole t = default(TrameProtocole);
if (_ListTramesRecues.Count > 0)
{
t = _ListTramesRecues[0];
_ListTramesRecues.Remove(t);
}
return(t);
}
private void SendMessageToArduino(MessageProtocol mess, ArduinoBotComm bot)
{
if (bot == null)
{
Logger.GlobalLogger.error("Robot inconu !");
return;
}
if (bot.Connected)
{
if (bot.stateComm == StateArduinoComm.STATE_COMM_NONE)
{
if (mess is PCtoEMBMessageAskSensor)
{
bot.stateComm = StateArduinoComm.STATE_COMM_WAIT_SENSOR;
}
else if (mess is PCtoEMBMessageCloseClaw || mess is PCtoEMBMessageOpenClaw)
{
bot.stateComm = StateArduinoComm.STATE_COMM_WAIT_ACK;
}
else if (mess is PCtoEMBMessageTurn || mess is PCtoEMBMessageMove)
{
bot.stateComm = StateArduinoComm.STATE_COMM_WAIT_ACK;
}
else if (mess is PCtoEMBMessagePing)
{
bot.stateComm = StateArduinoComm.STATE_COMM_WAIT_PING;
}
else if (mess is PCtoEMBMessageRespConn)
{
bot.stateComm = StateArduinoComm.STATE_COMM_WAIT_ACK;
}
else if (mess is PCtoEMBMessageAutoMode)
{
bot.stateComm = StateArduinoComm.STATE_COMM_WAIT_ACK;
}
else
{
Logger.GlobalLogger.error("Envoi d'un message non connu !");
}
bot.MessageAttenteAck(mess, bot.CountSend);
TrameProtocole trame = _SerialXbee.EncodeTrame(_IdPc, bot.id, bot.CountSend++, mess);
//trame.num = bot.CountSend++;
_SerialXbee.PushTrameAEnvoyer(trame);
}
}
else
{
Logger.GlobalLogger.error("Envoi d'un message à un robot non connecté :(");
}
}
public TrameProtocole getDecodedTrame()
{
if (_bTrameCompleted)
{
TrameProtocole Trame = _trameReceived;
_protocolState = 0;
_bTrameCompleted = false;
_trameReceived = new TrameProtocole();
return(Trame);
}
else
{
throw new NotSupportedException("Trame pas complete !");
}
}
private void _ThreadEnvoiTrames()
{
while (true)
{
if (TrameAEnvoyerDisponible())
{
/* Envoi */
//List<byte> Envoyes = new List<byte>();
TrameProtocole trame = PopTrameAEnvoyer();
Logger.GlobalLogger.debug("Envoi d'une trame (" + trame.ToString() + ") ", 1);
_XbeeAPI.sendApiFrame(trame.dst, _TrameEncoder.MakeTrameBinaryWithEscape(trame));
Logger.GlobalLogger.debug("OK");
}
Thread.Sleep(_ThreadDelay);
}
}
private void _ThreadCheckMessageRecus()
{
while (true)
{
/* Traitement des trames entrantes */
if (_SerialXbee.TrameRecusDisponible())
{
TrameProtocole trame = _SerialXbee.PopTrameRecus();
// Si traitement par l'application (données capteur ou autre )
// Les Ack et autre sont gerrer ici
if (TraiteTrameRecue(trame))
{
// Envoi au couches supérrieures
if (OnNewTrameArduinoReceived != null)
{
MessageProtocol message = _SerialXbee.DecodeTrame(trame);
ArduinoBotComm robot = ArduinoManager.getArduinoBotById(trame.src);
NewTrameArduinoReceveidEventArgs arg = new NewTrameArduinoReceveidEventArgs(message, robot);
OnNewTrameArduinoReceived(this, arg);
}
}
}
/* Verification message en attente envoi */
/*if (_MessagesEnAttenteEnvoi.Count > 0)
* {
*
* int count = _MessagesEnAttenteEnvoi.Count;
* for(int i=0 ; i< count;i++)
* {
* if (_MessagesEnAttenteEnvoi[i].robot.stateComm == StateArduinoComm.STATE_COMM_NONE)
* {
* Logger.GlobalLogger.debug("Envoi d'un message en attente au robot :" + _MessagesEnAttenteEnvoi[i].robot + " Messsage : " + _MessagesEnAttenteEnvoi[i].message.GetType().ToString(),1);
* // L'arduino est libre, on peut envoyer
* SendMessageToArduino(_MessagesEnAttenteEnvoi[i].message, _MessagesEnAttenteEnvoi[i].robot);
* }
* }
*
* }*/
Thread.Sleep(_ThreadMessageRecuDelay);
}
}
/* Encode un message En trame pour envoyer au Arduino */
public TrameProtocole EncodeTrame(MessageProtocol message, byte src, byte dst, ushort num)
{
TrameProtocole trame = new TrameProtocole();
switch (message.headerMess)
{
case (byte)PCtoEMBmessHeads.ASK_PING:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessagePing)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.ASK_SENSOR:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageAskSensor)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.CLOSE_CLAW:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageCloseClaw)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.OPEN_CLAW:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageOpenClaw)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.MOVE:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageMove)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.TURN:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageTurn)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.RESP_CONN:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageRespConn)message).getBytes());
break;
case (byte)PCtoEMBmessHeads.AUTO_MODE:
trame = MakeTrame(src, dst, num, ((PCtoEMBMessageAutoMode)message).getBytes());
break;
default:
Logger.GlobalLogger.error("Erreur à envoyer inconnu !");
break;
}
return(trame);
}
/* Construit la trame à partir des valeurs données en params */
public TrameProtocole MakeTrame(byte src, byte dst, ushort num, byte[] data)
{
if (data.Length > TrameProtocole.BUFFER_DATA_IN)
{
throw new Exception("Taille Maximum dépassée");
}
TrameProtocole trame = new TrameProtocole();
trame.src = src;
trame.dst = dst;
trame.num = num;
trame.length = Convert.ToByte(data.Length);
trame.data = data;
trame.crc = MessagesUtils.crc16_protocole(trame);
//trame.state = 0; // Initialise la trame en non envoyée
return(trame);
}
/* decode une trame en Message */
public MessageProtocol DecodeTrame(TrameProtocole trame)
{
MessageProtocol message = new MessageProtocol();
switch (trame.data[0]) // Header
{
case (byte)EMBtoPCmessHeads.ACK:
message = new EMBtoPCMessageGlobalAck();
message.headerMess = trame.data[0];
((EMBtoPCMessageGlobalAck)message).idTrame = (ushort)(trame.data[1] << 8);
((EMBtoPCMessageGlobalAck)message).idTrame += trame.data[2];
((EMBtoPCMessageGlobalAck)message).valueAck = trame.data[3];
break;
case (byte)EMBtoPCmessHeads.ASK_CONN:
message = new EMBtoPCMessageAskConn();
message.headerMess = trame.data[0];
break;
case (byte)EMBtoPCmessHeads.RESP_PING:
message = new EMBtoPCMessageRespPing();
message.headerMess = trame.data[0];
break;
case (byte)EMBtoPCmessHeads.RESP_SENSOR:
message = new EMBtoPCMessageRespSensor();
message.headerMess = trame.data[0];
((EMBtoPCMessageRespSensor)message).idSensor = trame.data[1];
((EMBtoPCMessageRespSensor)message).valueSensor = trame.data[2];
break;
case (byte)EMBtoPCmessHeads.AUTO_MODE_OFF:
message = new EMBtoPCMessageAutoModeOff();
message.headerMess = trame.data[0];
break;
default:
Logger.GlobalLogger.error("Erreur Message recu inconnu !");
break;
}
return(message);
}
/* Retourne la valeur du CRC-16 de la trame */
public static ushort crc16_protocole(TrameProtocole trame)
{
byte[] d = getBytesCrc(trame);
return crc16.calc_crc16(d, d.Length);
}
// Decode la trame en Message //
public MessageProtocol DecodeTrame(TrameProtocole trame)
{
return _TrameDecoder.DecodeTrame(trame);
}
/* Recuperes les bytes pour le calcul du CRC */
private static byte[] getBytesCrc(TrameProtocole trame)
{
try
{
byte[] retVal = new byte[trame.length + 5];
int index = 0;
retVal[index++] = trame.src;
retVal[index++] = trame.dst;
retVal[index++] = (byte)(trame.num >> 8);
retVal[index++] = (byte)(trame.num & 0xFF);
retVal[index++] = trame.length;
foreach (byte data in trame.data)
retVal[index++] = data;
return retVal;
}
catch (Exception e)
{
Logger.GlobalLogger.error(e.ToString());
return new byte[] { 0 };
}
}
// Ajouter une trame à traiter //
public void PushTrameAEnvoyer(TrameProtocole trame)
{
_ListTramesToSend.Add(trame);
}
// Ajouter une trame à traiter //
public void PushTrameRecus(TrameProtocole trame)
{
Logger.GlobalLogger.debug("Reception d'une trame (" + trame.ToString() + ") ", 1);
_ListTramesRecues.Add(trame);
}
// Decode la trame en Message //
public MessageProtocol DecodeTrame(TrameProtocole trame)
{
return(_TrameDecoder.DecodeTrame(trame));
}
/* Retourne la valeur du CRC-16 de la trame */
public static ushort crc16_protocole(TrameProtocole trame)
{
byte[] d = getBytesCrc(trame);
return(crc16.calc_crc16(d, d.Length));
}
/* Traite la trame et retourne si la trame doit remonter a la couche supérieure (true) */
private bool TraiteTrameRecue(TrameProtocole trame)
{
MessageProtocol message = _SerialXbee.DecodeTrame(trame);
ArduinoBotComm robot = ArduinoManager.getArduinoBotById(trame.src);
if (message is EMBtoPCMessageAskConn)
{
if (robot == null) // Le robot n'as jamais été céer, on le créer
{
robot = new ArduinoBotComm(trame.src);
robot.DateLastMessageReceived = DateTime.Now;
_ArduinoManager.addArduinoBot(robot);
}
Logger.GlobalLogger.debug("Reception du message EMBtoPCMessageAskConn par " + robot.id);
if (!robot.Connected) // Robot non connecté, on l'ajoute dans la liste des connectés
{
robot.Connect();
robot.DateLastMessageReceived = DateTime.Now;
robot.stateComm = StateArduinoComm.STATE_COMM_NONE;
robot.CountSend = trame.num; // Utilisation du compteur du robot
MessageProtocol reponse = MessageBuilder.createRespConnMessage(0x01);
// Ajout a la liste à envoyer
//TrameProtocole trameRet = _SerialXbee.EncodeTrame(_IdPc, trame.src, robot.CountSend++, reponse);
//trameRet.num = robot.CountSend++;
robot.PushMessageAEnvoyer(reponse);
//_SerialXbee.PushTrameToSend(trameRet);
return(true); // Notification a l'application
}
else
{
Logger.GlobalLogger.error("Reception d'une demande de connexion d'un bot déjà connecté");
return(false);
}
}
else if (message is EMBtoPCMessageGlobalAck)
{
Logger.GlobalLogger.debug("Reception du message EMBtoPCMessageGlobalAck par " + robot.id);
if (robot == null)
{
Logger.GlobalLogger.error("Robot Inconnu in EMBtoPCMessageGlobalAck");
return(false); // Marquer comme traité
}
if (robot.Connected) // Robot connecté
{
robot.DateLastMessageReceived = DateTime.Now;
EMBtoPCMessageGlobalAck msg = ((EMBtoPCMessageGlobalAck)message);
if (msg.valueAck == 0x00) // !akitement negatif Message erreur
{
Logger.GlobalLogger.error("Ackitement Negatif !");
}
else if (msg.valueAck == 0x01)
{
if (robot.stateComm == StateArduinoComm.STATE_COMM_WAIT_ACK) // On attendais un ACK
{
robot.stateComm = StateArduinoComm.STATE_COMM_NONE;
}
robot.AckRecu(msg.idTrame);
//_SerialXbee.DeleteTrame(msg.idTrame);
}
else if (msg.valueAck == 0x02)
{
Logger.GlobalLogger.error("CRC corrompu, on r'envois !");
}
else
{
Logger.GlobalLogger.error("Ackitement inconnu !");
}
return(false);
}
else
{
Logger.GlobalLogger.error("Reception d'un ACK alors que le robot n'est pas connecté !");
return(false);
}
}
else if (message is EMBtoPCMessageRespPing)
{
Logger.GlobalLogger.debug("Reception du message EMBtoPCMessageRespPing par " + robot.id);
if (robot == null)
{
Logger.GlobalLogger.error("Robot Inconnu in EMBtoPCMessageRespPing");
return(false); // Marquer comme traité
}
if (robot.Connected) // Robot connecté
{
robot.DateLastMessageReceived = DateTime.Now;
if (robot.stateComm == StateArduinoComm.STATE_COMM_WAIT_PING) // On attendais un ACK
{
robot.stateComm = StateArduinoComm.STATE_COMM_NONE;
}
/* Supprimer les demande de ping sans ackitements en provenance du robot */
foreach (MessageProtocol mp in robot.ListMessageAttenteAck())
{
if (mp.headerMess == (byte)PCtoEMBmessHeads.ASK_PING)
{
robot.SupprimerMessage(mp);
}
}
//List<TrameProtocole> Listtp = _SerialXbee.FetchTrameSentNoAck();
/*foreach (TrameProtocole tp in Listtp)
* {
* if (tp.data[0] == (byte)PCtoEMBmessHeads.ASK_PING)
* if(tp.dst == robot.id)
* _SerialXbee.DeleteTrame(tp.num);
* }*/
return(false);
}
else
{
Logger.GlobalLogger.error("Reception d'un ACK alors que le robot n'est pas connecté !");
return(false);
}
}
else if (message is EMBtoPCMessageAutoModeOff)
{
if (robot == null)
{
Logger.GlobalLogger.error("Robot Inconnu in EMBtoPCMessageAutoModeOff");
return(false); // Marquer comme traité
}
Logger.GlobalLogger.debug("Reception du message EMBtoPCMessageAutoModeOff par " + robot.id);
if (robot.Connected) // Robot connecté
{
robot.DateLastMessageReceived = DateTime.Now;
/*if (robot.stateComm == StateArduinoComm.STATE_COMM_WAIT_PING) // On attendais un ACK
* robot.stateComm = StateArduinoComm.STATE_COMM_NONE;*/
return(true); // Envoie au couche suppérieure
}
else
{
Logger.GlobalLogger.error("Reception d'un message alors que le robot n'est pas connecté ! EMBtoPCMessageAutoModeOff");
return(false);
}
}
else if (message is EMBtoPCMessageRespSensor)
{
Logger.GlobalLogger.debug("Reception du message EMBtoPCMessageRespSensor par " + robot.id);
if (robot == null)
{
Logger.GlobalLogger.error("Robot Inconnu in EMBtoPCMessageRespSensor");
return(false); // Marquer comme traité
}
if (robot.Connected) // Robot connecté
{
robot.DateLastMessageReceived = DateTime.Now;
if (robot.stateComm == StateArduinoComm.STATE_COMM_WAIT_SENSOR) // On attendais un ACK
{
robot.stateComm = StateArduinoComm.STATE_COMM_NONE;
}
foreach (MessageProtocol mp in robot.ListMessageAttenteAck())
{
if (mp.headerMess == (byte)PCtoEMBmessHeads.ASK_SENSOR)
{
robot.SupprimerMessage(mp);
}
}
return(true); // Envoi a l'apllication pour traitement
}
else
{
Logger.GlobalLogger.error("Reception d'un message alors que le robot n'est pas connecté ! EMBtoPCMessageRespSensor");
return(false);
}
}
else
{
if (robot == null)
{
Logger.GlobalLogger.error("Robot Inconnu");
}
Logger.GlobalLogger.error("Message de type Inconnu !");
return(false); // Marquer comme traité
}
}
// Ajouter une trame à traiter //
public void PushTrameAEnvoyer(TrameProtocole trame)
{
_ListTramesToSend.Add(trame);
}
public void _ThreadCheckEnvoi()
{
while (true)
{
// Chacun des robots
List <ArduinoBotComm> listeArduino = _ArduinoManager.ListeArduino;
for (int i = 0; i < listeArduino.Count; i++)
{
// Si il est connecté
if (listeArduino[i].Connected)
{
/* Keep alive */
// Pas de messages depuis DateLastMessageReceived
if (DateTime.Now - listeArduino[i].DateLastMessageReceived > TimeSpan.FromSeconds(_TimeOutKeepAlive))
{
// On n'est pas en attente d'un ACK
if (listeArduino[i].stateComm == StateArduinoComm.STATE_COMM_NONE)
{
MessageProtocol reponse = MessageBuilder.createAskPingMessage();
Logger.GlobalLogger.debug("Envoi d'un Ping au robot " + listeArduino[i].id);
PushSendMessageToArduino(reponse, listeArduino[i]);
//SendMessageToArduino(reponse, listeArduino[i]);
}
}
/* Rejeux */
List <MessageProtocol> AttAck = listeArduino[i].ListMessageAttenteAck();
foreach (MessageProtocol mess in AttAck)
{
/* On doit en envoyer un rejeux */
if ((DateTime.Now - mess.time) > TimeSpan.FromSeconds(_DelayRejeu))
{
if (mess.countRejeu < _MaxRejeu)
{
//listeArduino[i].ResendMessageAttenteAck(mess);
listeArduino[i].AddRejeuxMessageAttenteAck(mess);
listeArduino[i].UpdateDateEnvoiMessageAttenteAck(mess);
Logger.GlobalLogger.debug("Declenchement Renvoi ", 1);
//listeArduino[i].MessageAttenteAck(mess, listeArduino[i].CountSend);
TrameProtocole trame = _SerialXbee.EncodeTrame(_IdPc, listeArduino[i].id, listeArduino[i].CountSend, mess);
_SerialXbee.PushTrameAEnvoyer(trame);
break;
}
else
{
_OnArduinoTimeout(listeArduino[i]);
Logger.GlobalLogger.info("Pas de réponses de l'arduino, suppression !");
break;
// Pas de réponse depuis longtemps ? on déconnect
}
}
}
if (listeArduino[i].IsMessageAEnvoyer())
{
if (listeArduino[i].Connected)
{
// On n'est pas en attente pour ne pas spamer
if (listeArduino[i].stateComm == StateArduinoComm.STATE_COMM_NONE)
{
MessageProtocol mess = listeArduino[i].PopMessageAEnvoyer();
//_SerialXbee.PushTrameAEnvoyer(_SerialXbee.EncodeTrame(_IdPc,listeArduino[i].id,listeArduino[i].CountSend,mess));
SendMessageToArduino(mess, listeArduino[i]);
Logger.GlobalLogger.debug("Envoi d'un message");
break;
}
}
}
}
}
Thread.Sleep(_ThreadKeepAliveDelay);
}
}
// Ajouter une trame à traiter //
public void PushTrameRecus(TrameProtocole trame)
{
Logger.GlobalLogger.debug("Reception d'une trame (" + trame.ToString() + ") ", 1);
_ListTramesRecues.Add(trame);
}
/* Recuperre les Bytes */
public static byte[] getBytes(TrameProtocole trame)
{
try
{
byte[] retVal = new byte[trame.length + 10];
int index = 0;
retVal[index++] = (byte)ProtocoleCharEnum.PROTOCOL_START_1;
retVal[index++] = (byte)ProtocoleCharEnum.PROTOCOL_START_2;
retVal[index++] = trame.src;
retVal[index++] = trame.dst;
retVal[index++] = (byte)(trame.num >> 8);
retVal[index++] = (byte)(trame.num & 0xFF);
retVal[index++] = trame.length;
foreach (byte data in trame.data)
retVal[index++] = data;
retVal[index++] = (byte)(trame.crc >> 8);
retVal[index++] = (byte)(trame.crc & 0xFF);
retVal[index++] = (byte)ProtocoleCharEnum.PROTOCOL_STOP;
return retVal;
}
catch (Exception e)
{
Logger.GlobalLogger.error(e.ToString());
return new byte[] { 0 };
}
}
