From 08c46b0aa2c1791ca404a60ca0f75e4687553076 Mon Sep 17 00:00:00 2001 From: dufourj Date: Wed, 24 May 2006 20:13:50 +0000 Subject: Ai : - rajout de parametres ; - rajout de fonctions. Es : - fonctions de gettage de jack/couleur de sélection. --- i/marvin/src/ai/ai.cc | 93 ++++++++++++++++++++++++++++++++++++++------- i/marvin/src/ai/ai.hh | 11 +++++- i/marvin/src/es/es.cc | 36 +++++++++++------- i/marvin/src/es/es.hh | 11 ++++-- i/marvin/src/es/test_es.cc | 2 +- i/marvin/src/motor/motor.cc | 8 ++++ i/marvin/src/motor/motor.hh | 2 + 7 files changed, 129 insertions(+), 34 deletions(-) (limited to 'i/marvin') diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc index a24dcc7..1adcf5e 100644 --- a/i/marvin/src/ai/ai.cc +++ b/i/marvin/src/ai/ai.cc @@ -24,14 +24,16 @@ #include "ai.hh" //constructeur -Ai::Ai(const Config & config) - :motor_(config), es_(config), +Ai::Ai (const Config & config) + : motor_(config), es_(config), log_ ("Ai"), schedulableMotor_(callback, motor_.getFd()), schedulableEs_(callback, es_.getFd()), - roundDuration_(config.get("ai.roundDuration")) + round_duration_ (config.get ("ai.round_duration")), + schedule_time_ (config.get ("ai.schedule_time")), + reference_sensor_mask_ (config.get ("ai.ref_sensor_mask")) { - scheduler_.insert(schedulableMotor_); - scheduler_.insert(schedulableEs_); + scheduler_.insert (schedulableMotor_); + scheduler_.insert (schedulableEs_); } Ai::~Ai(void) @@ -39,7 +41,7 @@ Ai::~Ai(void) // On réinitialise // Initialise les moteurs // XXX -// motor_.init (); + motor_.reset (); // initialise la carte es es_.reset (); // On sync @@ -49,18 +51,28 @@ Ai::~Ai(void) } while (!sync ()); } + // Initialisation du robal void -Ai::init(void) +Ai::init (void) { // Initialisation de motor - motor_.init (); + motor_.reset (); // Initialisation de la carte Es es_.reset (); // Paf on sync while (!update ()); } +/// Stop the mouvement of the motor +void +Ai::stop (void) +{ + motor_.stop (); + // XXX In fact, we should wait a little here... + while (!update ()); +} + /// Synchronize data of under class bool Ai::sync (void) @@ -69,17 +81,72 @@ Ai::sync (void) } /// Wait for something to happened -bool Ai::update (void) +bool +Ai::update (void) { - scheduler_.schedule (100, true); + /// Wait schedule_time + scheduler_.schedule (schedule_time_, true); bool retour = sync (); // On vérifie que le match n'est pas fini - if (Timer::getRoundTime () > roundDuration_) - throw std::runtime_error ("Fin de match, merci d'avoir - participé !"); + if (Timer::getRoundTime () > round_duration_) + throw std::runtime_error ("End of match ! You win !"); return retour; } +/// Function to wait a little but still syncing data +void +Ai::temporisation(int msec) +{ + // Get current time of process + int time = Timer::getProgramTime(); + time += msec; + // Update until we have spend enough time + do + { + update(); + } + while (time > Timer::getProgramTime()); +} + +/// Wait until jack is out (true) or in (false) +void +Ai::waitJack (bool out) +{ + do + { + udpate (); + } + while (es_.isJackOut () != out); +} + +/// Init things for a match. +void +Ai::prepare (void) +{ + // XXX We should check if the jack is not already in + // We first wait for the jack to be put inside + waitJack (false); + // We reference all the color + referenceSensors (); + // We first wait for the jack to be put inside + waitJack (true); + // Ok the match begin ! Go go go ! + Timer::startRound (); +} + +/// Reference sensors +void +Ai::referenceSensors (void) +{ + // Reference sensors + es_.refColor (reference_sensor_mask_); + // Update data + while (!update ()); + // Disable capturing all sensors + es_.enableAllSensors (false); + // Update data + while (!update ()); +} /// programme d'homologation du robal void Ai::progHomoloRobal(void) diff --git a/i/marvin/src/ai/ai.hh b/i/marvin/src/ai/ai.hh index 2883be9..25bba63 100644 --- a/i/marvin/src/ai/ai.hh +++ b/i/marvin/src/ai/ai.hh @@ -45,7 +45,9 @@ class Ai scheduler::SchedulableReadFd schedulableMotor_; scheduler::SchedulableReadFd schedulableEs_; // Paramètres générales du robot - const int RoundDuration_; + const int round_duration_; + const int schedule_time_; + const int reference_sensor_mask_; public: /// Constructeur Ai(const Config & config); @@ -57,7 +59,6 @@ class Ai /// Arrête complêtement le robot void stop(void); /// Fonction de tempo - /// XXX Vérifier que ce soit bien des milisec void temporisation (int msec); /// Attend le jack rentre(false) ou sorte (true) void waitJack (bool out); @@ -65,6 +66,12 @@ class Ai bool update (void); /// La célèbre fonction sync bool sync (void); + /// Stop the mouvement of the motor + void stop (void); + /// Init things for a match. + void prepare (void); + /// Reference sensors + void referenceSensors (void); /// programme d'homologation du robal void progHomoloRobal(void); }; diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 8bc90ec..ab73b3e 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -32,7 +32,7 @@ /// Constructeur Es::Es (const Config & config) : proto_ (*this), log_ ("Es"), lcdKeyPressed_ (-1), - front_sensor_ (false), jack_ (false), colorModeBlue_ (false) + front_sensor_ (false), jackIn_ (false), colorModeBlue_ (false) { // Récupération des valeurs de configuration dans le fichier loadConfig (config); @@ -73,7 +73,6 @@ Es::loadConfig (const Config & config) rvbSniffClearLimit_ = config.get ("es.rvb_sniff_clear_limit"); rvbSensorMaskStat_ = config.get ("es.rvb_sensor_mask_stat"); rvbSensorStat_ = config.get ("es.rvb_sensor_stat"); - rvbSniffRefMask_ = config.get ("es.rvb_sniff_ref_mask"); rvbSniffMaskStat_ = config.get ("es.rvb_sniff_mask_stat"); rvbSniffStat_ = config.get ("es.rvb_sniff_stat"); rvbSniffFrontStat_ = config.get ("es.rvb_sniff_front_stat"); @@ -90,7 +89,6 @@ Es::reset (void) // First, enable all sensors setRVBSensorsConfig (rvbSensorFalseIC_, rvbSensorMaxOv_); - enableAllSensors (true); // Send configurations setMainStat (mainStat_); @@ -104,11 +102,7 @@ Es::reset (void) setRVBSniffFrontStat (rvbSniffFrontStat_); lcdGetKey (lcdKeyStat_); - // We want to reference some sensors - // XXX We should wait ! - setRefColor (rvbSniffRefMask_); - // Disable useless sensors - enableAllSensors (false); + enableAllSensors (true); } /// Stat for the main () @@ -133,6 +127,20 @@ Es::setAckStat (int freq) proto_.send ('F', "b", ackFreq_); } +/// Get the state of the jack +bool +Es::isJackOut (void) +{ + return !jackIn_; +} + +/// Get the color mode of the button +bool +Es::isColorModeBlue (void) +{ + return colorModeBlue_; +} + // Envoie de la config des sensors RVB void Es::setRVBSensorsConfig(int false_ic, int max_ov) { @@ -152,7 +160,7 @@ void Es::setRVBSensorsStat(int mask_captor, int freq) } // règle la couleur actuelle comme référente -void Es::setRefColor(int mask_captor, int mode) +void Es::refColor(int mask_captor, int mode) { proto_.send ('r', "wb", mask_captor, mode); } @@ -373,22 +381,22 @@ void Es::receive(char command, const Proto::Frame & frame) switch (value) { case 0: - jack_ = colorModeBlue_ = false; + jackIn_ = colorModeBlue_ = false; break; case 1: - jack_ = true; + jackIn_ = true; colorModeBlue_ = false; break; case 2: - jack_ = false; + jackIn_ = false; colorModeBlue_ = true; break; case 3: - jack_ = colorModeBlue_ = true; + jackIn_ = colorModeBlue_ = true; break; } log_ ("Others", Log::debug) << "Color mode " << (colorModeBlue_ ? "Blue" : "Red") - << (jack_ ? ", jack in..." : ", jack out !!!"); + << (jackIn_ ? ", jack in..." : ", jack out !!!"); } break; /* LCD */ diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh index dd9ac1c..cbf989e 100644 --- a/i/marvin/src/es/es.hh +++ b/i/marvin/src/es/es.hh @@ -59,8 +59,6 @@ class Es : public Proto::Receiver int rvbSniffRefRatio_, rvbSniffGreenLimit_, rvbSniffClearLimit_; /// RVB Sensors raw stats int rvbSensorMaskStat_, rvbSensorStat_; - /// Reference which sensor at boot time - int rvbSniffRefMask_; /// RVB Sniff stats int rvbSniffMaskStat_, rvbSniffStat_; /// RVB Sensors raw stats @@ -70,7 +68,7 @@ class Es : public Proto::Receiver /// Sharps Sharps sharps_; /// Others module, jack & colour - bool jack_, colorModeBlue_; + bool jackIn_, colorModeBlue_; int othersStat_; /// Stat of front sensors int rvbSniffFrontStat_; @@ -97,6 +95,10 @@ class Es : public Proto::Receiver void shutUp (void); /// Set frequency of ack void setAckStat (int freq); + /// Get the state of the jack + bool isJackOut (void); + /// Get the color mode of the button + bool isColorModeBlue (void); /// Envoie de la config des sensors RVB void setRVBSensorsConfig(int false_ic, int max_ov); @@ -105,7 +107,7 @@ class Es : public Proto::Receiver /// 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 = 0); + void refColor(int mask_captor, int mode = 0); /// Règle les stats d'affichage de la couleur void setRVBSniffStat (int mask_captor, int freq); /// Configure statistic for the sensor of the ball @@ -116,6 +118,7 @@ class Es : public Proto::Receiver void enableAllSensors (bool enable); /// Set frequency of jack, selectoul printed out function + /// Use this function with frequency 0 to disable this stat. void setOthersStat (int freq); /// Discute avec les servo... diff --git a/i/marvin/src/es/test_es.cc b/i/marvin/src/es/test_es.cc index 4213ebb..e5c90e5 100644 --- a/i/marvin/src/es/test_es.cc +++ b/i/marvin/src/es/test_es.cc @@ -65,7 +65,7 @@ class TestEs : public Tester "Blocking wait (time)\n" " - time : time to wait in ms"); interpreter.add - ("colorRef", Interpreter::memFunc (es_, &Es::setRefColor), + ("colorRef", Interpreter::memFunc (es_, &Es::refColor), "Reference current color as green for sensors (mask)"); interpreter.add ("rvbAll", Interpreter::memFunc (es_, &Es::enableAllSensors), diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc index af3d29f..2664df0 100644 --- a/i/marvin/src/motor/motor.cc +++ b/i/marvin/src/motor/motor.cc @@ -30,6 +30,14 @@ Motor::Motor (void) { } +/// Reset. +void +Motor::reset (void) +{ + finish_ = false; + asserv_.reset (); +} + /// Linear and angular move. T(mm) is the linear distance. A(mm) is the /// angular distance which means the arc length. void diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh index 509ed8b..5770ebb 100644 --- a/i/marvin/src/motor/motor.hh +++ b/i/marvin/src/motor/motor.hh @@ -54,6 +54,8 @@ class Motor : public Asserv::Receiver void findHole (void); /// Stop now. void stop (void); + /// Reset. + void reset (void); private: /// Next seq number. inline void nextSeq (void); -- cgit v1.2.3