From bf03b37b06877b55e4f23b627647a6b156ed664e Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 25 May 2006 22:14:43 +0000 Subject: * Constification de getFd dans la classe Serial et ses clients * Fonction afterMatch dans ai * Fonction motorSmartFindHole * Déclaration de la fonction motorDeblock * Ajout d'info sur le lcd lors de l'init * Utilisation des capteurs barillet --- i/marvin/src/ai/ai.cc | 102 ++++++++++++++++++++++++++++++++++++++++-- i/marvin/src/ai/ai.hh | 8 +++- i/marvin/src/ai/test_ai.cc | 5 +++ i/marvin/src/asserv/asserv.cc | 2 +- i/marvin/src/asserv/asserv.hh | 2 +- i/marvin/src/es/es.cc | 10 ++--- i/marvin/src/motor/motor.cc | 11 ++++- i/marvin/src/motor/motor.hh | 4 +- i/marvin/src/proto/proto.cc | 2 +- i/marvin/src/proto/proto.hh | 2 +- i/marvin/src/serial/serial.hh | 2 +- 11 files changed, 134 insertions(+), 16 deletions(-) (limited to 'i/marvin') diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc index 53f191e..0d24afa 100644 --- a/i/marvin/src/ai/ai.cc +++ b/i/marvin/src/ai/ai.cc @@ -132,6 +132,10 @@ Ai::waitJack (bool out) void Ai::prepare (void) { + es_.lcdPrint(""); + 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 @@ -139,7 +143,11 @@ Ai::prepare (void) waitJack (false); // We reference all the color referenceSensors (); + // Set actual position + motor_.getAsserv().setPos(0,0,0); es_.barilletInit(); + es_.lcdPrint("Virer le jack..."); + es_.lcdPrint("PRET POUR MATCH"); while (!update ()); // We first wait for the jack to be put inside waitJack (true); @@ -152,6 +160,27 @@ Ai::prepare (void) 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) @@ -188,7 +217,7 @@ void Ai::progHomoloRobal(void) motorMove(500, 0); // temporisation (100); std::cout << "Find hole !" << std::endl; - motorFindHole (); + motorBasicFindHole (); /// On tourne XXX /// motorRotate(-M_PI/4); @@ -214,7 +243,7 @@ void Ai::progHomoloRobal(void) motorMove(300, 0); std::cout << "Find hole !" << std::endl; - motorFindHole (); + motorBasicFindHole (); es_.dropWhiteBall(); while (!update ()); temporisation (3000); @@ -225,6 +254,15 @@ void Ai::progHomoloRobal(void) /// Tourner 3 fois en chantant du Mickael Jackson /// Again.... /// Fin du match + afterMatch(); +} + +void +Ai::progMatch(void) +{ + /// On prépare le robot + prepare(); + /// Faire des crêpes... } void @@ -250,7 +288,7 @@ Ai::motorRotate (double d) } void -Ai::motorFindHole (void) +Ai::motorBasicFindHole (void) { motor_.findHole (); do @@ -259,3 +297,61 @@ Ai::motorFindHole (void) } while (!motor_.finish ()); } + +/// 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(); + /// 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 + motorRotate(-0.21); + motor_.findHole(); + } + } + while (!motor_.finish() || motor_.blocked()); + /// On regarde si on a blocké + if(motor_.blocked()) + return -1; + if(motor_.finish()) + return es_.colorSeen(es_.holeRVB_); + /// XXX Le return qui ne doit jamais arriver normalement... + return 0; +} + +/// Procedure to unblock the robal +void +Ai::motorDeblock (void) +{ +} diff --git a/i/marvin/src/ai/ai.hh b/i/marvin/src/ai/ai.hh index f0143ec..b7b0833 100644 --- a/i/marvin/src/ai/ai.hh +++ b/i/marvin/src/ai/ai.hh @@ -68,12 +68,18 @@ class Ai 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); void motorMove (int d, int a); void motorRotate (double d); - void motorFindHole (void); + void motorBasicFindHole (void); + int motorSmartFindHole (double distOut); + void motorDeblock(void); }; #endif // ai_hh diff --git a/i/marvin/src/ai/test_ai.cc b/i/marvin/src/ai/test_ai.cc index 27d32a1..dd51b7d 100644 --- a/i/marvin/src/ai/test_ai.cc +++ b/i/marvin/src/ai/test_ai.cc @@ -61,6 +61,9 @@ class TestAi : public Tester "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_, @@ -74,6 +77,8 @@ class TestAi : public Tester 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 ("_postcall", Interpreter::memFunc (*this, &TestAi::postcall)); } diff --git a/i/marvin/src/asserv/asserv.cc b/i/marvin/src/asserv/asserv.cc index 5726dbf..9298c3c 100644 --- a/i/marvin/src/asserv/asserv.cc +++ b/i/marvin/src/asserv/asserv.cc @@ -77,7 +77,7 @@ Asserv::wait (int timeout/*-1*/) /// Récupère le File Descriptor int -Asserv::getFd (void) +Asserv::getFd (void) const { return proto_.getFd (); } diff --git a/i/marvin/src/asserv/asserv.hh b/i/marvin/src/asserv/asserv.hh index 0d9f5a2..ddbc104 100644 --- a/i/marvin/src/asserv/asserv.hh +++ b/i/marvin/src/asserv/asserv.hh @@ -69,7 +69,7 @@ class Asserv : public Proto::Receiver /// Attend que toute les émissions soit terminées. bool wait (int timeout = -1); /// Récupère le File Descriptor - int getFd (void); + int getFd (void) const; /// Reset the board and send parameters. void reset (void); /// Set PWM. diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 86bbb3b..8c4c6fb 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -617,27 +617,27 @@ Es::newBallFront(void) switch (positionBarillet_) { case avant0: - stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:white; + stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = avant4; log_ ("Es::Barillet", Log::debug) << "trou 0:" << ((stockBarillet[0] == white)?"white":"pas white"); break; case avant1: - stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:white; + stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = avant0; log_ ("Es::Barillet", Log::debug) << "trou 1:" << ((stockBarillet[1] == white)?"white":"pas white"); break; case avant2: - stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:white; + stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = avant1; log_ ("Es::Barillet", Log::debug) << "trou 2:" << ((stockBarillet[2] == white)?"white":"pas white"); break; case avant3: - stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:white; + stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = avant2; log_ ("Es::Barillet", Log::debug) << "trou 3:" << ((stockBarillet[3] == white)?"white":"pas white"); break; case avant4: - stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:white; + stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black; positionBarillet_ = avant3; log_ ("Es::Barillet", Log::debug) << "trou 4:" << ((stockBarillet[4] == white)?"white":"pas white"); break; diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc index d2a7250..d3fadfc 100644 --- a/i/marvin/src/motor/motor.cc +++ b/i/marvin/src/motor/motor.cc @@ -102,11 +102,20 @@ Motor::stop (void) /// get the file descriptor int -Motor::getFd(void) +Motor::getFd(void) const { return asserv_.getFd(); } +/// get the position of robal +void +Motor::getPosition(double & x, double & y, double & a) const +{ + x = x_; + y = y_; + a = a_; +} + /// Next seq number. void Motor::nextSeq (void) diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh index 68da820..f1c9843 100644 --- a/i/marvin/src/motor/motor.hh +++ b/i/marvin/src/motor/motor.hh @@ -62,9 +62,11 @@ class Motor : public Asserv::Receiver /// Stop now. void stop (void); /// get the file descriptor - int getFd(void); + int getFd(void) const; /// Reset. void reset (void); + /// get the position of robal + void getPosition(double & x, double & y, double & a) const; private: /// Next seq number. inline void nextSeq (void); diff --git a/i/marvin/src/proto/proto.cc b/i/marvin/src/proto/proto.cc index 216a345..50e363d 100644 --- a/i/marvin/src/proto/proto.cc +++ b/i/marvin/src/proto/proto.cc @@ -221,7 +221,7 @@ Proto::wait (int timeout/*-1*/) } /// Récupère le File Descriptor -int Proto::getFd(void) +int Proto::getFd(void) const { return serial_.getFd(); } diff --git a/i/marvin/src/proto/proto.hh b/i/marvin/src/proto/proto.hh index 8979a3d..340b1fb 100644 --- a/i/marvin/src/proto/proto.hh +++ b/i/marvin/src/proto/proto.hh @@ -108,7 +108,7 @@ class Proto : public NonCopyable /// fonction bloquante, n'utiliser que pour les tests. bool wait (int timeout = -1); /// Récupère le File Descriptor - int getFd(void); + int getFd(void) const; public: /// Les clients de Proto doivent dériver de Receiver. diff --git a/i/marvin/src/serial/serial.hh b/i/marvin/src/serial/serial.hh index 23feb42..d66971e 100644 --- a/i/marvin/src/serial/serial.hh +++ b/i/marvin/src/serial/serial.hh @@ -58,7 +58,7 @@ class Serial /// millisecondes. bool wait (int timeout = -1) { return sb_->wait (timeout); } /// Récupère le descripteur. - int getFd (void) { return sb_->getFd (); } + int getFd (void) const { return sb_->getFd (); } }; #endif // serial_hh -- cgit v1.2.3