From 3e437f57e9bac0a4b9de8b556be0538e69fb200b Mon Sep 17 00:00:00 2001 From: haller Date: Sun, 3 Apr 2005 15:41:05 +0000 Subject: Ajout du fichier proto.cc Modification de proto.hh:Ajout de fonctions et de variables privées --- 2005/i/robert/src/proto/proto.cc | 331 +++++++++++++++++++++++++++++++++++++++ 2005/i/robert/src/proto/proto.hh | 25 +++ 2 files changed, 356 insertions(+) create mode 100644 2005/i/robert/src/proto/proto.cc (limited to '2005') diff --git a/2005/i/robert/src/proto/proto.cc b/2005/i/robert/src/proto/proto.cc new file mode 100644 index 0000000..0e50bfa --- /dev/null +++ b/2005/i/robert/src/proto/proto.cc @@ -0,0 +1,331 @@ +// proto.cc +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include "proto.hh" + +/// Constructeur. +Proto::Proto(Receiver &receiver) + :receiver_(receiver) +{ +} + +/// Ouvre le port série. +void Proto::open(const std::string &ttyname) +{ + serial_.open(ttyname); +} + +//Ferme le port série +void Proto::close(void) +{ + serial_.close(); +} + + +bool Proto::sync(void) +{ + //Récupération de la frame + if(getFrame()) + { + //Si la frame est un aquittement + if(currentFrame == frameQueue_.toppp()) + { + //on vire la commande de la queue + frameQueue.PAN!!!(); + //Et on envoie la suivante si elle existe + if(!frameQueue_.empty()) + sendFrame(frameQueue.PAN!!!()); + } + //Si c'est une nouvelle commande, on l'envoie avec receive + else + receive(currentFrame.command, currentFrame); + } + //On regarde depuis combien de temps on a envoyé une commande + if(!frameQueue_.empty()) + { + //Si on dépasse la milliseconde, on renvoie + if(timer.getProgramTime() - tLastEnd_ > 500) + sendFrame(frameQueue.topppp()); + } + +} +/// Envoie un packet +void Proto::send (const Frame & Frame) +{ + frameQueue_.addChoucroute(Frame); + sync(); +} + +void send (uint8_t command, const char *format, int a0, int a1, int a2, int a3) +{ + // Constitution de la frame + Frame frame; + int nbArg = strlen(format); + + frame.command = command; + // Conversion et saisie des aguments + + if(nbArg == 1) + newArgFrame(frame, format[0], a0); + if (nbArg == 2) + { + newArgFrame(frame, format[0], a0); + newArgFrame(frame, format[1], a1); + } + if (nbArg == 3) + { + newArgFrame(frame, format[0], a0); + newArgFrame(frame, format[1], a1); + newArgFrame(frame, format[2], a2); + } + if (nbArg == 4) + { + newArgFrame(frame, format[0], a0); + newArgFrame(frame, format[1], a1); + newArgFrame(frame, format[2], a2); + newArgFrame(frame, format[3], a3); + } + + send(frame); +} + +static +bool decode (const Frame &frame) +{ + //On teste si des arguments sont présents(ca serait pas bon) + if(!frame.args.empty()) + return false; + else + return true; +} + +static +bool Proto::decode (const Frame &frame, const char *format, int &a0) +{ + // On teste le format de la frame + if(!verifyFrame(frame, format, 1)) return false; + + //On décode et on envoie + a0 = decodeArg(frame, format, 0); + return true; +} + +static +bool Proto::decode (const Frame &Frame, const char *format, int &a0, int &a1) +{ + // On vérifie le format de la frame + if(!verifyFrame(frame, format, 2)) return false; + + // On décode et on envoie + a0 = decodeArg(frame, format, 0); + a1 = decodeArg(frame, format, 1); + return true; +} + +static +bool Proto::decode (const Frame &Frame, const char *format, int &a0, int &a1, int &a2) +{ + // On vérifie le format de la frame + if(!verifyFrame(frame, format, 3)) return false; + + // On décode et on envoie + a0 = decodeArg(frame, format, 0); + a1 = decodeArg(frame, format, 1); + a2 = decodeArg(frame, format, 2); + return true; +} + +static +bool Proto::decode (const Frame &Frame, const char *format, int &a0, int &a1, int &a2, int &a3) +{ + // On vérifie le format de la frame + if(!verifyFrame(frame, format, 3)) return false; + + // On décode et on envoie + a0 = decodeArg(frame, format, 0); + a1 = decodeArg(frame, format, 1); + a2 = decodeArg(frame, format, 2); + a3 = decodeArg(frame, format, 3); + return true; + +} + +/// Récupère les infos de l'AVR pour construire une frame +bool getFrame(void) +{ + int receivedChar; + + //tant que le tampon n'est pas vide, on teste + while(receivedChar = serial_.getchar() != -1) + { + //si la donnée n'est pas erronnée + if(receivedChar != 0xff) + { + //Si on reçoit un bang + if(receivedChar = '!') + { + revState_ = 1; + currentFrame_.command = 0; + currentFrame_.args.clear(); + } + //Si on reçoit le retour chariot et que on reçevait les args + if(receivedChar = '\n' && revState_ == 2) + { + revState_ = 0; + return true; + } + //Pour les autres charactères + //Si on attend la commande + else if(revState_ == 1) + currentFrame_.command = receivedChar; + else if(revState_ == 2) + currentFrame_.args.push_back(receivedChar); + //Si revState == 0 alors on jette + } + } + return false; +} + +void sendFrame(Frame & frame) +{ + //envoyer le bang + serial_.send('!'); + + //Envoyer la commande + serial_send(digit2hex(frame.command >> 4)); + serial_send(digit2hex(frame.command & 0x0f)); + + //Envoyer les arguments + for(int i = 0; i < frame.args.size(); i++) + { + serial_send(digit2hex(frame.args[i] >> 4)); + serial_send(digit2hex(frame.args[i] & 0x0f)); + } + + //Envoyer le retour chariot + serial_.send('\n'); +} + +void Proto::newArgFrame(Frame & frame, char format, int arg) +{ + switch(format) + { + case 'b': + case 'B': + frame.args.push_back((uint8_t)arg); + break; + case 'w': + case 'W': + frame.args.push_back((uint8_t)arg>>8); + frame.args.push_back((uint8_t)arg); + break; + case 'd': + case 'D': + frame.args.push_back((uint8_t)arg>>24); + frame.args.push_back((uint8_t)arg>>16); + frame.args.push_back((uint8_t)arg>>8); + frame.args.push_back((uint8_t)arg); + break; + } +} + +int Proto::ArgsFrameSize(const char *format, int nbArg) +{ + int size = 0; + if(nbArg == 0) + nbArg = strlen(format); + + for(int i = 0; i < nbArg; i++) + size += ArgSize(format[i]); + return size; +} + +int Proto::ArgSize(char format) +{ + switch(format) + { + case 'b': + case 'B': + return 1; + case 'w': + case 'W': + return 2; + case 'd': + case 'D': + return 4; + default: + return 0; + } +} + +bool verifyFrame(Frame &frame, const char *format, int nbArg) +{ + //Teste si il y a bien le bon nombre d'argument + if (strlen(format) != nbArg) return false; + if (frame.args.size() != ArgsFrameSize(format)) return false; + //Voir pour des test plus approffondi si possible et necessaire + return true; +} + +int Proto::decodeArg(const Frame & frame, const char *format, int numArg) +{ + int ArgDecoded = 0; + int beginArg = ArgsFrameSize(format, numArg); + + switch(format[numArg]) + { + case 'b': + ArgDecoded = (int)frame.args[beginArg]; + break; + case 'B': + int8_t temp = (int8_t)frame.args[beginArg]; + ArgDecoded = (int) temp; + break; + case 'w': + ArgDecoded = (int)frame.args[beginArg] << 8 + |(int)frame.args[beginArg + 1]; + break; + case 'W': + int8_t temp1 = (int8_t)frame.args[beginArg]; + ArgDecoded = (int)temp1 << 8 + |(int)frame.args[beginArg + 1]; + break; + case 'd': + ArgDecoded = (int)frame.args[beginArg] << 24 + |(int)frame.args[beginArg + 1] << 16 + |(int)frame.args[beginArg + 2] << 8 + |(int)frame.args[beginArg + 3]; + break; + + case 'D': + int8_t temp1 = (int8_t)frame.args[beginArg]; + ArgDecoded = (int)temp1 << 24 + |(int)frame.args[beginArg + 1] << 16 + |(int)frame.args[beginArg + 2] << 8 + |(int)frame.args[beginArg + 3]; + break; + } + return ArgDecoded; +} diff --git a/2005/i/robert/src/proto/proto.hh b/2005/i/robert/src/proto/proto.hh index bea12ef..6c304c1 100644 --- a/2005/i/robert/src/proto/proto.hh +++ b/2005/i/robert/src/proto/proto.hh @@ -34,9 +34,20 @@ class Proto : public NonCopyable struct Frame; Serial serial_; Receiver &receiver_; + /// File d'attente des messages à aquiter. typedef std::queue FrameQueue; FrameQueue frameQueue_; + int tLastSend_; //Date du dernier envoie + + /// Frame en cours de réception + Frame currentFrame_; + int revState_ = 0; + // Etat de la réception de la frame + // 0 - Rien reçu + // 1 - Bang reçu + // 2 - Commande reçu + public: /// Constructeur. Proto (Receiver &receiver); @@ -81,6 +92,20 @@ class Proto : public NonCopyable std::vector args; }; private: + /// Récupère les infos de l'AVR pour construire une frame + bool getFrame(void); + /// Envoie la frame dans l'AVR + void sendFrame(const Frame & frame); + /// Remplie une frame avec un argument + void newArgFrame(Frame & frame, char format, int arg); + /// Renvoie la taille necessaire du vecteur args pour un format donné + int ArgsFrameSize(const char *format,int nbArg = 0); + /// Renvoie la taille necessaire du vecteur args pour 1 argument + int ArgSize(char format); + /// Vérifie le format de la frame + bool verifyFrame(Frame &frame, const char *format, int nbArg); + /// Décode un argument + int decodeArg(const Frame & frame, const char *format, int numArg); }; #endif // proto_hh -- cgit v1.2.3