From 2fd79d3f76e6364e3d69ef8ebf3453bf33605987 Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 5 May 2005 11:16:39 +0000 Subject: Codage du programme d'homologuation Mise en place de l'arret en fin de match --- 2005/i/robert/src/ai/ai.cc | 110 ++++++++++++++++++++++++++----------- 2005/i/robert/src/ai/ai.hh | 4 +- 2005/i/robert/src/ai/test_ai.cc | 9 ++- 2005/i/robert/src/asserv/asserv.cc | 58 +++++++++---------- 4 files changed, 117 insertions(+), 64 deletions(-) (limited to '2005/i') diff --git a/2005/i/robert/src/ai/ai.cc b/2005/i/robert/src/ai/ai.cc index 8762475..c184f95 100644 --- a/2005/i/robert/src/ai/ai.cc +++ b/2005/i/robert/src/ai/ai.cc @@ -23,6 +23,8 @@ // // }}} +#include + #include "ai.hh" #include "config/config.hh" #include "timer/timer.hh" @@ -41,10 +43,20 @@ Ai::Ai(const Config & config) roundDuration_(config.get("ai.roundDuration")), vitesseAsc_(config.get("ai.vitesseAsc")) { + //instance = this scheduler_.insert (schedulableMotor_); scheduler_.insert (schedulableEs_); } +Ai::~Ai (void) +{ + init (); + do { + update (); + } + while (motor_.sync ()); +} + /// Initialise le robot void Ai::init(void) { @@ -53,8 +65,9 @@ void Ai::init(void) // initialise la carte es es_.init(); // on init la vision - oVision_.init(motor_.colorState() ? Group::redSkittle : - Group::greenSkittle); /// XXX + //oVision_.init(motor_.colorState() ? Group::redSkittle : +// Group::greenSkittle); /// XXX + update(); } /// stop le robot @@ -67,54 +80,85 @@ void Ai::stop(void) /// Lance le robot void Ai::run(void) { + //Initialise le robot + std::cout << "On init le robot" << std::endl; + init(); // Attend l'entrée du jack "pret à initialiser" + std::cout << "On attend le jack" << std::endl; waitJack(false); + std::cout << "Le jack est mis" << std::endl; // Attend la première sortie du jack(init) waitJack(true); - //Initialise le robot - init(); + std::cout << "Le jack est sorti" << std::endl; + //On fait une rotation pour recaler y + std::cout << "On tourne -> -PI/2" << std::endl; + rotation(-M_PI/2); // On recule dans le mur + std::cout << "On se recale" << std::endl; recale (); + motor_.setPosition(motor_.getX(), -60, -M_PI / 2); // On avance vers la position de départ en y + std::cout << "On avance de 155mm" << std::endl; basic (155); // 215 - 60 = 155 // On rotate de 90 ° + std::cout << "On rotate -> 0" << std::endl; rotation (0); // On se recale vers x + std::cout << "On se recale" << std::endl; recale(); + motor_.setPosition(60, motor_.getY(), 0); + std::cout << "On attend le jack" << std::endl; // Attend le jack "pret à partir" waitJack(false); + std::cout << "Le jack est mis" << std::endl; // Attend la seconde sortie du jack(Daniel) waitJack(true); + std::cout << "Le jack est sortie" << std::endl; // on lance le temps + std::cout << "Lancmeent du match" << std::endl; Timer::startRound(); // On temporise pour laisser Taz sortir - temporisation(500); + std::cout << "Attente du 15 secondes" << std::endl; + temporisation(15000); ///XXX // On avance pour pouvoir aller vers la boule parallèlement au socle + std::cout << "On avance de 540mm" << std::endl; basic(540); // 600 - 60 // Aller devant la boule - goTo(1200, -975, M_PI); + std::cout << "GoTo 870,-675 -> Pi" << std::endl; + goTo(870, -675, M_PI); + // On change l'accélération + motor_.setAccel(32); // Taper la boule - basic(-220); + std::cout << "Recule de 410mm" << std::endl; + basic(-410); + // On restaure la valeur de l'acceleration + motor_.setAccel(64); // se placer devant nos quilles près du pont - basic(220); + std::cout << "Avance de 410mm" << std::endl; + basic(410); // On va derière les quilles - goTo(motor_.getX(),motor_.getY() - 150 ,M_PI); /// Mauvaise valeur, changer valeur - // On set la vitesse à 2 + std::cout << "GoTo" << std::endl; + goTo(motor_.getX(),motor_.getY() - 150 ,M_PI); +/* // On set la vitesse à 2 int MaxLSpeedConf = motor_.getMaxLSpeed(); + std::cout << "Reduction vitesse" << std::endl; motor_.setMaxSpeed(2, -1); // On colle les quilles - basic (-250); // 260 - 10 de marge + std::cout << "Recule de 400mm" << std::endl; + basic (-400); // 260 - 10 de marge // On retaure la vitesse + std::cout << "Acceleration vitesse" << std::endl; motor_.setMaxSpeed(MaxLSpeedConf, -1); - // On temporise.... - temporisation(200); // XXX Mauvaise valeur, changer valeur + // XXX On temporise et on fait mumuse avec l'ascenceur */ + // On a fini notre run + std::cout << "On a fini" << std::endl; } /// Attend. void Ai::wait (int t) { + std::cout << "On attend le putain de Jack" << std::endl; motor_.wait(t); - throw std::runtime_error("nan faut pas utiliser"); } /// Fonction de temporisation @@ -131,8 +175,11 @@ void Ai::temporisation(int t) // Attend le jack entré (false) ou sorti (true). void Ai::waitJack (bool out) { - while (motor_.jackState() != out) + do + { update(); + } + while (motor_.jackState() != out); } /// Rejoint un point. (argument en mm) @@ -155,19 +202,6 @@ void Ai::recale (void) update(); } while(!motor_.idle()); - double angle = motor_.getA(); - if(angle > M_PI / 4 && angle < (3 * M_PI) / 4) // Si on point vers la droite - { - motor_.setPosition(motor_.getX(), -60, -M_PI / 2); - } - else if (angle < M_PI / 4 || angle > (7 * M_PI) / 4) // Si on pointe vers 0 - { - motor_.setPosition(60, motor_.getY(), 0); - } - else // On pointe vers y - { - motor_.setPosition(motor_.getX(), -2040, 0); - } } /// Mouvement basic. @@ -176,7 +210,7 @@ void Ai::basic (double d) motor_.linearMove(d); do { - update(); // XXX Et si on se prend un mur????? + update(); } while (!motor_.idle()); } @@ -184,6 +218,8 @@ void Ai::basic (double d) /// Rotation (argument en radian) void Ai::rotation (double a) { + // On vérifie que ca ne fasse pas moins de trois degré + // XXX motor_.rotation(a); do { @@ -214,11 +250,19 @@ void Ai::ventouses (void) /// XXX Temps bool Ai::update (void) { - scheduler_.schedule(); - motor_.sync(); - es_.sync(); + scheduler_.schedule(500, true); // Gestion des cas critiques /// XXX - + sync (); + // On vérifie que le match n'est pas fini + if (Timer::getRoundTime () > roundDuration_) + throw std::runtime_error ("Fin de match, merci d'avoir participé !"); return true; } + +void +Ai::sync (void) +{ + motor_.sync(); + es_.sync(); +} diff --git a/2005/i/robert/src/ai/ai.hh b/2005/i/robert/src/ai/ai.hh index e1f63ed..4de1e5d 100644 --- a/2005/i/robert/src/ai/ai.hh +++ b/2005/i/robert/src/ai/ai.hh @@ -40,7 +40,7 @@ class Ai // Modules de controle du robot Motor motor_; Es es_; - OVision oVision_; + //OVision oVision_; // Scheduler du robot scheduler::Scheduler scheduler_; scheduler::SchedulableReadFd schedulableMotor_; @@ -52,6 +52,7 @@ class Ai public: /// Constructeur Ai(const Config & config); + ~Ai (void); // Initialise le robot. void init (void); /// Arrète le robot. @@ -78,6 +79,7 @@ class Ai void ventouses (void); /// Attend une mise à jour bool update (void); + void sync (void); }; #endif // ai_hh diff --git a/2005/i/robert/src/ai/test_ai.cc b/2005/i/robert/src/ai/test_ai.cc index c71c09b..0d0aae8 100644 --- a/2005/i/robert/src/ai/test_ai.cc +++ b/2005/i/robert/src/ai/test_ai.cc @@ -76,7 +76,7 @@ main (int argc, char **argv) case 'j': if(++i > argc) throw std::runtime_error("syntax error"); - if(argv[i][0] == 0) + if(argv[i][0] == '0') { std::cout << "Attente de l'entrée de Jack" << std::endl; ai_.waitJack(false); @@ -130,7 +130,13 @@ main (int argc, char **argv) case 'U': ai_.update(); break; + case 'z': + ai_.init(); + break; + default: + throw std::runtime_error ("Error de syntaxe"); } + ai_.update (); break; } case 'w': @@ -144,6 +150,7 @@ main (int argc, char **argv) { ai_.wait (stop - t); t = Timer::getProgramTime (); + ai_.update(); } break; } diff --git a/2005/i/robert/src/asserv/asserv.cc b/2005/i/robert/src/asserv/asserv.cc index 36b7534..41ba763 100644 --- a/2005/i/robert/src/asserv/asserv.cc +++ b/2005/i/robert/src/asserv/asserv.cc @@ -87,7 +87,7 @@ void Asserv::linearMove(double distance) { // Conversion mm->PasW int distPas = mm2pas(distance); - log_ ("linearMove") << "avConv" << distance << "apConv" << distPas; + //log_ ("linearMove") << "avConv" << distance << "apConv" << distPas; // On envoie la commande à l'AVR proto_.send('l', "w", distPas); } @@ -96,7 +96,7 @@ void Asserv::angularMove(double angle) { // Conversion radian->256ème int a = radTo256(angle); - log_ ("angularMove") << "avConv" << angle << "apConv" << a; + //log_ ("angularMove") << "avConv" << angle << "apConv" << a; // Envopie vers avr proto_.send('a',"b", a); } @@ -106,8 +106,8 @@ void Asserv::goToPosition(double xPos, double yPos) // Convertion mm->PasD int x = mm2pas(xPos, true); int y = mm2pas(yPos, true); - log_ ("goToPosition") << "avConvX" << xPos << "apConvX" << x; - log_ ("goToPosition") << "avConvY" << yPos << "apConvY" << y; + //log_ ("goToPosition") << "avConvX" << xPos << "apConvX" << x; + //log_ ("goToPosition") << "avConvY" << yPos << "apConvY" << y; // Envoie vers l'AVR proto_.send('g', "dd", x, y); } @@ -117,14 +117,14 @@ void Asserv::fuckTheWall(double speed) { // Conversion mm/s->Pas/Period int v = mms2ppp(speed); - log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v; + //log_ ("fuckTheWall") << "avConv" << speed << "apConv" << v; // Envoie vers l'AVR fuckTheWall(v); } void Asserv::fuckTheWall(int speed) { - log_ ("fuckTheWall") << "avConv"; + //log_ ("fuckTheWall") << "avConv"; // Envoie vers l'AVR proto_.send('f',"b", speed); } @@ -134,24 +134,24 @@ void Asserv::setSpeed(double xSpeed, double ySpeed) // Conversion mm/s->Pas/Period int vx = mms2ppp(xSpeed); int vy = mms2ppp(ySpeed); - log_ ("setSpeed") << "avConvX" << xSpeed << "apConvX" << vx; - log_ ("setSpeed") << "avConvY" << ySpeed << "apConvY" << vy; + //log_ ("setSpeed") << "avConvX" << xSpeed << "apConvX" << vx; + //log_ ("setSpeed") << "avConvY" << ySpeed << "apConvY" << vy; // Envoie vers l'AVR setSpeed(vx,vy); } void Asserv::setSpeed(int xSpeed, int ySpeed) { - log_ ("setSpeed") << "avConvX" << xSpeed ; - log_ ("setSpeed") << "avConvY" << ySpeed ; + //log_ ("setSpeed") << "avConvX" << xSpeed ; + //log_ ("setSpeed") << "avConvY" << ySpeed ; // Envoie vers l'AVR proto_.send('s',"bb", xSpeed, ySpeed); } void Asserv::setPwm(int xPwm, int yPwm) { - log_ ("setPwm") << "avConvX" << xPwm; - log_ ("setPwm") << "avConvY" << yPwm; + //log_ ("setPwm") << "avConvX" << xPwm; + //log_ ("setPwm") << "avConvY" << yPwm; // Envoie sur l'AVR proto_.send('w',"ww" ,xPwm, yPwm); } @@ -169,64 +169,64 @@ void Asserv::statCounter(int period) pCounter_ = period; // Convertion s->Period - int p = s2period(period); - log_ ("statCounter") << "avConv" << period << "apConv" << p; + //int p = s2period(period); + //log_ ("statCounter") << "avConv" << period << "apConv" << p; // Envoie sur l'AVR - proto_.send('C', "b", p); + proto_.send('C', "b", period); } void Asserv::statPosition(int period) { pPosition_ = period; // Convertion s->Period - int p = s2period(period); + //int p = s2period(period); // Envoie sur l'AVR - proto_.send('X', "b", p); + proto_.send('X', "b", period); } void Asserv::statMotor(int period) { pMotor_ = period; // Convertion s->Period - int p = s2period(period); + //int p = s2period(period); // Envoie sur l'AVR - proto_.send('S', "b", p); + proto_.send('S', "b", period); } void Asserv::statPwm(int period) { pPwm_ = period; // Convertion s->Period - int p = s2period(period); + // int p = s2period(period); // Envoie sur l'AVR - proto_.send('W', "b", p); + proto_.send('W', "b", period); } void Asserv::statTiming(int period) { pTiming_ = period; // Convertion s->Period - int p = s2period(period); + //int p = s2period(period); // Envoie sur l'AVR - proto_.send('T', "b", p); + proto_.send('T', "b", period); } void Asserv::statInPort(int period) { pStatInPort_ = period; // Convertion s->Period - int p = s2period(period); + //int p = s2period(period); // Envoie sur l'AVR - proto_.send('P', "b", p); + proto_.send('P', "b", period); } void Asserv::statSharp(int period) { pSharp_ = period; // Conversion s->period - int p = s2period(period); + //int p = s2period(period); // Envoie sur l'AVR - proto_.send('H', "b", p); + proto_.send('H', "b", period); } /// Change les paramètres @@ -309,7 +309,7 @@ void Asserv::setDSample (int dSample) void Asserv::setAccel(int accel) { accel_ = accel_; - log_ ("setAccel") << "avConv" << accel; + //log_ ("setAccel") << "avConv" << accel; // envoie vers l'AVR proto_.send('p',"bb", 'a', accel); } @@ -453,7 +453,7 @@ void Asserv::loadConfig(const Config & config) pMotor_ = 0; pPwm_ = 0; pTiming_ = 0; - pStatInPort_ = 0; + pStatInPort_ = 32; pSharp_ = 0; } -- cgit v1.2.3