summaryrefslogtreecommitdiff
path: root/i/marvin/src/es
diff options
context:
space:
mode:
authordufourj2006-05-24 01:13:15 +0000
committerdufourj2006-05-24 01:13:15 +0000
commit1d57bcc0e69306423d76cfa40a37bad2acf61577 (patch)
treef7b5643d5406c7c1ba6f163107df296d3aeea486 /i/marvin/src/es
parentfb348e99165ff47969a255b2bbd66a1be008da20 (diff)
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.
Diffstat (limited to 'i/marvin/src/es')
-rw-r--r--i/marvin/src/es/es.cc224
-rw-r--r--i/marvin/src/es/es.hh75
-rw-r--r--i/marvin/src/es/test_es.cc93
3 files changed, 341 insertions, 51 deletions
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 <cstring>
+#include <iostream>
+
/// 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<std::string>("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 <string>
+#include <vector>
class Config;
/// Gère la carte es et ses capteurs et actionneurs
class Es : public Proto::Receiver
{
+ typedef std::vector<int> SensorRVB;
+ typedef std::vector<SensorRVB> SensorsRVB;
+ typedef std::vector<int> SniffRVB;
+ typedef std::vector<int> 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 <iostream>
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