summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--i/marvin/src/ai/ai.cc93
-rw-r--r--i/marvin/src/ai/ai.hh11
-rw-r--r--i/marvin/src/es/es.cc36
-rw-r--r--i/marvin/src/es/es.hh11
-rw-r--r--i/marvin/src/es/test_es.cc2
-rw-r--r--i/marvin/src/motor/motor.cc8
-rw-r--r--i/marvin/src/motor/motor.hh2
7 files changed, 129 insertions, 34 deletions
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<int>("ai.roundDuration"))
+ round_duration_ (config.get<int> ("ai.round_duration")),
+ schedule_time_ (config.get<int> ("ai.schedule_time")),
+ reference_sensor_mask_ (config.get<int> ("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<int> ("es.rvb_sniff_clear_limit");
rvbSensorMaskStat_ = config.get<int> ("es.rvb_sensor_mask_stat");
rvbSensorStat_ = config.get<int> ("es.rvb_sensor_stat");
- rvbSniffRefMask_ = config.get<int> ("es.rvb_sniff_ref_mask");
rvbSniffMaskStat_ = config.get<int> ("es.rvb_sniff_mask_stat");
rvbSniffStat_ = config.get<int> ("es.rvb_sniff_stat");
rvbSniffFrontStat_ = config.get<int> ("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);