summaryrefslogtreecommitdiff
path: root/2004/i/nono/src/qdai/qdai.cc
diff options
context:
space:
mode:
Diffstat (limited to '2004/i/nono/src/qdai/qdai.cc')
-rw-r--r--2004/i/nono/src/qdai/qdai.cc250
1 files changed, 205 insertions, 45 deletions
diff --git a/2004/i/nono/src/qdai/qdai.cc b/2004/i/nono/src/qdai/qdai.cc
index afffd97..684b67e 100644
--- a/2004/i/nono/src/qdai/qdai.cc
+++ b/2004/i/nono/src/qdai/qdai.cc
@@ -24,15 +24,30 @@
// }}}
#include "qdai.h"
#include "motor/movement_types.h"
+#include "motor/movement_reposition.h"
+#include "date/date.h"
+#include <iostream>
#include <cmath>
+/// Constructeur.
+QdAi::QdAi (void)
+ : ana_ (gspp_),
+ ga_ (motor_.getAsserv ()),
+ gc_ (ga_, gspp_, ga_.getNbIo ()),
+ gp_ (gc_),
+ v4l_ ("/dev/video", ImageLoader::yuv),
+ lastCapture_ (0),
+ front_ (135), bigFront_ (182), width_ (260), rear_ (135)
+{
+}
+
/// Initialise le robot.
void
QdAi::init (void)
{
/// Calibre la camera.
- v4l_.calibrate ();
+ //v4l_.calibrate ();
/// Asservi les moteurs.
motor_.go ();
}
@@ -50,63 +65,172 @@ int
QdAi::run (void)
{
int state = 0;
- // Mises à jour.
- update ();
- // Automate.
- switch (state)
+ int oldstate = -1;
+ bool stop = false;
+ int gpioJack = gp_.getByName ("jack");
+ int gpioRoul = gp_.getByName ("roul");
+ int gpioRoulS = gp_.getByName ("rouls");
+ int gpioContactL = gp_.getByName ("contactl");
+ int gpioContactR = gp_.getByName ("contactr");
+ int anaSharpL = ana_.getByName ("ana1");
+ int anaSharpR = ana_.getByName ("ana0");
+ while (!stop && Date::getInstance ().round () < roundDuration_)
{
- case 10:
- // Recherche de balle.
- {
- Movement *m = new MovementRoundCruise (0.1);
- motor_.setMovement (m);
- // Attendre de trouver une balle.
- }
- break;
- case 20:
- // Trackage de balle.
+ // Mises à jour.
+ update ();
+ if (oldstate != state)
{
- Goto *g = new GotoTrack ();
- Movement *m = new MovementGoto (g);
- motor_.setMovement (m);
- updateToEnd ();
+ std::cout << "qdai: " << state << std::endl;
+ oldstate = state;
}
- break;
- case 30:
- // Goto zone d'essai.
+ // Automate.
+ switch (state)
{
- // Goto près de la zone d'essais.
+ case 0:
+ // Initialise le matos.
+ gspp_.moveTo (1, 5);
+ std::cout << '\b' << std::endl;
+ if (gp_.get (gpioJack))
+ state = 1;
+ break;
+ case 1:
+ // Début de match.
+ Date::getInstance ().startRound ();
+ gp_.set (gpioRoul, 1);
+ state = 1000;
+ break;
+// Parcours balle gratuite.
+ case 1000:
+ // Direction première balle.
{
- Goto *g = new GotoSimple (8 * 300, 450);
+ Goto *g = new GotoSimple (900, 1500);
Movement *m = new MovementGoto (g);
- motor_.setMovement (m);
- updateToEnd ();
+ motor_.addMovement (m);
+ if (!updateToEnd ()) break;
}
- // Rotation vers la zone d'essais.
+ state = 1001;
+ break;
+ case 1001:
+ // Direction en-but.
{
- Movement *m = new MovementRotation (0.0);
- motor_.setMovement (m);
- updateToEnd ();
+ Goto *g = new GotoSimple (1200, 1800);
+ Movement *m = new MovementGoto (g);
+ motor_.addMovement (m);
+ if (!updateToEnd ()) break;
}
- // Goto la zone d'essais.
{
- Goto *g = new GotoSimple (8 * 300 + 15, 450);
+ Goto *g = new GotoSimple (2400, 1800);
Movement *m = new MovementGoto (g);
- motor_.setMovement (m);
+ motor_.addMovement (m);
+ if (!updateToEnd ()) break;
+ }
+ state = 1002;
+ break;
+ case 1002:
+ // Levage benne.
+ gspp_.moveTo (1, 57);
+ // Raprochement en-but.
+ {
+ Movement *m = new MovementReposition (0.4, gp_, gpioContactL,
+ gpioContactR, ana_,
+ anaSharpL, anaSharpR);
+ motor_.addMovement (m);
+ if (!updateToEnd ()) break;
+ }
+ // Recalage du tracker.
+ motor_.getTracker ().adjustX (9 * 300 - front_);
+ motor_.getTracker ().adjustAngle (0);
+ state = 1003;
+ break;
+ case 1003:
+ // Décharge.
+ gp_.reset (gpioRoul);
+ if (!updateWait (200)) break;
+ gp_.set (gpioRoulS, 1);
+ if (!updateWait (200)) break;
+ gp_.set (gpioRoul, 1);
+ if (!updateWait (3000)) break;
+ state = 1004;
+ break;
+ case 1005:
+ // Recule et retourne.
+ {
+ Movement *m = new MovementBasic (-1.0, 100);
+ motor_.addMovement (m);
+ if (!updateToEnd ()) break;
+ }
+ {
+ Movement *m = new MovementRotation (-2/3 * M_PI);
+ motor_.addMovement (m);
+ if (!updateToEnd ()) break;
+ }
+ stop = true;
+ break;
+// Parcours découverte.
+ case 2010:
+ // Recherche de balle.
+ {
+ Movement *m = new MovementRoundCruise (0.1);
+ motor_.addMovement (m);
+ }
+ // Attendre de trouver une balle.
+ state = 2020;
+ break;
+ case 2020:
+ // Trackage de balle.
+ {
+ }
+ state = 2021;
+ break;
+ case 2021:
+ // Gobage de la balle.
+ {
+ Movement *m = new MovementBasic (0.7, 300);
+ motor_.addMovement (m);
updateToEnd ();
}
+ state = 2030;
+ break;
+ case 2030:
+ // Goto zone d'essai.
+ {
+ // Goto près de la zone d'essais.
+ {
+ Goto *g = new GotoSimple (8 * 300, 450);
+ Movement *m = new MovementGoto (g);
+ motor_.addMovement (m);
+ updateToEnd ();
+ }
+ // Rotation vers la zone d'essais.
+ {
+ Movement *m = new MovementRotation (0.0);
+ motor_.addMovement (m);
+ updateToEnd ();
+ }
+ // Goto la zone d'essais.
+ {
+ Goto *g = new GotoSimple (8 * 300 + 15, 450);
+ Movement *m = new MovementGoto (g);
+ motor_.addMovement (m);
+ updateToEnd ();
+ }
+ }
+ state = 2040;
+ break;
+ case 2040:
+ // Essai.
+ {
+ stop = true;
+ // Lever la benne.
+ // Sortir les balles.
+ // Baisser la benne.
+ }
+ break;
+ default:
+ std::cout << "qdai: unknown state" << std::endl;
}
- state = 40;
- break;
- case 40:
- // Essai.
- {
- // Lever la benne.
- // Sortir les balles.
- // Baisser la benne.
- }
- break;
}
+ return 0;
}
/// Mises à jour périodiques.
@@ -115,14 +239,50 @@ QdAi::update (void)
{
// Mise à jour moteur.
motor_.waitOk ();
+ // Mise à jour capteurs.
+ gspp_.update ();
// Mise à jour camera.
+ if (lastCapture_ + captureInterval_ < Date::getInstance ().start ())
+ {
+ }
}
-/// Mises à jour jusqu'à fin du mouvement en cours.
-void
+/// Mises à jour jusqu'à fin du mouvement en cours. Renvois faux à la fin
+/// du match.
+bool
QdAi::updateToEnd (void)
{
while (!motor_.end ())
+ {
+ if (Date::getInstance ().round () >= roundDuration_)
+ return false;
update ();
+ Date::wait (50);
+ }
+ return true;
+}
+
+/// Mises à jour jusqu'à timeout. Renvois faux à la fin du match.
+bool
+QdAi::updateWait (int t)
+{
+ int start;
+ start = Date::getInstance ().start ();
+ while (Date::getInstance ().start () < start + t)
+ {
+ if (Date::getInstance ().round () >= roundDuration_)
+ return false;
+ update ();
+ Date::wait (50);
+ }
+ return true;
}
+int
+main (int argc, char **argv)
+{
+ QdAi qdai_;
+ qdai_.init ();
+ qdai_.run ();
+ qdai_.stop ();
+}