summaryrefslogtreecommitdiff
path: root/2004/i
diff options
context:
space:
mode:
authorschodet2004-06-27 14:51:05 +0000
committerschodet2004-06-27 14:51:05 +0000
commit713849b389cb89444afab87491b012a845a36cd5 (patch)
tree507fbc02cf1cd8adcda50746c40afee3a184fe9b /2004/i
parentd4adf20d86ce0e1e887523042d79e22a5930d954 (diff)
Plein de changements.
Diffstat (limited to '2004/i')
-rw-r--r--2004/i/nono/src/qdai/Makefile.defs10
-rw-r--r--2004/i/nono/src/qdai/qdai.cc250
-rw-r--r--2004/i/nono/src/qdai/qdai.h23
-rw-r--r--2004/i/nono/src/qdai/test_robot.cc79
4 files changed, 306 insertions, 56 deletions
diff --git a/2004/i/nono/src/qdai/Makefile.defs b/2004/i/nono/src/qdai/Makefile.defs
index 91ae2a5..48a5301 100644
--- a/2004/i/nono/src/qdai/Makefile.defs
+++ b/2004/i/nono/src/qdai/Makefile.defs
@@ -1,8 +1,10 @@
TARGETS += qdai test_robot
-qdai_SOURCES = qdai.cc config.a date.a erreur.a image.a logger.o motor.o \
- ovision.o serial.o utils.o video4linux.o
-test_robot_SOURCES = test_robot.cc io_cmd.cc motor_cmd.cc date.a \
- io.a logger.a motor.a serial.a utils.a config.a erreur.a
+qdai_SOURCES = qdai.cc date.a io.a image.a logger.a motor.a \
+ movement_reposition.cc ovision.a serial.a utils.a \
+ video4linux.a config.a
+test_robot_SOURCES = test_robot.cc gpio_cmd.cc servo_cmd.cc motor_cmd.cc \
+ movement_reposition.cc analog_cmd.cc \
+ date.a io.a logger.a motor.a serial.a utils.a config.a
qdai: $(qdai_SOURCES:%.cc=%.o)
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 ();
+}
diff --git a/2004/i/nono/src/qdai/qdai.h b/2004/i/nono/src/qdai/qdai.h
index 9384aa5..cc8a334 100644
--- a/2004/i/nono/src/qdai/qdai.h
+++ b/2004/i/nono/src/qdai/qdai.h
@@ -25,14 +25,30 @@
//
// }}}
#include "motor/motor.h"
+#include "io/analog_servo_pp.h"
+#include "io/gpio_asserv.h"
+#include "io/gpio_concat.h"
+#include "io/gpio_servo_pp.h"
+#include "io/gpio_param.h"
#include "video4linux/video4linux.h"
/// Quick & Dirty Ai.
class QdAi
{
Motor motor_;
+ GpioServoPp gspp_;
+ AnalogServoPp ana_;
+ GpioAsserv ga_;
+ GpioConcat gc_;
+ GpioParam gp_;
Video4Linux v4l_;
+ static const int roundDuration_ = 90000;
+ static const int captureInterval_ = 500;
+ int lastCapture_;
+ double front_, bigFront_, width_, rear_;
public:
+ /// Constructeur.
+ QdAi (void);
/// Initialise le robot.
void init (void);
/// Arrète le robot.
@@ -41,8 +57,11 @@ class QdAi
int run (void);
/// Mises à jour périodiques.
void update (void);
- /// Mises à jour jusqu'à fin du mouvement en cours.
- void updateToEnd (void);
+ /// Mises à jour jusqu'à fin du mouvement en cours. Renvois faux à la fin
+ /// du match.
+ bool updateToEnd (void);
+ /// Mises à jour jusqu'à timeout. Renvois faux à la fin du match.
+ bool updateWait (int t);
};
#endif // qdai_h
diff --git a/2004/i/nono/src/qdai/test_robot.cc b/2004/i/nono/src/qdai/test_robot.cc
index f2b1de5..e9041b0 100644
--- a/2004/i/nono/src/qdai/test_robot.cc
+++ b/2004/i/nono/src/qdai/test_robot.cc
@@ -23,15 +23,19 @@
//
// }}}
#include "date/date.h"
+#include "io/analog_servo_pp.h"
+#include "io/analog_cmd.h"
#include "io/gpio_asserv.h"
#include "io/gpio_concat.h"
#include "io/gpio_servo_pp.h"
-#include "io/io_cmd.h"
+#include "io/gpio_cmd.h"
+#include "io/gpio_param.h"
#include "io/servo.h"
#include "io/servo_cmd.h"
#include "motor/asserv.h"
#include "motor/motor_cmd.h"
#include "motor/motor.h"
+#include "motor/movement_reposition.h"
#include <iostream>
#include <unistd.h>
@@ -43,7 +47,10 @@ syntax (void)
{
std::cout <<
"test_robot - teste tout le robot.\n"
- << motorHelp << ioHelp << servoHelp <<
+ << motorHelp << gpioHelp << servoHelp << analogHelp <<
+ " p <s> <gpil> <gpir> <anal> <anar>\n"
+ " Mouvement de repositionnement.\n"
+ " T Démarre la tempo de fin de match.\n"
" <timeout> attend (ms)\n"
" ? cet ecran d'aide" << std::endl;
}
@@ -55,13 +62,16 @@ main (int argc, char **argv)
{
Motor m;
GpioServoPp gspp;
+ AnalogServoPp ana (gspp);
GpioAsserv ga (m.getAsserv ());
GpioConcat gc (ga, gspp, ga.getNbIo ());
+ GpioParam gp (gc);
int i = 1;
while (i < argc)
{
- if (!ioCmd (argc, argv, i, gc)
+ if (!gpioCmd (argc, argv, i, gp)
&& !servoCmd (argc, argv, i, gspp)
+ && !analogCmd (argc, argv, i, ana)
&& !motorCmd (argc, argv, i, m))
{
switch (argv[i][0])
@@ -69,6 +79,59 @@ main (int argc, char **argv)
case '?':
syntax ();
return 0;
+ case 'p':
+ {
+ i++;
+ double s;
+ if (i >= argc) break;
+ s = atof (argv[i++]);
+ int gpil, gpir;
+ if (i >= argc) break;
+ gpil = gp.getByName (argv[i++]);
+ if (i >= argc) break;
+ gpir = gp.getByName (argv[i++]);
+ int anal, anar;
+ if (i >= argc) break;
+ anal = ana.getByName (argv[i++]);
+ if (i >= argc) break;
+ anar = ana.getByName (argv[i++]);
+ std::cout << "test: reposition " << s << ' ' << gpil
+ << ' ' << gpir << ' ' << anal << ' ' << anar <<
+ std::endl;
+ /// HACK
+ Movement *mov = new MovementReposition (s, gp, gpil,
+ gpir, ana,
+ anal, anar);
+ m.addMovement (mov);
+ }
+ break;
+ case 'T':
+ i++;
+ Date::getInstance ().startRound ();
+ break;
+ case 'P':
+ {
+ i++;
+ double a;
+ char c;
+ if (i >= argc) break;
+ c = argv[i++][0];
+ if (i >= argc) break;
+ a = atof (argv[i++]);
+ switch (c)
+ {
+ case 'x':
+ m.getTracker ().adjustX (a);
+ break;
+ case 'y':
+ m.getTracker ().adjustY (a);
+ break;
+ case 'a':
+ m.getTracker ().adjustAngle (a);
+ break;
+ }
+ }
+ break;
default:
int s;
s = atoi (argv[i++]);
@@ -83,13 +146,19 @@ main (int argc, char **argv)
while (Date::getInstance ().start () < start + s)
{
m.ok ();
+ gspp.update ();
Date::wait (50);
}
break;
}
}
- m.waitEnd ();
- gspp.update ();
+ while (!m.end ())
+ {
+ m.waitOk ();
+ gspp.update ();
+ if (Date::getInstance ().round () > 90000)
+ return 0;
+ }
}
return 0;
}