summaryrefslogtreecommitdiff
path: root/i/chuck/src/ai
diff options
context:
space:
mode:
authorbecquet2007-05-10 18:49:20 +0000
committerbecquet2007-05-10 18:49:20 +0000
commit8f486613be58ced269db1d437e560c16558604e8 (patch)
tree41e94b2122a118cb06abf6fc2a0038cd1dfbec4a /i/chuck/src/ai
parent4daa2c76c2a028e4b2c8ab379e7d1e0f535a0a31 (diff)
Création de chuck, le programme du robot 2007.
Diffstat (limited to 'i/chuck/src/ai')
-rw-r--r--i/chuck/src/ai/Makefile.defs5
-rw-r--r--i/chuck/src/ai/ai.cc511
-rw-r--r--i/chuck/src/ai/ai.hh86
-rw-r--r--i/chuck/src/ai/test_ai.cc105
4 files changed, 707 insertions, 0 deletions
diff --git a/i/chuck/src/ai/Makefile.defs b/i/chuck/src/ai/Makefile.defs
new file mode 100644
index 0000000..5016f06
--- /dev/null
+++ b/i/chuck/src/ai/Makefile.defs
@@ -0,0 +1,5 @@
+PROGRAMS += test_ai
+
+ai_OBJECTS = ai.o $(motor_OBJECTS) $(es_OBJECTS) $(scheduler_OBJECTS)
+
+test_ai_OBJECTS = test_ai.o $(ai_OBJECTS) $(tester_OBJECTS)
diff --git a/i/chuck/src/ai/ai.cc b/i/chuck/src/ai/ai.cc
new file mode 100644
index 0000000..c9be04a
--- /dev/null
+++ b/i/chuck/src/ai/ai.cc
@@ -0,0 +1,511 @@
+// AI.CC
+// marvin - programme du robot 2006. {{{
+//
+// Copyright (C) 2006 Nicolas Haller
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// Contact :
+// Web: http://perso.efrei.fr/~haller/
+// Email: <nicolas@boiteameuh.org>
+// }}}
+#include "ai.hh"
+#include "config/config.hh"
+#include "timer/timer.hh"
+
+#include <cmath>
+
+/// Fonction callback useless
+static void
+callback (void)
+{}
+
+//constructeur
+Ai::Ai (const Config & config)
+ :es_(config), log_ ("Ai"),
+ schedulableMotor_(callback, motor_.getFd()),
+ schedulableEs_(callback, es_.getFd()),
+ 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_);
+}
+
+Ai::~Ai(void)
+{
+ // On réinitialise
+ // Initialise les moteurs
+ // XXX
+ motor_.reset ();
+ // initialise la carte es
+ es_.reset ();
+ // On sync
+ do
+ {
+ scheduler_.schedule (500, true);
+ }
+ while (!sync ());
+}
+
+// Initialisation du robal
+void
+Ai::init (void)
+{
+ // Initialisation de motor
+ 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)
+{
+ bool motor = motor_.sync ();
+ bool es = es_.sync ();
+ return es && motor;
+}
+
+/// Wait for something to happened
+bool
+Ai::update (void)
+{
+ /// Wait schedule_time
+ scheduler_.schedule (schedule_time_, true);
+ bool retour = sync ();
+ // On vérifie que le match n'est pas fini
+ 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
+ {
+ update ();
+ }
+ while (es_.isJackOut () != out);
+}
+
+/// Init things for a match.
+void
+Ai::prepare (void)
+{
+ es_.lcdPrint("");
+ es_.lcdPrint("Prepa match...");
+ es_.lcdPrint("Inserer Jack");
+ es_.setOthersStat (10);
+ while (!update ());
+ // 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 ();
+ es_.barilletInit();
+ es_.lcdPrint("Barillet pret ?");
+ es_.lcdPrint("Virer le jack !");
+ while (!update ());
+ // We first wait for the jack to be put inside
+ waitJack (true);
+ // Set actual position
+ motor_.getAsserv().setPos(0,0,0);
+ // Ok the match begin ! Go go go !
+ Timer::startRound ();
+ es_.setOthersStat (0);
+ // Shut up fucking beach !
+ es_.setOthersStat (0);
+ es_.setOthersStat (0);
+ while (!update ());
+}
+
+/// Some after after a match
+void
+Ai::afterMatch(void)
+{
+ // On attend de mettre le jack après le match
+ es_.lcdPrint("FIN DU MATCH");
+// es_.lcdPrint("Inserer jack...");
+ while (!update());
+ waitJack(false);
+ // On vide le barillet
+ es_.barilletEmpty();
+ es_.lcdPrint("Vidange barillet");
+ while (!update());
+ waitJack(true);
+ // On init les cartes histoire de
+ es_.reset();
+ motor_.reset();
+ es_.lcdPrint("Power off!!");
+ while (!update());
+}
+
+/// 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)
+{
+ /// On init le robal
+ prepare();
+ es_.barilletDebutLancement();
+ while (!update ());
+ temporisation (1000);
+ es_.barilletLancement();
+ while (!update ());
+ /// on avance un poil parce que le mur c'est mal
+ std::cout << "Move !" << std::endl;
+ motorMove(500, 0);
+ std::cout << "Rotate !" << std::endl;
+ motorRotate(-0.54);
+ std::cout << "Move !" << std::endl;
+ motorMove(1200, 0);
+ std::cout << "Rotate !" << std::endl;
+ motorRotate(1.40);
+ motorMove(500, 0);
+// temporisation (100);
+ std::cout << "Find hole !" << std::endl;
+ motorBasicFindHole ();
+
+ /// On tourne XXX
+/// motorRotate(-M_PI/4);
+ /// On cherche au moins une balle (sinon c'est con)
+/// motorMove(1000, 0); /// XXX Voir la distance
+ /// XXX Avancer d'une certaine facon pour choper des balles
+ /// XXX Ouais ba y'a encore ddes truc à faire là
+ /// On cherche un tru (repositionnement??)
+/// motor_.lockGoodHole();
+/// while (!update ());
+/// motor_.findHole();
+/// while (!update ());
+ /// On trouve un trou, chouette
+ /// Aspirer le trou
+ //es_.extraitBalle();
+ /// mettre une baballe
+ es_.dropWhiteBall();
+ while (!update ());
+ temporisation (3000);
+ es_.deposeBalle ();
+ while (!update ());
+ temporisation (3000);
+ motorMove(300, 0);
+
+ std::cout << "Find hole !" << std::endl;
+ motorBasicFindHole ();
+ es_.dropWhiteBall();
+ while (!update ());
+ temporisation (3000);
+ es_.deposeBalle ();
+ while (!update ());
+ temporisation (3000);
+ motorMove(300, 0);
+ /// Tourner 3 fois en chantant du Mickael Jackson
+ /// Again....
+ /// Fin du match
+ afterMatch();
+}
+
+void
+Ai::progMatch(void)
+{
+ double dummy, aStart, aEnd;
+
+ // Init of cards
+ init ();
+ /// On prépare le robot
+ prepare();
+ /// Faire des crêpes...
+ /// Init du barillet
+ es_.barilletDebutLancement();
+ while (!update ());
+ log_ ("match") << "Totem activator";
+ es_.totemActivator (true);
+ /// En avant guingamp
+ log_ ("match") << "Eloignement du bord";
+ motorMove(480,0);
+ es_.barilletLancement();
+ while (!update ());
+ /// On tourner de -90°
+ log_ ("match") << "Petite rotation de 90°";
+ motorMove(216,-216);
+ /// On dit bonjour au mur
+ log_ ("match") << "Va à l'opposé";
+ if (!motorMove (1000, 0))
+ {
+ log_ ("match") << "Un mur ! Reculons";
+ motorMove (-60, 0);
+ }
+ /// On tourne bizarrement mais de 90°
+ log_ ("match") << "On tourne à 90° en deux fois";
+ motorMove(-36,36);
+ motorMove(180, 180);
+ /// On avance un peu
+ log_ ("match") << "On avance de l'autre coté";
+ motorMove (1420, 0);
+ log_ ("match") << "On recule un peu";
+ motorMove (-150, 0);
+ log_ ("match") << "On tourne";
+ motorMove (216, 216);
+ motorMove (-150, 0);
+
+ /* Here, we are in front of all our holes */
+
+ int rotationSens = 1;
+ const double distanceMax[2] = { -1600., 200. };
+ // Force a turn back !
+ double distanceCur, remainingDist;
+ int ret;
+
+ /* Save angle */
+ motor_.getPosition (dummy, dummy, aStart);
+ do
+ {
+ log_ ("match") << "Start loop for finding holes";
+
+ // Save Y position each time
+ motor_.getPosition (dummy, distanceCur, dummy);
+
+ // Look for an hole !
+ if (rotationSens % 2)
+ remainingDist = std::abs (distanceMax[1] - distanceCur);
+ else
+ remainingDist = std::abs (distanceMax[0] - distanceCur);
+ // Protection again stupid computing
+ if (remainingDist > 0.)
+ ret = motorSmartFindHole (remainingDist);
+ else
+ // Force a turn back !
+ ret = 0;
+ switch (ret)
+ {
+ case 0:
+ // Here we should turn back in the other way
+ log_ ("match") << "Trop long, demi tour !";
+ if (rotationSens % 2)
+ motor_.move (432, -432);
+ else
+ motor_.move (432, 432);
+ rotationSens++;
+ /* Compute the new angle */
+ motor_.getPosition (dummy, dummy, aStart);
+ break;
+ case -2:
+ /// La couleur est unknown
+ log_ ("match") << "Hooooo une couleur de l'espace!!!!";
+ // break;
+ case 2:
+ case 1:
+ {
+ log_ ("match") << "Ok on a une couleur!";
+ bool retDrop;
+ //if (((ret == es_.blueColor_) && es_.isColorModeBlue () )
+ // || ((ret != es_.blueColor_) && !es_.isColorModeBlue ()))
+ if (true)
+ {
+ // If it is our colour, put a white ball,
+ log_ ("match") << "C'est notre couleur -> drop white !";
+ retDrop = es_.dropWhiteBall();
+ while (!update ());
+ if (retDrop)
+ {
+ log_ ("match") << "Tempo & depot!";
+ temporisation (3000);
+ es_.deposeBalle ();
+ temporisation (1000);
+ while (!update ());
+ }
+ }
+ else
+ {
+ // otherwise, put a black one.
+ log_ ("match") << "Pas notre couleur, drop noir!";
+ retDrop = es_.dropBlackBall();
+ while (!update ());
+ if (retDrop)
+ {
+ log_ ("match") << "Tempo & depot!";
+ temporisation (3000);
+ es_.deposeBalle ();
+ temporisation (1000);
+ while (!update ());
+ }
+ }
+ // Put ourself back in the right direction
+ motor_.getPosition (dummy, dummy, aEnd);
+ log_ ("match") << "Remise en place de l'angle : " << aStart << " end " << aEnd;
+ motor_.rotate (aStart - aEnd);
+ }
+ break;
+ default:
+ log_ ("match") << "Hooooo on a default!!!!";
+ break;
+ }
+ } while (true);
+
+ afterMatch();
+}
+
+/// Avance le robal et dit si il a finit (true) ou si il a bloqué
+bool
+Ai::motorMove (int d, int a)
+{
+ motor_.move(d, a); /// XXX Voir la distance
+ do
+ {
+ update ();
+ }
+ while (!motor_.finish ()/* && !motor_.blocked()*/);
+ if(motor_.finish())
+ return true;
+ return false;
+}
+
+void
+Ai::motorRotate (double d)
+{
+ motor_.rotate (d);
+ do
+ {
+ update ();
+ }
+ while (!motor_.finish ());
+}
+
+void
+Ai::motorBasicFindHole (void)
+{
+ motor_.findHole ();
+ do
+ {
+ update ();
+ }
+ while (!motor_.finish ());
+ es_.enableAllSensors (true);
+ es_.setRVBHoleStat (2);
+ temporisation (500);
+ log_ ("FindHole") << "We see" << es_.colorSeen (es_.holeRVB_) ;
+ es_.setRVBHoleStat (0);
+}
+
+/// Renvoie une couleur définie dans es ou -1 si on s'est mangé un mur ou 0 si
+/// distanceOut
+int
+Ai::motorSmartFindHole (double distOut)
+{
+ /// On sauvegarde la position actuelle
+ double bx, x, by, y, ba, a;
+ double dist;
+ motor_.getPosition(bx, by, ba);
+ /// on lance le robal à la recherche d'un tru
+ motor_.findHole();
+ do
+ {
+ update();
+ if (distOut != 0)
+ {
+ /// On regarde si on a pas atteint la distOut
+ motor_.getPosition(x, y, a);
+ /// XXX C'est propre ça?
+ dist = sqrt(((x - bx)*(x - bx)) + ((y - by)*(y - by)));
+ if (dist > distOut)
+ return 0;
+ }
+ /// on regarde si les capteurs voit un trou
+ if(es_.colorSeen(es_.leftFrontRVB_) == es_.redColor_ ||
+ es_.colorSeen(es_.leftFrontRVB_) == es_.blueColor_)
+ {
+ // Si c'est le cas on repositionne le robot
+ motor_.stop();
+ while(!update());
+ // XXX Vérifier l'angle
+ motorRotate(0.21);
+ motor_.findHole();
+ }
+ else if(es_.colorSeen(es_.rightFrontRVB_) == es_.redColor_ ||
+ es_.colorSeen(es_.rightFrontRVB_) == es_.blueColor_)
+ {
+ // Si c'est le cas on repositionne le robot
+ motor_.stop();
+ while(!update());
+ // XXX Vérifier l'angle
+ log_ ("SmartFindHole") << "Rotation spéciale ho un trou sous le capteur";
+ motorRotate(-0.21);
+ motor_.findHole();
+ }
+ }
+ while (!motor_.finish() /*&& !motor_.blocked()*/);
+ /// On regarde si on a blocké
+// if(motor_.blocked())
+// return -1;
+ if(motor_.finish())
+ {
+ es_.enableAllSensors (true);
+ es_.setRVBHoleStat (2);
+ temporisation (500);
+ int ret = es_.colorSeen(es_.holeRVB_);
+ es_.setRVBHoleStat (0);
+ es_.enableAllSensors (false);
+ return ret;
+ }
+ /// XXX Le return qui ne doit jamais arriver normalement...
+ return 0;
+}
+
+/// Procedure to unblock the robal
+void
+Ai::motorDeblock (void)
+{
+}
diff --git a/i/chuck/src/ai/ai.hh b/i/chuck/src/ai/ai.hh
new file mode 100644
index 0000000..29598b4
--- /dev/null
+++ b/i/chuck/src/ai/ai.hh
@@ -0,0 +1,86 @@
+#ifndef ai_hh
+#define ai_hh
+// ai.hh
+// marvin - programme du robot 2006. {{{
+//
+// Copyright (C) 2006 Nicolas Haller
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// Contact :
+// Web: http://perso.efrei.fr/~haller/
+// Email: <nicolas@boiteameuh.org>
+// }}}
+/// Intelligence supérieur(à celui d'une balle de ping-pong) artificielle
+/// (comme une blonde) du robot.
+
+#include "motor/motor.hh"
+#include "es/es.hh"
+#include "scheduler/scheduler.hh"
+#include "scheduler/schedulable_read_fd.hh"
+#include "log/log.hh"
+
+class Ai
+{
+ private:
+ // Modules de contrôles du robot
+ Motor motor_;
+ Es es_;
+ /// Logger
+ Log log_;
+ // Le scheduler
+ scheduler::Scheduler scheduler_;
+ scheduler::SchedulableReadFd schedulableMotor_;
+ scheduler::SchedulableReadFd schedulableEs_;
+ // Paramètres générales du robot
+ const int round_duration_;
+ const int schedule_time_;
+ const int reference_sensor_mask_;
+ bool mustRotate_;
+ public:
+ /// Constructeur
+ Ai(const Config & config);
+ /// Destructeur du robot
+ ~Ai(void);
+
+ /// Initialisation du robal
+ void init(void);
+ /// Arrête complêtement le robot
+ void stop(void);
+ /// Fonction de tempo
+ void temporisation (int msec);
+ /// Attend le jack rentre(false) ou sorte (true)
+ void waitJack (bool out);
+ /// Attend une mise à jour...
+ bool update (void);
+ /// La célèbre fonction sync
+ bool sync (void);
+ /// Init things for a match.
+ void prepare (void);
+ /// Some after after a match
+ void afterMatch(void);
+ /// Reference sensors
+ void referenceSensors (void);
+ /// programme d'homologation du robal
+ void progHomoloRobal(void);
+ /// programme de match du robal
+ void progMatch(void);
+ bool motorMove (int d, int a);
+ void motorRotate (double d);
+ void motorBasicFindHole (void);
+ int motorSmartFindHole (double distOut);
+ void motorDeblock(void);
+};
+#endif // ai_hh
diff --git a/i/chuck/src/ai/test_ai.cc b/i/chuck/src/ai/test_ai.cc
new file mode 100644
index 0000000..6a7d09f
--- /dev/null
+++ b/i/chuck/src/ai/test_ai.cc
@@ -0,0 +1,105 @@
+// test_ai.cc
+// marvin - programme du robot 2006. {{{
+//
+// Copyright (C) 2006 Dufour Jérémy
+//
+// Robot APB Team/Efrei 2004.
+// Web: http://assos.efrei.fr/robot/
+// Email: robot AT efrei DOT fr
+//
+// This program is free software; you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation; either version 2 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program; if not, write to the Free Software
+// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+//
+// }}}
+
+#include "tester/tester.hh"
+#include "ai/ai.hh"
+#include "timer/timer.hh"
+
+class TestAi : public Tester
+{
+ private:
+ Ai ai_;
+ /// Called after each command
+ void postcall (void)
+ {
+ while (!ai_.update ())
+ ;
+ }
+ /// Wait.
+ void wait (int ms)
+ {
+ int t, stop;
+ t = Timer::getProgramTime ();
+ stop = t + ms;
+ while (t < stop)
+ {
+ ai_.temporisation (stop - t);
+ t = Timer::getProgramTime ();
+ }
+ }
+ public:
+ TestAi(int argc, char ** argv)
+ :Tester(argc, argv), ai_(config_)
+ {}
+ void preRun (void)
+ {
+ Interpreter &interpreter = getInterpreter();
+ // Add functions
+ interpreter.add("init", Interpreter::memFunc(ai_, &Ai::init),
+ "Initialise le robal(void)");
+ interpreter.add("prepare", Interpreter::memFunc(ai_, &Ai::prepare),
+ "Lance la procédure pré-Match");
+ interpreter.add("afterMatch", Interpreter::memFunc(ai_,
+ &Ai::afterMatch),
+ "Lance la procédure post match");
+ interpreter.add("stop", Interpreter::memFunc(ai_, &Ai::stop),
+ "Stop le robal(void)");
+ interpreter.add("tempo", Interpreter::memFunc(ai_,
+ &Ai::temporisation),
+ "On temporise(int msec)");
+ interpreter.add("jack", Interpreter::memFunc(ai_, &Ai::waitJack),
+ "On attend que le jack sorte/rentre(bool sorte)");
+ interpreter.add("sensors", Interpreter::memFunc(ai_,
+ &Ai::referenceSensors),
+ "Initialise les capteurs RVB(void)");
+ interpreter.add("homologation", Interpreter::memFunc(ai_,
+ &Ai::progHomoloRobal),
+ "Programme d'homologation du robal");
+ interpreter.add("match", Interpreter::memFunc(ai_, &Ai::progMatch),
+ "Programme de match qui déchire du robal");
+ interpreter.add("findHoleBasic", Interpreter::memFunc(ai_, &Ai::motorBasicFindHole),
+ "Find a hole, basic version");
+ interpreter.add ("_postcall",
+ Interpreter::memFunc (*this, &TestAi::postcall));
+ }
+ void postRun(void)
+ {
+ // On init le robal histoire de ...
+ ai_.init();
+ }
+};
+
+int main (int argc, char **argv)
+{
+ try
+ {
+ TestAi ta(argc, argv);
+ ta.run ();
+ }
+ catch (const std::exception &e)
+ {
+ std::cerr << e.what () << std::endl;
+ }
+}