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/src/es/es.cc | 224 ++++++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 191 insertions(+), 33 deletions(-) (limited to 'i/marvin/src/es/es.cc') 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 (); +} + -- cgit v1.2.3