From 1d57bcc0e69306423d76cfa40a37bad2acf61577 Mon Sep 17 00:00:00 2001 From: dufourj Date: Wed, 24 May 2006 01:13:15 +0000 Subject: Marvin : * Es : - supporte pleins de nouveauté du code ES avr. * Proto : - commande moche qui envoye un string plutôt qu'une liste d'arguments ; - support de 5 arguments max au lieu de 4. --- i/marvin/runtime/rc/config | 12 +-- i/marvin/src/es/es.cc | 224 +++++++++++++++++++++++++++++++++++++------- i/marvin/src/es/es.hh | 75 ++++++++++++--- i/marvin/src/es/test_es.cc | 93 +++++++++++++++++- i/marvin/src/proto/proto.cc | 52 +++++++--- i/marvin/src/proto/proto.hh | 7 +- 6 files changed, 389 insertions(+), 74 deletions(-) (limited to 'i') diff --git a/i/marvin/runtime/rc/config b/i/marvin/runtime/rc/config index 8744cc1..320d121 100644 --- a/i/marvin/runtime/rc/config +++ b/i/marvin/runtime/rc/config @@ -28,11 +28,7 @@ asserv.pwm_stat = 0 asserv.timer_stat = 0 ### ES -es.tty = "/dev/ttyS0" - -### Motor -motor.pStatPosition = 8 -motor.pStatMotor = 0 +es.tty = "/dev/ttyS1" ### Log # Default minimum level outputed @@ -47,8 +43,10 @@ motor.pStatMotor = 0 # - file:/path/filename : to a file ; # - ram : in a circular buffer for getting it at the end of the program by # network. -log.logger.default = "ram" +# log.logger.default = "ram" # log.logger.asserv = "null" +#log.logger.default = "stdout" +#log.level.default = "debug" ### Exemples foo = "toto" @@ -56,4 +54,4 @@ foo = "toto" foobar = (4 5 6 7 8) # TestProto -testProto.tty = "/home/nicolas/robotSVN/n/asserv/src/asserv/uart0.pts" +#testProto.tty = "/home/nicolas/robotSVN/n/asserv/src/asserv/uart0.pts" diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 8454ac5..b397cef 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -26,35 +26,58 @@ #include "es/es.hh" #include "config/config.hh" +#include +#include + /// Constructeur Es::Es (const Config & config) - :proto_(*this), log_("Es") + : proto_ (*this), log_ ("Es"), lcd_key_pressed_ (-1), + front_sensor_ (false) { // Récupération des valeurs de configuration dans le fichier - loadConfig(config); - proto_.open(tty_); + loadConfig (config); + proto_.open (tty_); + + sensorsRvb_.resize (9); + for (int compt = 0; compt < 9; compt++) + sensorsRvb_[0].resize (4); + sniffRvb_.resize (9); + sharps_.resize (3); } -void Es::init(void) +void +Es::reset (void) { // On reset l'AVR - proto_.send('z'); - // XXX Voir pour envoyer les config + proto_.send ('z'); + // Send configuration TODO + // We want to reference some sensors + // First, enable all sensors + enableAllSensors (true); + /// XXX Is it to often ? +// setAckStat (1); + // XXX We should wait ! + // Reference sensors 1-7 + setRefColor (127, 0); + // Disable useless sensors + enableAllSensors (false); } bool -Es::wait(int timeout /*-1*/) +Es::wait (int timeout /*-1*/) { - return proto_.wait(timeout); + return proto_.wait (timeout); } /// Récupère le File Descriptor -int Es::getFd(void) +int +Es::getFd (void) { return proto_.getFd(); } -void Es::loadConfig(const Config & config) +void +Es::loadConfig (const Config & config) { tty_ = config.get("es.tty"); } @@ -62,80 +85,215 @@ void Es::loadConfig(const Config & config) // Envoie de la config des sensors RVB void Es::setRVBSensorsConfig(int false_ic, int max_ov) { - proto_.send('p',"bw", false_ic, max_ov); + proto_.send ('p',"bb", false_ic, max_ov); } // Envoie de la config des sniff RVB -void Es::setRVBSniffConfig(int ref_ratio) -{ - proto_.send('x', "b", ref_ratio); -} -void Es::receive(char command, const Proto::Frame & frame) +void Es::setRVBSniffConfig (int ref_ratio, int green_limit, int clear_limit) { + proto_.send ('x', "bww", ref_ratio, green_limit, clear_limit); } // Règle les stats des sensors RVB void Es::setRVBSensorsStat(int mask_captor, int freq) { - proto_.send('S', "wb", mask_captor, freq); + proto_.send ('S', "wb", mask_captor, freq); } // règle la couleur actuelle comme référente -void Es::setRefColor(int mask_captor, int mode) +void Es::setRefColor(int mask_captor, int mode = 0) { - proto_.send('r', "wb", mask_captor, mode); + proto_.send ('r', "wb", mask_captor, mode); } -/// Règle les stats si le "sensors is seen" -// XXX Faudra lui filer un vrai nom quand même -void Es::setRVBSeenStat(int mask_captor, int freq) +/// Règle les stats d'affichage de la couleur +void +Es::setRVBSniffStat (int mask_captor, int freq) { - proto_.send('A', "wb", mask_captor, freq); + proto_.send ('A', "wb", mask_captor, freq); +} + +/// Configure statistic for the sensor of the ball +void +Es::setRVBBallStat (int sensor_num, int freq) +{ + proto_.send ('B', "bb", sensor_num, freq); } // Discute avec les servo... -void Es::setServoPos(int servoNb, int servoPos) +void Es::setServoPos (int servo_mask, int servoPos) +{ + proto_.send ('m', "bb", servo_mask, servoPos); +} + +/// Set update frequency of sharps +void +Es::setSharpUpdate (int sharp_mask, int freq) +{ + proto_.send ('h', "bb", sharp_mask, freq); +} + +/// Set statistics frequency of sharps +void +Es::setSharpStat (int sharp_mask, int freq) +{ + proto_.send ('H', "bb", sharp_mask, freq); +} + +/// Set configuration of threshold sharps +void +Es::setSharpThreshold (int sharp_num, int threshold_high, + int threshold_low) +{ + proto_.send ('o', "bww", sharp_num, threshold_high, threshold_low); +} + +/// Print something on the LCD (max 32 char) +void +Es::lcdPrint (const std::string &message) +{ + // XXX Yerk ! + std::memset (lcd_mess_char_, '\0', 32); + int size = message.size (); + std::memcpy (lcd_mess_char_, message.data (), size > 32 ? 32 : size); + proto_.send ('L', lcd_mess_char_, 32); +} + +/// Get the current pressed keys of the LCD +void +Es::lcdGetKey (int freq) { - proto_.send('m', "bb", servoNb, servoPos); + proto_.send ('l', "b", freq); } // Règle la vitesse des turbines void Es::setTurbineSpeed(int turbNb, int speed) { - proto_.send('v', "bw", turbNb, speed); + proto_.send ('v', "bw", turbNb, speed); } // Règle le sens de rotation du barillet void Es::setTheMeaningOfRotationOfBarillet(int answer) { - proto_.send('w', "b", answer); + proto_.send ('w', "b", answer); } // Init n°2 of barillet void Es::init2Barillet(void) { - proto_.send('g'); + proto_.send ('g'); } // Dépose une balle du barillet void Es::deposeBalle(void) { - proto_.send('d'); + proto_.send ('d'); } /// Extrait une balle void Es::extraitBalle(void) { - proto_.send('e'); + proto_.send ('e'); } /// Rotation du barillet void Es::rotationBarillet(int posFinal) { - proto_.send('t', "b", posFinal); + proto_.send ('t', "b", posFinal); +} + +/// Put barillet in sleep mode +void +Es::barilletSleep (void) +{ + proto_.send ('s'); +} + +/// Enable all the sensors or just the 4 and 1 near the ground +void +Es::enableAllSensors (bool enable) +{ + if (enable) + proto_.send ('u', "b", 1); + else + proto_.send ('u', "b", 0); +} + +/// Shut up ! +void +Es::shutUp (void) +{ + proto_.send ('f', "b", 0); +} + +/// Set frequency of ack +void +Es::setAckStat (int freq) +{ + proto_.send ('F', "b", freq); } -bool Es::sync(void) +/// Stat for the main () +void +Es::setMainStat (int freq) { - return proto_.sync(); + proto_.send ('Z', "b", freq); } + +void Es::receive(char command, const Proto::Frame & frame) +{ + int errCode, red, blue, clear, green, compt, value; + + switch (command) + { + case 'F': + if (proto_.decode (frame, "b", errCode)) + { + switch (errCode) + { + case 1: + front_sensor_ = true; + break; + } + shutUp (); + } + break; + case 'Z': + // TODO + break; + case 'S': + if (proto_.decode (frame, "wwwww", compt, red, blue, clear, green)) + { + sensorsRvb_[compt][0] = red; + sensorsRvb_[compt][1] = blue; + sensorsRvb_[compt][2] = clear; + sensorsRvb_[compt][3] = green; + } + break; + case 'A': + if (proto_.decode (frame, "bb", compt, value)) + { + sniffRvb_[compt] = value; + } + break; + case 'B': + if (proto_.decode (frame, "bb", compt, value)) + { + sniffRvb_[compt] = value; + } + break; + case 'H': + if (proto_.decode (frame, "ww", compt, value)) + { + std::cout << "[" << compt << "]" << " = " << value << std::endl; + sharps_[compt] = value; + } + break; + } +} + +bool Es::sync (void) +{ + return proto_.sync (); +} + diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh index deb9771..ecdcd08 100644 --- a/i/marvin/src/es/es.hh +++ b/i/marvin/src/es/es.hh @@ -28,12 +28,17 @@ #include "proto/proto.hh" #include +#include class Config; /// Gère la carte es et ses capteurs et actionneurs class Es : public Proto::Receiver { + typedef std::vector SensorRVB; + typedef std::vector SensorsRVB; + typedef std::vector SniffRVB; + typedef std::vector Sharps; private: // Objet Proto de communication vers la carte es Proto proto_; @@ -41,40 +46,70 @@ class Es : public Proto::Receiver std::string tty_; /// Système de log Log log_; + /// For LCD communication + char lcd_mess_char_[32]; + /// Key pressed by the LCD Keyboard + int lcd_key_pressed_; + /// Frontal choc ! + bool front_sensor_; + /// Vector of sensors + SensorsRVB sensorsRvb_; + /// Sniff RVB + SniffRVB sniffRvb_; + /// Sharps + Sharps sharps_; public: /// Constructeur Es (const Config & config); - /// Initialise les capteurs et actionneurs - void init(void); + /// Initialise les capteurs et actionneurs, reset + void reset (void); /// On attend ... - bool wait(int timeout = -1); + bool wait (int timeout = -1); /// Récupère le File Descriptor - int getFd(void); + int getFd (void); /// définition du receiver::receive - void receive(char command, const Proto::Frame & frame); + void receive (char command, const Proto::Frame & frame); /// sync.. - bool sync(void); - private: + bool sync (void); + /// Charge les paramètre du fichier de config - void loadConfig(const Config & config); + void loadConfig (const Config & config); /// Envoie de la config des sensors RVB void setRVBSensorsConfig(int false_ic, int max_ov); /// Envoie de la config des sniff RVB - void setRVBSniffConfig(int ref_ratio); + void setRVBSniffConfig(int ref_ratio, int green_limit, int clear_limit); /// Règle les stats des sensors RVB void setRVBSensorsStat(int mask_captor, int freq); /// règle la couleur actuelle comme référente void setRefColor(int mask_captor, int mode); - /// Règle les stats si le "sensors is seen" - // XXX Faudra lui filer un vrai nom quand même - void setRVBSeenStat(int mask_captor, int freq); + /// Règle les stats d'affichage de la couleur + void setRVBSniffStat (int mask_captor, int freq); + /// Configure statistic for the sensor of the ball + void setRVBBallStat (int sensor_num, int freq); + /// Enable all the sensors or just the 4 and 1 near the ground + void enableAllSensors (bool enable); + /// Discute avec les servo... - void setServoPos(int servoNb, int servoPos); + void setServoPos (int servoNb, int servoPos); + + /// Set update frequency of sharps + void setSharpUpdate (int sharp_mask, int freq); + /// Set statistics frequency of sharps + void setSharpStat (int sharp_mask, int freq); + /// Set configuration of threshold sharps + void setSharpThreshold (int sharp_num, int threshold_high, + int threshold_low); + + /// Print something on the LCD (max 32 char) + void lcdPrint (const std::string &message); + /// Get the current pressed keys of the LCD + void lcdGetKey (int freq); + /// Règle la vitesse des turbines - void setTurbineSpeed(int turbNb, int speed); + void setTurbineSpeed (int turbNb, int speed); /// Règle le sens de rotation du barillet - void setTheMeaningOfRotationOfBarillet(int answer); + void setTheMeaningOfRotationOfBarillet (int answer); /// Init n°2 of barillet void init2Barillet(void); /// Dépose une balle du barillet @@ -83,5 +118,15 @@ class Es : public Proto::Receiver void extraitBalle(void); /// Rotation du barillet void rotationBarillet(int posFinal); + /// Put barillet in sleep mode + void barilletSleep (void); + + /// Shut up ! + void shutUp (void); + /// Set frequency of ack + void setAckStat (int freq); + + /// Stat for the main () + void setMainStat (int freq); }; #endif // es_hh diff --git a/i/marvin/src/es/test_es.cc b/i/marvin/src/es/test_es.cc index be60620..13ca8a3 100644 --- a/i/marvin/src/es/test_es.cc +++ b/i/marvin/src/es/test_es.cc @@ -25,11 +25,32 @@ #include "tester/tester.hh" #include "es/es.hh" +#include "timer/timer.hh" + +#include class TestEs : public Tester { private: Es es_; + /// Called after each command called. + void postcall (void) + { + while (!es_.wait ()) + ; + } + /// Wait. + void wait (int ms) + { + int t, stop; + t = Timer::getProgramTime (); + stop = t + ms; + while (t < stop) + { + es_.wait (stop - t); + t = Timer::getProgramTime (); + } + } public: // Constructor TestEs (int argc, char ** argv) @@ -38,12 +59,78 @@ class TestEs : public Tester { Interpreter &interpreter = getInterpreter (); // Add functions. - interpreter.add ("z", Interpreter::memFunc ( es_, &Es::init ), "Fonction reset ()"); - interpreter.add ("w", Interpreter::memFunc ( es_, &Es::wait ), "Fonction wait ()"); + interpreter.add ("reset", Interpreter::memFunc (es_, &Es::reset), + "Reset ()"); + interpreter.add ("wait", Interpreter::memFunc (*this, &TestEs::wait), + "Blocking wait (time)\n" + " - time : time to wait in ms"); + interpreter.add + ("rvbStat", Interpreter::memFunc (es_, &Es::setRVBSensorsStat), + "Enable RVB Sensors raw stats (mask, freq)"); + interpreter.add + ("colorStat", Interpreter::memFunc (es_, &Es::setRVBSniffStat), + "Enable RVB Sensors color stats (mask, freq)"); + interpreter.add + ("ballStat", Interpreter::memFunc (es_, &Es::setRVBBallStat), + "Enable Ball stats (num, freq)\n" + " - num : sensor number (7 or 8)"); + interpreter.add + ("colorRef", Interpreter::memFunc (es_, &Es::setRefColor), + "Reference current color as green for sensors (mask)"); + interpreter.add + ("rvbAll", Interpreter::memFunc (es_, &Es::enableAllSensors), + "Drop a ball at the rear ()"); + interpreter.add + ("servoPos", Interpreter::memFunc (es_, &Es::setServoPos), + "Set servo position (mask, pos)"); + interpreter.add + ("sharpUp", Interpreter::memFunc (es_, &Es::setSharpUpdate), + "Set sharp update frequency (mask, freq)"); + interpreter.add + ("sharpStat", Interpreter::memFunc (es_, &Es::setSharpStat), + "Set sharp stats (mask, freq)"); + interpreter.add + ("sharpThreshold", Interpreter::memFunc (es_, &Es::setSharpThreshold), + "Set sharp threshold (num, high, low)\n" + " - num : sharp number (1-3)\n" + " - high : high threshold\n" + " - low : low threshold"); + interpreter.add + ("lcdPrint", Interpreter::memFunc (es_, &Es::lcdPrint), + "Print a message (32 char max) onto the LCD (message)"); + interpreter.add + ("lcdKey", Interpreter::memFunc (es_, &Es::lcdGetKey), + "Set stat for key if pressed (freq)"); + interpreter.add + ("turbSpeed", Interpreter::memFunc (es_, &Es::setTurbineSpeed), + "Set turbine speed (num, speed)\n" + " - num : 1 front, 2 rear\n" + " - speed : 256 - 819"); + interpreter.add + ("turbMin", Interpreter::memFunc (es_, &Es::init2Barillet), + "Turbine minimal speed ()"); + interpreter.add + ("turbDep", Interpreter::memFunc (es_, &Es::deposeBalle), + "Drop a ball at the rear ()"); + interpreter.add + ("barGoTo", Interpreter::memFunc (es_, &Es::rotationBarillet), + "Barillet go to position (pos)\n" + " - pos : in 40° of a round"); + interpreter.add + ("barSleep", Interpreter::memFunc (es_, &Es::barilletSleep), + "Barillet in sleep mode (low speed turbine)"); + interpreter.add + ("statAck", Interpreter::memFunc (es_, &Es::setAckStat), + "Set ack stat (freq)"); + interpreter.add + ("statMain", Interpreter::memFunc (es_, &Es::setMainStat), + "Set main stat (freq)"); + interpreter.add ("_postcall", + Interpreter::memFunc (*this, &TestEs::postcall)); } }; - int +int main (int argc, char **argv) { try diff --git a/i/marvin/src/proto/proto.cc b/i/marvin/src/proto/proto.cc index 07a6634..4380f07 100644 --- a/i/marvin/src/proto/proto.cc +++ b/i/marvin/src/proto/proto.cc @@ -59,7 +59,7 @@ Proto::sync(void) // Récupération de la frame while (getFrame()) { -// log_ ("recv") << "frame" << currentFrame_; +// log_ ("recv", Log::debug) << "frame" << currentFrame_; // Si la frame est un aquittement if(currentFrame_ == frameQueue_.front()) { @@ -119,22 +119,34 @@ Proto::send (char command, const char *format, int a0, int a1, { 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::send (char command, const std::string &str, int size, bool reliable) +{ + Proto::Frame frame; + + 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, @@ -149,21 +161,21 @@ bool Proto::decode (const Proto::Frame &frame) { int dummy; - return decode(frame, "", dummy, dummy, dummy, 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); + 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); + return decode(frame, format, a0, a1, dummy, dummy, dummy); } bool @@ -171,18 +183,26 @@ Proto::decode (const Frame &frame, const char *format, int &a0, int &a1, int &a2) { int dummy; - return decode(frame, format, a0, a1, a2, 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); + decodeArg(frame, format, a0, a1, a2, a3, a4); return true; } @@ -267,7 +287,8 @@ Proto::getFrame(void) { // On renvoie en mettant le compteur à 0, la commande sera // renvoyer de retour à sync -// log_("Erreur de reception") << "commande inconnue" << ""; +// log_("Erreur de reception", Log::debug) +// << "commande inconnue" << ""; tLastSend_ = 0; revState_ = 0; return false; @@ -280,7 +301,7 @@ Proto::getFrame(void) void Proto::sendFrame(const Frame & frame) { -// log_ ("send") << "frame" << frame; +// log_ ("send", Log::debug) << "frame" << frame; // envoyer le bang serial_.putchar('!'); @@ -355,9 +376,9 @@ Proto::argsFrameSize(const char *format) /// Décode un argument void -Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int &a2, int &a3) +Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int &a2, int &a3, int &a4) { - int temp[4]; + int temp[5]; int pos = 0; for(int i = 0; *format != '\0'; format++,i++) @@ -412,6 +433,7 @@ Proto::decodeArg(const Frame & frame, const char *format, int &a0, int &a1, int a1 = temp[1]; a2 = temp[2]; a3 = temp[3]; + a4 = temp[4]; } bool diff --git a/i/marvin/src/proto/proto.hh b/i/marvin/src/proto/proto.hh index 47ef9a0..4e1f24c 100644 --- a/i/marvin/src/proto/proto.hh +++ b/i/marvin/src/proto/proto.hh @@ -83,6 +83,9 @@ class Proto : public NonCopyable /// 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 send (char command, const std::string &str, int size, bool reliable = + true); /// permet d'envoyer un packet robert void Proto::sendUnreliable (char command, const char *format, int a0, int a1, int a2, int a3, int a4, int a5); @@ -97,6 +100,8 @@ class Proto : public NonCopyable &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, @@ -125,7 +130,7 @@ class Proto : public NonCopyable 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 &a1, int &a2, int &a3, int &a4); }; /// Affiche une frame. -- cgit v1.2.3