From 8c8042f7a644eabae5fe20135d64c7258b73e8d0 Mon Sep 17 00:00:00 2001 From: haller Date: Tue, 3 May 2005 23:30:57 +0000 Subject: Correction de bug Implémentation d'une file pour goTo --- 2005/i/robert/src/motor/motor.cc | 68 +++++++++++++++++++++++++---------- 2005/i/robert/src/motor/motor.hh | 14 ++++++++ 2005/i/robert/src/motor/test_motor.cc | 19 +++++++--- 3 files changed, 79 insertions(+), 22 deletions(-) (limited to '2005/i/robert') diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc index b743d1d..f693bc3 100644 --- a/2005/i/robert/src/motor/motor.cc +++ b/2005/i/robert/src/motor/motor.cc @@ -34,6 +34,9 @@ Motor::Motor (const Config & config) // XXX Quant est-il des positions de départ du robot??? pStatPosition_ = config.get("motor.pStatPosition"); pStatMotor_ = config.get("motor.pStatMotor"); + posX_ = 65; + posY_ = -225; + posA_ = 0; } /// Initialise les moteurs @@ -41,13 +44,9 @@ void Motor::init (void) { // on reset la carte asserv_.reset(); - // On stop le moteur - stop(); // on règle le rafraichissement des positions asserv_.statPosition(pStatPosition_); asserv_.statMotor(pStatMotor_); - // on remet les position à 0 XXX à 0? dns ai en dur - posX_ = posY_ = posA_ = 0; log_ ("initialisation") << "Etat" << "Terminée"; } @@ -77,20 +76,20 @@ void Motor::goTo(double x, double y, double a) { //Détermination de la distance à parcourir - double dist = sqrt((x - posX_) * (x- posX_) + (y - posY_) * (y - posY_)); // XXX A vérifier + double dist = sqrt((x - posX_) * (x- posX_) + (y - posY_) * (y - posY_)); //Détermination de l'angle qui va bien double angle = acos ((x - posX_) / dist); + if((y - posY_) < 0) + angle = -angle; - //On tourne - rotation(angle); // XXX est-ce que ca attend quelque chose avant de passer à la commande suivant ou bien... - log_ ("goTo") << "Etat" << "Rotation" << "angle" << angle; - // XXX while (!idle) { sync();} - //On avance - linearMove(dist); - log_("goTo") << "Etat" << "linearMove" << "distance" << dist; - // rotation vers l'angle a - rotation(a); - log_("goTo") << "Etat" << "Dernière rotation" << "angle" << a; + // 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 (); } void Motor::recalage(void) @@ -145,6 +144,23 @@ bool Motor::jackState(void) asserv_.statInPort(); } +bool Motor::colorState (void) +{ + pinUpdated_ = false; + asserv_.statInPort(1); /// XXX La période est défini comment?? (en dur, et c'est 8 période d'asserv) + while(!pinUpdated_) + { + sync(); + } + int temp = pinState_ & 0x0002; // XXX Ouais à tester + if(temp == 0x0002) // si le jack est retiré + return true; + else + return false; + /// On fait fermer sa gueule à l'AVR sur l'epineux sujet du port d'entrée + asserv_.statInPort(); +} + void Motor::receiveCounter (double lMotor, double rMotor) { } @@ -199,7 +215,23 @@ void Motor::receiveTazState(int state, int subState) void Motor::done (void) { - idle_ = true; - asserv_.finishAck(); - log_ ("done") << "Action précédente" << "Terminée"; + 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" << ""; } diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh index 1ab092a..b6d649f 100644 --- a/2005/i/robert/src/motor/motor.hh +++ b/2005/i/robert/src/motor/motor.hh @@ -28,14 +28,26 @@ #include "config/config.hh" #include "asserv/asserv.hh" +#include + /// Gère les moteurs de déplacement du robot class Motor : public Asserv::Receiver { + struct ordre + { + // Ordre (true pour linéaire, false pour rotation + bool ordre_; + double arg_; + // Constructeur + ordre(bool ordre, double arg) { ordre_ = ordre; arg_ = arg; } + }; private: /// Communication avec l'asservissement Asserv asserv_; /// Système de log Log log_; + /// Pile de commande + std::queue fOrdre_; /// position double posX_; double posY_; @@ -79,6 +91,8 @@ class Motor : public Asserv::Receiver void wait(int timeout); /// Retoure l'état du jack (false entrée et true sortie) bool jackState(void); + /// Retourne la couleur selectionné + bool colorState (void); /// déclaration des fonctions de receiver void receiveCounter (double lMotor, double rMotor); void receivePosX (double xPos); diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc index 9ccd4b8..c3f5aa9 100644 --- a/2005/i/robert/src/motor/test_motor.cc +++ b/2005/i/robert/src/motor/test_motor.cc @@ -72,8 +72,8 @@ main (int argc, char **argv) case 'P': std::cout << "position:\n" << "X: " << motor.getX() << - "Y: " << motor.getY() << - "A: " << motor.getA() << std::endl; + " Y: " << motor.getY() << + " A: " << motor.getA() << std::endl; break; case 'g': if(++i + 2 > argc) @@ -88,7 +88,9 @@ main (int argc, char **argv) break; case 'I': std::cout << "Moteur idle? " - << motor.idle() << std::endl; + << (motor.idle() ? "true" : "false") + << std::endl; + break; case 'l': if(++i > argc) throw std::runtime_error("syntax error"); @@ -100,7 +102,16 @@ main (int argc, char **argv) motor.rotation(strtod(argv[i], 0)); break; case 'J': - std::cout << "Etat du Jack: " << (motor.jackState() ? "Retiré" : "Dedans"); + std::cout << "Etat du Jack: " << (motor.jackState() ? "Retiré" : "Dedans") << std::endl; + break; + case 'C': + std::cout << "Couleur Sélectionné: " << (motor.colorState() ? "Rouge" : "Vert") << std::endl; + break; + case 'L': + while(!motor.idle()) + { + motor.wait(-1); + } break; } break; -- cgit v1.2.3