From a8e25e107bfd486b945f656d5358b63259799909 Mon Sep 17 00:00:00 2001 From: dufourj Date: Wed, 11 Jan 2006 16:22:49 +0000 Subject: Correction des syncs --- 2005/i/robert/src/ai/ai.cc | 136 +++++++++++++++++++------------- 2005/i/robert/src/ai/ai.hh | 2 - 2005/i/robert/src/ai/test_ai.cc | 18 +---- 2005/i/robert/src/asserv/test_asserv.cc | 4 +- 2005/i/robert/src/es/es.cc | 20 +++-- 2005/i/robert/src/es/es.hh | 8 +- 2005/i/robert/src/es/test_es.cc | 9 +-- 2005/i/robert/src/motor/motor.cc | 4 +- 2005/i/robert/src/motor/motor.hh | 2 +- 2005/i/robert/src/motor/test_motor.cc | 16 +--- 2005/i/robert/src/proto/proto.cc | 16 ++-- 2005/i/robert/src/proto/proto.hh | 2 +- 12 files changed, 129 insertions(+), 108 deletions(-) (limited to '2005') diff --git a/2005/i/robert/src/ai/ai.cc b/2005/i/robert/src/ai/ai.cc index 25445e8..341f260 100644 --- a/2005/i/robert/src/ai/ai.cc +++ b/2005/i/robert/src/ai/ai.cc @@ -96,12 +96,6 @@ void Ai::stop(void) } while (!motor_.idle ()); } -/// Attend des données. -void Ai::wait (int t) -{ - motor_.wait(t); -} - /// Fonction de temporisation void Ai::temporisation(int t) { @@ -203,7 +197,7 @@ void Ai::ventouses (void) /// Attend une mise à jour bool Ai::update (void) { - scheduler_.schedule(500, true); + scheduler_.schedule(100, true); bool retour = sync (); // On vérifie que le match n'est pas fini if (Timer::getRoundTime () > roundDuration_) @@ -215,12 +209,7 @@ bool Ai::update (void) bool Ai::sync (void) { - bool motor = motor_.sync(); - bool es = es_.sync(); - if (es && motor) - return true; - else - return false; + return es_.sync () && motor_.sync (); } /// Reset la PWM à zéro. @@ -292,12 +281,33 @@ Ai::runProtectFond (void) void Ai::runVisionLine(void) { - initTest(); + initTest (); + int i = 1; + bool sensHorraire = true; do { - scanVision(true); - basic(100); - }while(motor_.getY() < 1700); + // On recherche une quille et on y va si besoin + scanVision (true); + std::cout << "Petite pause d'1 seconde" << std::endl; + temporisation (1000); + // On continue à tourner + rotation (((2. * M_PI) / 8.) * i); + if (sensHorraire) + i++; + else + i--; + if (i == 9) + { + sensHorraire = !sensHorraire; + i -= 2; + } + if (i == -1) + { + sensHorraire = !sensHorraire; + i += 2; + } + } + while (true); } // Pour les Tests. void @@ -317,7 +327,6 @@ Ai::runTest (void) //stop (); // On restaure la vitesse temporisation (400); - init (); std::cout << "On restaure la vitesse à 8" << std::endl; setAccel (96); setMaxSpeed (8, -1); @@ -384,7 +393,7 @@ void Ai::initMatch (void) temporisation(tempoDebutMatch_); }/*}}}*/ -// Parcourt la ligne du fond, en aveugle pour relever des quilles et les poser derrière soit +// Parcourt la ligne du fond, en aveugle pour relever des quilles et les poser derrière soit/*{{{*/ void Ai::parcourtFond (void) { @@ -430,17 +439,27 @@ Ai::parcourtFond (void) } std::cout << "Fini : " << Timer::getRoundTime () << std::endl; } - +/*}}}*/ // *** Fonctions de vision *** // On scan tout autour du robot void Ai::scanVision (bool followLine) /// XXX mettre un argument { + // XXX Cet algo n'est pas bon : c'est trop compliqué de retenir la + // position des quilles. + // Il faut prendre des photos jusquà qu'on trouve une quille. + // La liste doit être purgé à chaque déplacement car elle ne fonctionne + // qu'en position relative, à moins d'arriver à les stoquer en position + // absolue. + // Il serai interessant de stoquer la position (x, y, et orientation) et + // de maintenir cette liste et de virer les quilles que l'on a relever. + // XXX std::cout << "On commence le scannage de quiles" << std::endl; double angleDepart = motor_.getA(); std::vector > skittles; for (int i = 0; i < (followLine ? 1 : 4) ; i++) { + temporisation (1000); std::cout << "On prend la photo" << std::endl; // On prend la photo oVision_.takeShoot(); @@ -471,14 +490,16 @@ void Ai::analyseSkittles(std::vector > skittlesBrut) // On regarde zone par zone for(std::vector::iterator it2 = it->begin(); it2 != it->end(); it2++) { - if (!it2->alone) - continue; - if (it2->vertical) - continue; - if (it2->partial) // XXX to manage - continue; - if (it2->small) - continue; + std::cout << "On regarde si on a une quille qui a les bonnes " + "propriétés" << std::endl; +// if (!it2->alone) +// continue; +// if (it2->vertical) +// continue; +// if (it2->partial) // XXX to manage +// continue; +// if (it2->small) +// continue; std::cout << "On a une quille une gagnante !" << std::endl; goodSkittles_.push_back(*it2); } @@ -489,7 +510,8 @@ void Ai::analyseSkittles(std::vector > skittlesBrut) // On remonte la ze quille la meilleur détecté par la cam void Ai::remonteWithCam(void) { - int x, y , angle, dist; + int x, y , dist; + double angle; std::cout << "remonteWithCam" << std::endl; if (goodSkittles_.size () > 0) { @@ -500,30 +522,38 @@ void Ai::remonteWithCam(void) oVision_.getLoc(zeSkittles, x, y, angle, dist); std::cout << "x : " << x << ", y : " << y << ", a : " << angle << ", d : " << dist << "." << std::endl; - // On enlève la partie avant du robot du au calibrage - dist -= 90; - // On pivote de l'angle demandée - // XXX position relative par rapport à la prise de photo - std::cout << "Rotation" << std::endl; - rotation(motor_.getA() + angle); - std::cout << "Pas Max speed" << std::endl; -// setMaxSpeed(2,1); - std::cout << "basic dist/d" << std::endl; - basic(dist); - std::cout << "Ascenceur +" << std::endl; - ascenceur(true); -// basic (-dist - 5); -// rotation(M_PI); -// rotation(M_PI / 2); -// basic(30); // XXX vérifier valeur - ventouses(); - std::cout << "Max speed" << std::endl; - setMaxSpeed(8, 4); - std::cout << "basic - 30" << std::endl; - basic(-30); // XXX Vérifier valeur - std::cout << "Ascenceur -" << std::endl; - ascenceur(false); -// rotation(3 * M_PI / 4); + if (dist < 600) + { + // On enlève la partie avant du robot du au calibrage + // dist -= 90; + // On pivote de l'angle demandée + // XXX position relative par rapport à la prise de photo + std::cout << "Rotation" << std::endl; + rotation(motor_.getA() + angle); +// std::cout << "Max speed 2, -1" << std::endl; +// setMaxSpeed(3, -1); + std::cout << "basic dist" << std::endl; + basic(dist); + std::cout << "Ascenceur up" << std::endl; + ascenceur(true); + // basic (-dist - 5); + // rotation(M_PI); + // rotation(M_PI / 2); + // basic(30); // XXX vérifier valeur + std::cout << "On temporise de 1500 ms" << std::endl; + temporisation (1500); + std::cout << "On relache les ventouses" << std::endl; + ventouses(); + std::cout << "On temporise de 500 ms" << std::endl; + temporisation (1000); +// std::cout << "Max speed 4, -1" << std::endl; +// setMaxSpeed(4, -1); + std::cout << "On recule de la distance parcouru" << std::endl; + basic(-dist); // XXX Vérifier valeur + std::cout << "Ascenceur down" << std::endl; + ascenceur(false); + } + // XXX Here, we should check if the skittle is up or not } } diff --git a/2005/i/robert/src/ai/ai.hh b/2005/i/robert/src/ai/ai.hh index bf89910..b4a3c8d 100644 --- a/2005/i/robert/src/ai/ai.hh +++ b/2005/i/robert/src/ai/ai.hh @@ -63,8 +63,6 @@ class Ai void init (void); /// Arrète le robot. void stop (void); - /// Attend des données (préférez plutôt temporisation). - void wait (int t); /// Fonction de temporisation void temporisation (int t); // Attend le jack entré (false) ou sorti (true). diff --git a/2005/i/robert/src/ai/test_ai.cc b/2005/i/robert/src/ai/test_ai.cc index 129ed44..c1cae62 100644 --- a/2005/i/robert/src/ai/test_ai.cc +++ b/2005/i/robert/src/ai/test_ai.cc @@ -219,20 +219,10 @@ main (int argc, char **argv) break; } case 'w': - { - int stop, t; - if (i + 1 >= argc) - throw std::runtime_error ("syntax error"); - stop = atoi (argv[++i]) + Timer::getProgramTime (); - t = Timer::getProgramTime (); - while (t < stop) - { - ai_.wait (stop - t); - t = Timer::getProgramTime (); - ai_.update(); - } - break; - } + if (i + 1 >= argc) + throw std::runtime_error ("syntax error"); + ai_.temporisation (atoi (argv[++i])); + break; case '?': syntax (); return 0; diff --git a/2005/i/robert/src/asserv/test_asserv.cc b/2005/i/robert/src/asserv/test_asserv.cc index 228748c..6d4e672 100644 --- a/2005/i/robert/src/asserv/test_asserv.cc +++ b/2005/i/robert/src/asserv/test_asserv.cc @@ -311,7 +311,8 @@ main (int argc, char **argv) break; } } - asserv.sync(); + while (!asserv.wait ()) + ; break; } case 'w': @@ -325,7 +326,6 @@ main (int argc, char **argv) { asserv.wait (stop - t); t = Timer::getProgramTime (); - asserv.sync(); } break; } diff --git a/2005/i/robert/src/es/es.cc b/2005/i/robert/src/es/es.cc index d1e8e76..9c32e9b 100644 --- a/2005/i/robert/src/es/es.cc +++ b/2005/i/robert/src/es/es.cc @@ -66,7 +66,16 @@ bool Es::ascIsIdle(void) bool Es::obsTropPres(void) { - if(distGauche_ > teleSeuil_ || distDroite_ > teleSeuil_) + if (distGauche_ > teleSeuil_ || distDroite_ > teleSeuil_ + || obsBasTropPres ()) + return true; + else + return false; +} +bool +Es::obsBasTropPres (void) +{ + if (distBas_ > teleSeuilBas_) return true; else return false; @@ -89,9 +98,10 @@ void Es::ventouses(void) proto_.send('v',"b",pVentouses_); } -void Es::wait(int timeout) +bool +Es::wait(int timeout /*-1*/) { - proto_.wait(timeout); + return proto_.wait(timeout); } /// Récupère le File Descriptor @@ -114,6 +124,7 @@ void Es::loadConfig(const Config & config) pStatTele_ = config.get("es.pStatTele"); teleSeuil_ = config.get("es.teleSeuil"); + teleSeuilBas_ = config.get ("es.teleSeuilBas"); } void Es::receive(char command, const Proto::Frame & frame) @@ -137,8 +148,7 @@ void Es::receive(char command, const Proto::Frame & frame) ascFailure_ = true; break; case 'C': // retour télémètre - proto_.decode(frame,"ww", distGauche_, distDroite_); -// log_("Télémètre") << "Valeur gauche" << distGauche_ << "Valeur Droite" << distDroite_; + proto_.decode(frame,"www", distGauche_, distDroite_, distBas_); break; } diff --git a/2005/i/robert/src/es/es.hh b/2005/i/robert/src/es/es.hh index bccfcf6..df5aaec 100644 --- a/2005/i/robert/src/es/es.hh +++ b/2005/i/robert/src/es/es.hh @@ -59,6 +59,10 @@ class Es : public Proto::Receiver int distDroite_; /// Seuil du télémètre int teleSeuil_; + /// Distance du télémêtre du bas (quille dans l'ascenceur) + int distBas_; + /// Seuil du télémêtre du bas + int teleSeuilBas_; /// Système de log Log log_; @@ -75,6 +79,8 @@ class Es : public Proto::Receiver bool ascIsIdle(void); /// Renvoie si l'obstacle le plus proche est trop proche bool obsTropPres(void); + /// Renvoie la présence d'un obstacle devant le capteur du bas + bool obsBasTropPres (void); /// Fait monter l'ascenceur void monterAsc(void); /// Fait descendre l'ascenceur @@ -82,7 +88,7 @@ class Es : public Proto::Receiver /// Désactive les ventouses pour X secondes XXX vérifier la durées arg/config void ventouses(void); /// On attend ... - void wait(int timeout); + bool wait(int timeout = -1); /// Récupère le File Descriptor int getFd(void); /// Charge les paramètre du fichier de config diff --git a/2005/i/robert/src/es/test_es.cc b/2005/i/robert/src/es/test_es.cc index 630edc6..7e49762 100644 --- a/2005/i/robert/src/es/test_es.cc +++ b/2005/i/robert/src/es/test_es.cc @@ -93,10 +93,10 @@ main (int argc, char **argv) std::cout << "Le prochain obstacle est " << (es.obsTropPres() ? "TROP PRES." : "LOIN.") << std::endl; break; } - do - { - es.sync(); - }while(!es.ascIsIdle()); + + while (!es.wait (-1) || !es.ascIsIdle()) + ; + break; } case 'w': @@ -110,7 +110,6 @@ main (int argc, char **argv) { es.wait (stop - t); t = Timer::getProgramTime (); - es.sync(); } break; } diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc index c292e9c..7e78aff 100644 --- a/2005/i/robert/src/motor/motor.cc +++ b/2005/i/robert/src/motor/motor.cc @@ -206,9 +206,11 @@ bool Motor::sync(void) return false; } -void Motor::wait(int timeout) +bool +Motor::wait(int timeout) { asserv_.wait(timeout); + return sync (); } /// Récupère le File Descriptor diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh index b527525..ad67f84 100644 --- a/2005/i/robert/src/motor/motor.hh +++ b/2005/i/robert/src/motor/motor.hh @@ -100,7 +100,7 @@ class Motor : public Asserv::Receiver /// Syncronisation bool sync(void); /// On attend... - void wait(int timeout); + bool wait(int timeout = -1); /// Récupère le File Descriptor int getFd(void); /// Retoure l'état du jack (false entrée et true sortie) diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc index 137e355..f01e299 100644 --- a/2005/i/robert/src/motor/test_motor.cc +++ b/2005/i/robert/src/motor/test_motor.cc @@ -112,20 +112,9 @@ main (int argc, char **argv) throw std::runtime_error("syntax error"); motor.setAccel(strtol(argv[0], 0, 10)); break; - case 'L': - while(!motor.idle()) - { - motor.wait(-1); - motor.sync(); - } - break; - } - motor.sync(); - while(!motor.idle()) - { - motor.wait(-1); - motor.sync(); } + while(!motor.wait () || !motor.idle ()) + ; break; } @@ -140,7 +129,6 @@ main (int argc, char **argv) { motor.wait (stop - t); t = Timer::getProgramTime (); - motor.sync(); } break; } diff --git a/2005/i/robert/src/proto/proto.cc b/2005/i/robert/src/proto/proto.cc index a7ada32..3569a47 100644 --- a/2005/i/robert/src/proto/proto.cc +++ b/2005/i/robert/src/proto/proto.cc @@ -56,11 +56,10 @@ Proto::close(void) bool Proto::sync(void) { - bool reGet; // Récupération de la frame - while (reGet = getFrame()) + while (getFrame()) { - //log_ ("recv") << "frame" << currentFrame_; +// log_ ("recv") << "frame" << currentFrame_; // Si la frame est un aquittement if(currentFrame_ == frameQueue_.front()) { @@ -96,7 +95,6 @@ Proto::send (const Frame & frame, bool reliable) } else sendFrame(frame); - // XXX sync(); } @@ -197,7 +195,8 @@ bool Proto::wait (int timeout/*-1*/) { // On apelle serial_.wait avec un timeout toujours inf ou égal à proto::timeout_ - return serial_.wait (timeout < timeout_ ? timeout : timeout_); + serial_.wait (timeout < timeout_ && timeout != -1 ? timeout : timeout_); + return sync (); } /// Récupère le File Descriptor @@ -241,7 +240,6 @@ Proto::getFrame(void) currentFrame_.command = receivedChar; revState_ = 2; } - // On vérifie que le match n'est pas fini break; case 2: d = hex2digit (receivedChar); @@ -270,7 +268,7 @@ Proto::getFrame(void) { // On renvoie en mettant le compteur à 0, la commande sera // renvoyer de retour à sync - //log_("Erreur de reception") << "commande inconnue" << ""; +// log_("Erreur de reception") << "commande inconnue" << ""; tLastSend_ = 0; revState_ = 0; return false; @@ -283,7 +281,7 @@ Proto::getFrame(void) void Proto::sendFrame(const Frame & frame) { - //log_ ("send") << "frame" << frame; +// log_ ("send") << "frame" << frame; // envoyer le bang serial_.putchar('!'); @@ -292,7 +290,7 @@ Proto::sendFrame(const Frame & frame) // Envoyer les arguments for(std::vector::const_iterator it = frame.args.begin(); - it != frame.args.end(); it++) + it != frame.args.end(); ++it) { serial_.putchar(digit2hex(*it >> 4)); serial_.putchar(digit2hex(*it & 0x0f)); diff --git a/2005/i/robert/src/proto/proto.hh b/2005/i/robert/src/proto/proto.hh index 11b1ae3..47ef9a0 100644 --- a/2005/i/robert/src/proto/proto.hh +++ b/2005/i/robert/src/proto/proto.hh @@ -55,7 +55,7 @@ class Proto : public NonCopyable //Date du dernier envoie int tLastSend_; //Temps entre les ré-émission - static const int timeout_ = 500; + static const int timeout_ = 100; /// Frame en cours de réception Frame currentFrame_; -- cgit v1.2.3