public void PushSendMessageToArduino(MessageProtocol mess, ArduinoBotComm bot)
{
if (bot != null)
{
bot.PushMessageAEnvoyer(mess);
}
}
public void PushMessageAEnvoyer(MessageProtocol mess)
{
foreach (MessageProtocol m in _MessageEnAttenteEnvoi)
{
if (m.headerMess == mess.headerMess)
{
if (mess.headerMess == (byte)PCtoEMBmessHeads.ASK_SENSOR)
{
if (((PCtoEMBMessageAskSensor)mess).idSensor == ((PCtoEMBMessageAskSensor)m).idSensor)
{
Logger.GlobalLogger.error("Tentative d'envoi d'un message au robot qui existe déja ");
return;
}
}
else
{
Logger.GlobalLogger.error("Tentative d'envoi d'un message au robot qui existe déja ");
return;
}
}
}
_MessageEnAttenteEnvoi.Add(mess);
Logger.GlobalLogger.info("Envoi du message " + mess.ToString() + " au robot " + this.id);
}
/* On remet le message dans la liste */
public void ResendMessageAttenteAck(MessageProtocol mess)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.stateMessage = 0;
_MessageEnAttenteEnvoi[index] = tmp;
}
// Update Date Envoi
public void UpdateDateEnvoiMessageAttenteAck(MessageProtocol mess)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.time = DateTime.Now;
_MessageEnAttenteEnvoi[index] = tmp;
}
// Ajout d'un rejeu pour le message
public int AddRejeuxMessageAttenteAck(MessageProtocol mess)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.countRejeu++;
_MessageEnAttenteEnvoi[index] = tmp;
return(_MessageEnAttenteEnvoi[index].countRejeu);
}
// Suppression d'un message qui a recu son ACK
public void SupprimerMessage(MessageProtocol mess)
{
//int index = _MessageEnAttenteEnvoi.IndexOf(mess);
_MessageEnAttenteEnvoi.Remove(mess);
/*MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
* tmp.state = 1;
* tmp.time = DateTime.Now;
* tmp.countRejeu = 0;
* _MessageEnAttenteEnvoi[index] = tmp;*/
}
// Passe le message en attente de ACK
public void MessageAttenteAck(MessageProtocol mess, ushort num)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.stateMessage = 1;
tmp.time = DateTime.Now;
tmp.countRejeu = 0;
tmp.numMessEnvoye = num;
_MessageEnAttenteEnvoi[index] = tmp;
}
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é :(");
}
}
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);
}
/* 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);
}
// Encore le message en Trame//
public TrameProtocole EncodeTrame(byte src, byte dst,ushort num ,MessageProtocol message)
{
return _TrameEncoder.EncodeTrame(message, src, dst, num);
}
public void PushMessageAEnvoyer(MessageProtocol mess)
{
foreach (MessageProtocol m in _MessageEnAttenteEnvoi)
{
if (m.headerMess == mess.headerMess)
{
if (mess.headerMess == (byte)PCtoEMBmessHeads.ASK_SENSOR)
{
if (((PCtoEMBMessageAskSensor)mess).idSensor == ((PCtoEMBMessageAskSensor)m).idSensor)
{
Logger.GlobalLogger.error("Tentative d'envoi d'un message au robot qui existe déja ");
return;
}
}
else
{
Logger.GlobalLogger.error("Tentative d'envoi d'un message au robot qui existe déja ");
return;
}
}
}
_MessageEnAttenteEnvoi.Add(mess);
Logger.GlobalLogger.info("Envoi du message " + mess.ToString()+ " au robot "+ this.id);
}
public MessageProtocol PopMessageAEnvoyer()
{
return(_MessageEnAttenteEnvoi.Find(MessageProtocol.MessageAEnvoyer()));
}
// Encore le message en Trame//
public TrameProtocole EncodeTrame(byte src, byte dst, ushort num, MessageProtocol message)
{
return(_TrameEncoder.EncodeTrame(message, src, dst, num));
}
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);
}
}
public bool IsMessageAttenteAck()
{
return(_MessageEnAttenteEnvoi.FindAll(MessageProtocol.MessageAttenteAck()).Count > 0);
}
/*public MessageProtocol PopMessageAttenteAck()
* {
* return _MessageEnAttenteEnvoi.Find(MessageProtocol.MessageAttenteAck());
* }*/
public List <MessageProtocol> ListMessageAttenteAck()
{
return(_MessageEnAttenteEnvoi.FindAll(MessageProtocol.MessageAttenteAck()));
}
/* 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é
}
}
// Update Date Envoi
public void UpdateDateEnvoiMessageAttenteAck(MessageProtocol mess)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.time = DateTime.Now;
_MessageEnAttenteEnvoi[index] = tmp;
}
// Suppression d'un message qui a recu son ACK
public void SupprimerMessage(MessageProtocol mess)
{
//int index = _MessageEnAttenteEnvoi.IndexOf(mess);
_MessageEnAttenteEnvoi.Remove(mess);
/*MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.state = 1;
tmp.time = DateTime.Now;
tmp.countRejeu = 0;
_MessageEnAttenteEnvoi[index] = tmp;*/
}
/* On remet le message dans la liste */
public void ResendMessageAttenteAck(MessageProtocol mess)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.stateMessage = 0;
_MessageEnAttenteEnvoi[index] = tmp;
}
// Ajout d'un rejeu pour le message
public int AddRejeuxMessageAttenteAck(MessageProtocol mess)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.countRejeu++;
_MessageEnAttenteEnvoi[index] = tmp;
return _MessageEnAttenteEnvoi[index].countRejeu;
}
// Passe le message en attente de ACK
public void MessageAttenteAck(MessageProtocol mess,ushort num)
{
int index = _MessageEnAttenteEnvoi.IndexOf(mess);
MessageProtocol tmp = _MessageEnAttenteEnvoi[index];
tmp.stateMessage = 1;
tmp.time = DateTime.Now;
tmp.countRejeu = 0;
tmp.numMessEnvoye = num;
_MessageEnAttenteEnvoi[index] = tmp;
}
