summaryrefslogtreecommitdiff
path: root/i/chuck/src/proto/proto.cc
diff options
context:
space:
mode:
authorbecquet2007-05-10 18:49:20 +0000
committerbecquet2007-05-10 18:49:20 +0000
commit8f486613be58ced269db1d437e560c16558604e8 (patch)
tree41e94b2122a118cb06abf6fc2a0038cd1dfbec4a /i/chuck/src/proto/proto.cc
parent4daa2c76c2a028e4b2c8ab379e7d1e0f535a0a31 (diff)
Création de chuck, le programme du robot 2007.
Diffstat (limited to 'i/chuck/src/proto/proto.cc')
-rw-r--r--i/chuck/src/proto/proto.cc457
1 files changed, 457 insertions, 0 deletions
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 <iostream>
+#include <algorithm>
+#include <iterator>
+
+/// 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<int>(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<uint8_t>(d) << 4);
+ revState_ = 3;
+ }
+ break;
+ case 3:
+ d = hex2digit (receivedChar);
+ if (d == hexInvalid)
+ erreur = true;
+ else
+ {
+ currentFrame_.args.back() |= static_cast<uint8_t>(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<uint8_t>::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<uint8_t>(arg));
+ break;
+ case 'w':
+ case 'W':
+ frame.args.push_back(static_cast<uint8_t>(arg >> 8));
+ frame.args.push_back(static_cast<uint8_t>(arg));
+ break;
+ case 'd':
+ case 'D':
+ frame.args.push_back(static_cast<uint8_t>(arg >> 24));
+ frame.args.push_back(static_cast<uint8_t>(arg >> 16));
+ frame.args.push_back(static_cast<uint8_t>(arg >> 8));
+ frame.args.push_back(static_cast<uint8_t>(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<int>(frame.args[pos]);
+ pos++;
+ break;
+ case 'B':
+ {
+ int8_t t = static_cast<int8_t>(frame.args[pos]);
+ temp[i] = static_cast<int>(t);
+ pos++;
+ break;
+ }
+ case 'w':
+ temp[i] = static_cast<int>(frame.args[pos]) << 8
+ |static_cast<int>(frame.args[pos + 1]);
+ pos += 2;
+ break;
+ case 'W':
+ {
+ int8_t t = static_cast<int8_t>(frame.args[pos]);
+ temp[i] = static_cast<int>(t) << 8
+ |static_cast<int>(frame.args[pos + 1]);
+ pos += 2;
+ break;
+ }
+ case 'd':
+ temp[i] = static_cast<int>(frame.args[pos]) << 24
+ |static_cast<int>(frame.args[pos + 1]) << 16
+ |static_cast<int>(frame.args[pos + 2]) << 8
+ |static_cast<int>(frame.args[pos + 3]);
+ pos += 4;
+ break;
+
+ case 'D':
+ {
+ int8_t t = static_cast<int8_t>(frame.args[pos]);
+ temp[i] = static_cast<int>(t) << 24
+ |static_cast<int>(frame.args[pos + 1]) << 16
+ |static_cast<int>(frame.args[pos + 2]) << 8
+ |static_cast<int>(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<int> (os, " "));
+ os << '>';
+ return os;
+}
+