From 713849b389cb89444afab87491b012a845a36cd5 Mon Sep 17 00:00:00 2001 From: schodet Date: Sun, 27 Jun 2004 14:51:05 +0000 Subject: Plein de changements. --- 2004/i/nono/src/qdai/Makefile.defs | 10 +- 2004/i/nono/src/qdai/qdai.cc | 250 ++++++++++++++++++++++++++++++------- 2004/i/nono/src/qdai/qdai.h | 23 +++- 2004/i/nono/src/qdai/test_robot.cc | 79 +++++++++++- 4 files changed, 306 insertions(+), 56 deletions(-) (limited to '2004') 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 #include +/// 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 #include @@ -43,7 +47,10 @@ syntax (void) { std::cout << "test_robot - teste tout le robot.\n" - << motorHelp << ioHelp << servoHelp << + << motorHelp << gpioHelp << servoHelp << analogHelp << + " p \n" + " Mouvement de repositionnement.\n" + " T Démarre la tempo de fin de match.\n" " 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; } -- cgit v1.2.3