summaryrefslogtreecommitdiff
path: root/2005
diff options
context:
space:
mode:
authorhaller2005-04-03 15:41:05 +0000
committerhaller2005-04-03 15:41:05 +0000
commit3e437f57e9bac0a4b9de8b556be0538e69fb200b (patch)
treec6d55c240806848bc9fd5e3013d09715d1a8b3e0 /2005
parent758187cf08f95b343756fcd803f9be05928802f3 (diff)
Ajout du fichier proto.cc
Modification de proto.hh:Ajout de fonctions et de variables privées
Diffstat (limited to '2005')
-rw-r--r--2005/i/robert/src/proto/proto.cc331
-rw-r--r--2005/i/robert/src/proto/proto.hh25
2 files changed, 356 insertions, 0 deletions
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<Frame> 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<uint8_t> 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