From 1c34d302ede3696d9df925195d358185d4504fc5 Mon Sep 17 00:00:00 2001 From: haller Date: Wed, 4 May 2005 02:32:11 +0000 Subject: Les F c'est mal --- 2005/i/robert/src/motor/motor.cc | 75 +++++++++++++++++++++-------------- 2005/i/robert/src/motor/motor.hh | 4 ++ 2005/i/robert/src/motor/test_motor.cc | 3 ++ 3 files changed, 53 insertions(+), 29 deletions(-) (limited to '2005') diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc index f693bc3..5152dc5 100644 --- a/2005/i/robert/src/motor/motor.cc +++ b/2005/i/robert/src/motor/motor.cc @@ -29,7 +29,8 @@ /// Constructeur Motor::Motor (const Config & config) - :asserv_(config, *this), log_("Motor"),idle_(true), pinUpdated_(false) + :asserv_(config, *this), log_("Motor"),idle_(true), doneDone_(false), + f0Sended_(false), pinUpdated_(false) { // XXX Quant est-il des positions de départ du robot??? pStatPosition_ = config.get("motor.pStatPosition"); @@ -54,7 +55,6 @@ void Motor::init (void) void Motor::stop(void) { asserv_.setSpeed(); - while(!idle()){} ///XXX ca pue } double Motor::getX(void) @@ -83,13 +83,12 @@ void Motor::goTo(double x, double y, double a) angle = -angle; // On remplie la file - fOrdre_.push(ordre(false, angle)); fOrdre_.push(ordre(true, dist)); fOrdre_.push(ordre(false, a)); // On execute la première commande idle_ = false; - done (); + rotation(angle); } void Motor::recalage(void) @@ -113,13 +112,52 @@ void Motor::linearMove(double d) void Motor::rotation(double newA) { asserv_.angularMove(newA); - log_ ("rotation") << "Valeur" << newA; + log_ ("rotation") << "Valeur" << newA; idle_ = false; } bool Motor::sync(void) { - return asserv_.sync(); + log_("sync") << "Entré dans sync" << ""; + if(asserv_.sync()) + { + log_("sync") << "Toutes les commandes ont été envoyés et aquitté" << ""; + if(doneDone_) + { + if(!f0Sended_) + { + log_("sync") << "On envoie un F0" << ""; + asserv_.finishAck(); + f0Sended_ = true; + doneDone_ = false; + } + else + { + if(doneDone_) + f0Sended_ = false; + else + { + doneDone_ = f0Sended_ = false; + if (fOrdre_.empty()) + { + idle_ = true; + log_("sync") << "Rien à foutre" << "Idle"; + } + else + { + log_("sync") << "Execution de la prochaine instruction de la pile" << ""; + if(fOrdre_.front().ordre_) // Linear Move + linearMove(fOrdre_.front().arg_); + else + rotation(fOrdre_.front().arg_); + fOrdre_.pop(); + } + } + } + } + return true; + } + return false; } void Motor::wait(int timeout) @@ -183,10 +221,6 @@ void Motor::receivePosA (double aPos) void Motor::receiveSpeedStat (int leftError, int leftInt, int rightError, int rightInt) { - if(leftInt == 0 && rightInt == 0) // XXX c'est bon ca? - idle_ = true; - else - idle_ = false; } void Motor::receivePwm (double leftPwm, double rightPwm) @@ -215,23 +249,6 @@ void Motor::receiveTazState(int state, int subState) void Motor::done (void) { - log_("done") << "On rentre dans done" << ""; - if(!idle_) - { - asserv_.finishAck(); - if(!fOrdre_.empty()) - { - if(fOrdre_.front().ordre_) // Linear Move - linearMove(fOrdre_.front().arg_); - else - rotation(fOrdre_.front().arg_); - fOrdre_.pop(); - } - else - { - idle_ = true; - log_ ("done") << "Action précédente" << "Terminée"; - } - } - log_("done") << "On sort dans done" << ""; + log_("done") << "Passage dans done" << ""; + doneDone_ = true; } diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh index b6d649f..6080850 100644 --- a/2005/i/robert/src/motor/motor.hh +++ b/2005/i/robert/src/motor/motor.hh @@ -55,6 +55,10 @@ class Motor : public Asserv::Receiver double speed_; /// Etat des commandes bool idle_; + /// Drapeau pour quand on a reçu un F + bool doneDone_; + /// Drapeau levé quand on a envoyé un "Ta Gueule" + bool f0Sended_; /// Paramètre de conf int pStatPosition_; int pStatMotor_; diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc index c3f5aa9..e57ff53 100644 --- a/2005/i/robert/src/motor/test_motor.cc +++ b/2005/i/robert/src/motor/test_motor.cc @@ -111,9 +111,11 @@ main (int argc, char **argv) while(!motor.idle()) { motor.wait(-1); + motor.sync(); } break; } + motor.sync(); break; } @@ -128,6 +130,7 @@ main (int argc, char **argv) { motor.wait (stop - t); t = Timer::getProgramTime (); + motor.sync(); } break; } -- cgit v1.2.3