From 8f486613be58ced269db1d437e560c16558604e8 Mon Sep 17 00:00:00 2001 From: becquet Date: Thu, 10 May 2007 18:49:20 +0000 Subject: Création de chuck, le programme du robot 2007. --- i/chuck/src/proto/Makefile.defs | 5 + i/chuck/src/proto/proto.cc | 457 ++++++++++++++++++++++++++++++++++++++++ i/chuck/src/proto/proto.hh | 140 ++++++++++++ i/chuck/src/proto/test_proto.cc | 96 +++++++++ 4 files changed, 698 insertions(+) create mode 100644 i/chuck/src/proto/Makefile.defs create mode 100644 i/chuck/src/proto/proto.cc create mode 100644 i/chuck/src/proto/proto.hh create mode 100644 i/chuck/src/proto/test_proto.cc (limited to 'i/chuck/src/proto') diff --git a/i/chuck/src/proto/Makefile.defs b/i/chuck/src/proto/Makefile.defs new file mode 100644 index 0000000..b351026 --- /dev/null +++ b/i/chuck/src/proto/Makefile.defs @@ -0,0 +1,5 @@ +PROGRAMS += test_proto + +proto_OBJECTS = proto.o $(serial_OBJECTS) $(log_OBJECTS) $(utils_OBJECTS) $(timer_OBJECTS) + +test_proto_OBJECTS = test_proto.o $(proto_OBJECTS) $(tester_OBJECTS) diff --git a/i/chuck/src/proto/proto.cc b/i/chuck/src/proto/proto.cc new file mode 100644 index 0000000..50e363d --- /dev/null +++ b/i/chuck/src/proto/proto.cc @@ -0,0 +1,457 @@ +// 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 "timer/timer.hh" +#include "utils/hexa.hh" +#include "proto.hh" + +#include +#include +#include + +/// Constructeur. +Proto::Proto(Receiver &receiver) + : log_ ("proto"), receiver_(receiver), tLastSend_(0), revState_(0) +{ +} + +/// 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(); +} + +/// Teste si tout les packets ont été envoyés et aquités, sinon, essaye de +/// le faire. +bool +Proto::sync(void) +{ + // Récupération de la frame + while (getFrame()) + { + log_ ("recv", Log::debug) << "frame" << currentFrame_; + // Si la frame est un aquittement + if(currentFrame_ == frameQueue_.front()) + { + // on vire la commande de la queue + frameQueue_.pop(); + // Et on envoie la suivante si elle existe + if(!frameQueue_.empty()) + sendFrame(frameQueue_.front()); + } + // Si c'est une nouvelle commande, on l'envoie avec receive + else + receiver_.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() - tLastSend_ >= timeout_) + sendFrame(frameQueue_.front()); + } + return frameQueue_.empty(); +} + +/// Envoie un packet +void +Proto::send (const Frame & frame, bool reliable) +{ + if(reliable) + { + if(frameQueue_.empty()) + tLastSend_ = -timeout_; + frameQueue_.push(frame); + } + else + sendFrame(frame); +} + + +/// Envois un packet. COMMAND est la commande à envoyer, FORMAT, donne le +/// format et le nombre de paramètres ('b' : 8 bits, 'w' : 16 bits, 'd' : +/// 32 bits, majuscule pour signé). +void +Proto::send (char command, const char *format, int a0, int a1, + int a2, int a3, int a4, int a5, bool reliable) +{ + // Constitution de la frame + Proto::Frame frame; + frame.command = command; + + if (format[0] != '\0') + { + newArgFrame(frame, format[0],a0); + if (format[1] != '\0') + { + newArgFrame(frame, format[1],a1); + if (format[2] != '\0') + { + newArgFrame(frame, format[2],a2); + if (format[3] != '\0') + { + newArgFrame(frame, format[3],a3); + if(format[4] != '\0') + { + newArgFrame(frame, format[4],a4); + if(format[5] != '\0') + newArgFrame(frame, format[5],a5); + } + + } + } + } + } + send(frame,reliable); +} + +/// Send a string of size number. +void +Proto::sendStr (char command, const char *str, int size, bool reliable) +{ + Proto::Frame frame; + + frame.command = command; + + for (int compt = 0; compt < size; compt++) + newArgFrame (frame, 'b', str[compt]); + + send (frame,reliable); +} + +/// permet d'envoyer un packet pas fiable +void +Proto::sendUnreliable (char command, const char *format, int a0, int a1, + int a2, int a3, int a4, int a5) +{ + send(command, format, a0, a1, a2, a3, a4, a5, false); +} + +/// Teste si le packet correspond au format et décode les valeurs. Utilise +/// le même format que send. +bool +Proto::decode (const Proto::Frame &frame) +{ + int dummy; + return decode(frame, "", dummy, dummy, dummy, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0) +{ + int dummy; + return decode(frame, format, a0, dummy, dummy, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, int &a1) +{ + int dummy; + return decode(frame, format, a0, a1, dummy, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, + int &a1, int &a2) +{ + int dummy; + return decode(frame, format, a0, a1, a2, dummy, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, + int &a1, int &a2, int &a3) +{ + int dummy; + return decode(frame, format, a0, a1, a2, a3, dummy); +} + +bool +Proto::decode (const Frame &frame, const char *format, int &a0, + int &a1, int &a2, int &a3, int &a4) +{ + // Teste si il y a bien le bon nombre d'argument + if (static_cast(frame.args.size()) != argsFrameSize(format)) + return false; + // On décode et on envoie + decodeArg(frame, format, a0, a1, a2, a3, a4); + + return true; +} + +/// Attend que des caractères soit disponible pendant un temps maximum en +/// milliseconde et les traite. Renvois le résultat de sync (). Attention, +/// fonction bloquante, n'utiliser que pour les tests. +bool +Proto::wait (int timeout/*-1*/) +{ + // On apelle serial_.wait avec un timeout toujours inf ou égal à proto::timeout_ + serial_.wait (timeout < timeout_ && timeout != -1 ? timeout : timeout_); + return sync (); +} + +/// Récupère le File Descriptor +int Proto::getFd(void) const +{ + return serial_.getFd(); +} + +/// Récupère les infos de l'AVR pour construire une frame +bool +Proto::getFrame(void) +{ + int receivedChar, d; + bool erreur = false; + // tant que le tampon n'est pas vide, on teste + while((receivedChar = serial_.getchar()) != -1) + { + // 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 + else if(receivedChar == '\r' && revState_ == 2) + { + revState_ = 0; + return true; + } + // Pour les autres charactères + // Si on attend la commande + else + switch(revState_) + { + case 1: + if (!isalpha (receivedChar) || receivedChar == '?') + erreur = true; + else + { + currentFrame_.command = receivedChar; + revState_ = 2; + } + break; + case 2: + d = hex2digit (receivedChar); + if (d == hexInvalid) + erreur = true; + else + { + currentFrame_.args.push_back (static_cast(d) << 4); + revState_ = 3; + } + break; + case 3: + d = hex2digit (receivedChar); + if (d == hexInvalid) + erreur = true; + else + { + currentFrame_.args.back() |= static_cast(d); + revState_ = 2; + break; + } + } + // Si revState == 0 alors on jette + // Si on a reçu une erreur on renvoie + if(erreur) + { + // On renvoie en mettant le compteur à 0, la commande sera + // renvoyer de retour à sync + log_("Erreur de reception", Log::debug) + << "commande inconnue" << ""; + tLastSend_ = 0; + revState_ = 0; + return false; + } + } + return false; +} + +/// Envoie la frame dans l'AVR +void +Proto::sendFrame(const Frame & frame) +{ + log_ ("send", Log::debug) << "frame" << frame; + // envoyer le bang + serial_.putchar('!'); + + // Envoyer la commande + serial_.putchar(frame.command); + + // Envoyer les arguments + for(std::vector::const_iterator it = frame.args.begin(); + it != frame.args.end(); ++it) + { + serial_.putchar(digit2hex(*it >> 4)); + serial_.putchar(digit2hex(*it & 0x0f)); + } + + // Envoyer le retour chariot + serial_.putchar('\r'); + + // actualiser le timer + tLastSend_ = Timer::getProgramTime(); +} + +/// Remplie une frame avec un argument +void +Proto::newArgFrame(Proto::Frame & frame, char format, int arg) +{ + switch(format) + { + case 'b': + case 'B': + frame.args.push_back(static_cast(arg)); + break; + case 'w': + case 'W': + frame.args.push_back(static_cast(arg >> 8)); + frame.args.push_back(static_cast(arg)); + break; + case 'd': + case 'D': + frame.args.push_back(static_cast(arg >> 24)); + frame.args.push_back(static_cast(arg >> 16)); + frame.args.push_back(static_cast(arg >> 8)); + frame.args.push_back(static_cast(arg)); + break; + } +} + +/// Renvoie la taille necessaire du vecteur args pour un format donné +int +Proto::argsFrameSize(const char *format) +{ + int size = 0; + for(; *format != '\0'; format++) + switch(*format) + { + case 'b': + case 'B': + size += 1; + break; + case 'w': + case 'W': + size += 2; + break; + case 'd': + case 'D': + size += 4; + break; + default: + size += 0; + } + return size; +} + +/// Décode un argument +void +Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int &a2, int &a3, int &a4) +{ + int temp[5]; + int pos = 0; + + for(int i = 0; *format != '\0'; format++,i++) + { + switch(*format) + { + case 'b': + temp[i] = static_cast(frame.args[pos]); + pos++; + break; + case 'B': + { + int8_t t = static_cast(frame.args[pos]); + temp[i] = static_cast(t); + pos++; + break; + } + case 'w': + temp[i] = static_cast(frame.args[pos]) << 8 + |static_cast(frame.args[pos + 1]); + pos += 2; + break; + case 'W': + { + int8_t t = static_cast(frame.args[pos]); + temp[i] = static_cast(t) << 8 + |static_cast(frame.args[pos + 1]); + pos += 2; + break; + } + case 'd': + temp[i] = static_cast(frame.args[pos]) << 24 + |static_cast(frame.args[pos + 1]) << 16 + |static_cast(frame.args[pos + 2]) << 8 + |static_cast(frame.args[pos + 3]); + pos += 4; + break; + + case 'D': + { + int8_t t = static_cast(frame.args[pos]); + temp[i] = static_cast(t) << 24 + |static_cast(frame.args[pos + 1]) << 16 + |static_cast(frame.args[pos + 2]) << 8 + |static_cast(frame.args[pos + 3]); + pos += 4; + break; + } + } + } + a0 = temp[0]; + a1 = temp[1]; + a2 = temp[2]; + a3 = temp[3]; + a4 = temp[4]; +} + +bool +Proto::Frame::operator==(const Frame& frame) +{ + return this->command == frame.command && this->args == frame.args; +} + +/// Affiche une frame. +std::ostream & +operator<< (std::ostream &os, const Proto::Frame &f) +{ + os << '<' << f.command << ' '; + std::copy (f.args.begin (), f.args.end (), + std::ostream_iterator (os, " ")); + os << '>'; + return os; +} + diff --git a/i/chuck/src/proto/proto.hh b/i/chuck/src/proto/proto.hh new file mode 100644 index 0000000..a45db91 --- /dev/null +++ b/i/chuck/src/proto/proto.hh @@ -0,0 +1,140 @@ +#ifndef proto_hh +#define proto_hh +// proto.hh +// robert - programme du robot 2005. {{{ +// +// Copyright (C) 2005 Nicolas Schodet +// +// 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 "utils/non_copyable.hh" +#include "serial/serial.hh" +#include "log/log.hh" + +#include +#include + +/// Classe de dialogue avec une carte électronique par le port série. +class Proto : public NonCopyable +{ + public: + class Receiver; + /// Packet. + struct Frame + { + char command; + std::vector args; + bool operator==(const Frame& frame); + }; + private: + Log log_; + Serial serial_; + Receiver &receiver_; + + /// File d'attente des messages à aquiter. + typedef std::queue FrameQueue; + FrameQueue frameQueue_; + //Date du dernier envoie + int tLastSend_; + //Temps entre les ré-émission + static const int timeout_ = 100; + + /// Frame en cours de réception + Frame currentFrame_; + int revState_; + // Etat de la réception de la frame + // 0 - Rien reçu + // 1 - Bang reçu + // 2 - Commande reçu & nouveau argument + // 3 - char 1 argument reçu + + public: + /// Constructeur. + Proto (Receiver &receiver); + /// Ouvre le port série. + void open (const std::string &ttyname); + /// Ferme le port série. + void close (void); + /// Teste si tout les packets ont été envoyés et aquités, sinon, essaye de + /// le faire. + bool sync (void); + /// Envois un packet. + void send (const Frame &frame, bool reliable = true); + /// Envois un packet. COMMAND est la commande à envoyer, FORMAT, donne le + /// format et le nombre de paramètres ('b' : 8 bits, 'w' : 16 bits, 'd' : + /// 32 bits, majuscule pour signé). + void send (char command, const char *format = "", int a0 = 0, int a1 = 0, + int a2 = 0, int a3 = 0, int a4 = 0, int a5 = 0, bool reliable = true); + /// Send a string of size number. + void sendStr (char command, const char *str, int size, bool reliable = + true); + /// permet d'envoyer un packet robert + void sendUnreliable (char command, const char *format, int a0, + int a1, int a2, int a3, int a4, int a5); + //@{ + /// Teste si le packet correspond au format et décode les valeurs. Utilise + /// le même format que send. + static bool decode (const Frame &frame); + static bool decode (const Frame &frame, const char *format, int &a0); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1, int &a2); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1, int &a2, int &a3); + static bool decode (const Frame &frame, const char *format, int &a0, int + &a1, int &a2, int &a3, int &a4); + //@} + /// Attend que des caractères soit disponible pendant un temps maximum en + /// milliseconde et les traite. Renvois le résultat de sync (). Attention, + /// fonction bloquante, n'utiliser que pour les tests. + bool wait (int timeout = -1); + /// Récupère le File Descriptor + int getFd(void) const; + + public: + /// Les clients de Proto doivent dériver de Receiver. + class Receiver + { + public: + virtual ~Receiver(void) {}; + /// Recoit un packet. + virtual void receive (char command, const Frame &frame) = 0; + }; + 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(Proto::Frame & frame, char format, int arg); + /// Renvoie la taille necessaire du vecteur args pour un format donné + static int argsFrameSize(const char *format); + /// Décode un argument + static void decodeArg(const Frame & frame, const char *format, int &a0, + int &a1, int &a2, int &a3, int &a4); +}; + +/// Affiche une frame. +std::ostream & +operator<< (std::ostream &os, const Proto::Frame &f); + +#endif // proto_hh diff --git a/i/chuck/src/proto/test_proto.cc b/i/chuck/src/proto/test_proto.cc new file mode 100644 index 0000000..01cac96 --- /dev/null +++ b/i/chuck/src/proto/test_proto.cc @@ -0,0 +1,96 @@ +// test_proto.cc +// robert - programme du robot 2006. {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2006. +// 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" +#include "timer/timer.hh" +#include "tester/tester.hh" +#include "parser/parser.hh" + +#include +#include + +class TestProto : public Tester, Proto::Receiver +{ + private: + Proto proto_; + public: + // Constructor + TestProto (int argc, char ** argv) + : Tester (argc, argv), proto_(*this){} + void preRun (void) + { + Interpreter &interpreter = getInterpreter (); + // Récupère le tty de config + proto_.open(config_.get("testProto.tty")); + // Add functions. + interpreter.add ("s", Interpreter::memFunc ( *this, &TestProto::send ), + "Fonction send (string commande, string format, " + "int arg...)"); + interpreter.add ("w", Interpreter::memFunc ( proto_, &Proto::wait ), + "Fonction wait ()"); + } + void postRun(void) + { + proto_.close(); + } + void receive (char command, const Proto::Frame &frame) + { + std::cout << "received " << frame << std::endl; + } + void send (const Parser::AnyList & al, bool dryrun) + { + Parser::AnyList::const_iterator it = al.begin(); + // arguments envoyé à la fonctions Proto::send + int arg[6]; + // command avr + char command = any_cast (*it); + // format des arguments + std::string format = any_cast (*++it); + // Récupération des arguments + for (unsigned i = 0; i < 6; ++i) + { + if(i < al.size() - 2) + arg[i] = any_cast (*++it); + else + arg[i] = 0; + } + if (!dryrun) + proto_.send(command, format.c_str(), arg[0], arg[1], arg[2], + arg[3], arg[4], arg[5]); + } +}; + +int +main (int argc, char **argv) +{ + try + { + TestProto tp (argc, argv); + tp.run (); + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + } +} -- cgit v1.2.3