From da4a77ff094a0dc74414ec1de2cac08e8b7b60f4 Mon Sep 17 00:00:00 2001 From: haller Date: Thu, 17 Nov 2005 11:10:17 +0000 Subject: Importation brut du code récupérable de robert --- i/marvin/src/motor/Makefile.defs | 10 ++ i/marvin/src/motor/motor.cc | 306 +++++++++++++++++++++++++++++++++++++++ i/marvin/src/motor/motor.hh | 128 ++++++++++++++++ i/marvin/src/motor/test_motor.cc | 163 +++++++++++++++++++++ 4 files changed, 607 insertions(+) create mode 100644 i/marvin/src/motor/Makefile.defs create mode 100644 i/marvin/src/motor/motor.cc create mode 100644 i/marvin/src/motor/motor.hh create mode 100644 i/marvin/src/motor/test_motor.cc (limited to 'i/marvin/src/motor') diff --git a/i/marvin/src/motor/Makefile.defs b/i/marvin/src/motor/Makefile.defs new file mode 100644 index 0000000..51c658c --- /dev/null +++ b/i/marvin/src/motor/Makefile.defs @@ -0,0 +1,10 @@ +PROGRAMS += test_motor + +motor_OBJECTS = motor.o + +test_motor_OBJECTS = test_motor.o $(motor_OBJECTS) $(timer_OBJECTS) \ + $(config_OBJECTS) $(serial_OBJECTS) \ + $(asserv_OBJECTS) $(proto_OBJECTS) $(log_OBJECTS) \ + $(utils_OBJECTS) + +test_motor: $(test_motor_OBJECTS) diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc new file mode 100644 index 0000000..c292e9c --- /dev/null +++ b/i/marvin/src/motor/motor.cc @@ -0,0 +1,306 @@ +// motor.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include "motor.hh" + +#include + +/// Constructeur +Motor::Motor (const Config & config) + :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"); + pStatMotor_ = config.get("motor.pStatMotor"); + posX_ = 65; + posY_ = -225; + posA_ = 0; + asserv_.setXPos(posX_); + asserv_.setYPos(posY_); + asserv_.setAngle(posA_); +} + +/// Initialise les moteurs +void Motor::init (void) +{ + // on reset la carte + asserv_.reset(); + // on règle le rafraichissement des positions + asserv_.statPosition(pStatPosition_); + asserv_.statMotor(pStatMotor_); + //log_ ("initialisation") << "Etat" << "Terminée"; +} + +// Arrête les moteurs +void Motor::stop(void) +{ + asserv_.setSpeed(); + idle_ = true; +} + +double Motor::getX(void) +{ + return posX_; +} + +double Motor::getY(void) +{ + return posY_; +} + +double Motor::getA(void) +{ + return posA_; +} + +void Motor::setPosition(double x, double y, double a) //XXX Faire trois setteur +{ + asserv_.setXPos(x); + asserv_.setYPos(y); + asserv_.setAngle(a); + posX_ = x; + posY_ = y; + posA_ = a; +} + +void +Motor::setPwm(int leftPwm, int rightPwm) +{ + asserv_.setPwm (leftPwm, rightPwm); +} + +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_)); + //Détermination de l'angle qui va bien + double angle = acos ((x - posX_) / dist); + if((y - posY_) < 0) + angle = -angle; + + // On remplie la file + fOrdre_.push(ordre(true, dist)); + fOrdre_.push(ordre(false, a)); + + // On execute la première commande + idle_ = false; + rotation(angle); +} + +void Motor::recalage(void) +{ + asserv_.fuckTheWall(-3); // XXX Mettre l'arg en conf + //log_ ("recalage") << "Vitesse" << "-3"; + idle_ = false; +} + +bool Motor::idle(void) +{ + return idle_; +} + +void Motor::linearMove(double d) +{ + asserv_.linearMove(d); + //log_ ("linearMove") << "Valeur" << d; + idle_ = false; +} + +void Motor::rotation(double newA) +{ + asserv_.angularMove(newA); + //log_ ("rotation") << "Valeur" << newA; + idle_ = false; +} + +void Motor::setMaxSpeed(int maxLspeed, int maxRspeed) +{ + asserv_.setMaxSpeed(maxLspeed, maxRspeed); +} + +void Motor::setAccel(int accel) +{ + asserv_.setAccel(accel); +} + +int Motor::getMaxLSpeed(void) +{ + return asserv_.getMaxLSpeed(); +} + +bool Motor::sync(void) +{ + //log_("sync") << "Entré dans sync" << ""; + + // On regarde si toutes les commandes ont été envoyées et aquittées + if (asserv_.sync()) + { + //log_("sync") << "Toutes les commandes ont été envoyés et aquitté" << ""; + + if(doneDone_) // Si on a reçu des F depuis le dernier sync + { + //log_("sync") << "On a des F" << ""; + if(!f0Sended_) // Si on avait pas envoyé de f0 + { + //log_("sync") << "On envoie le F0" << ""; + asserv_.finishAck(); + f0Sended_ = true; + } + else // Si l'AVR vient d'aquitter le F0 + { + //log_("sync") << "On a déjà envoyé le F0, on s'en fout" << ""; + doneDone_ = false; + } + } + else // Si on a pas reçu de F + { + //log_("sync") << "On a pas reçu de F" << ""; + if(f0Sended_) // On peut envoyer une nouvelle commande + { + //log_("sync") << "Pret pour un nouveau travail" << ""; + if(fOrdre_.empty()) //Si la pile est vide + { + //log_("sync") << "On repasse en Idle" << ""; + idle_ = true; // Les moteurs sont idle + } + else + { + //log_("sync") << "Prochaine commande de la pile" << ""; + if(fOrdre_.front().ordre_) // Linear Move + linearMove(fOrdre_.front().arg_); + else + rotation(fOrdre_.front().arg_); + fOrdre_.pop(); + } + f0Sended_ = false; + } + // else + //log_("sync") << "Le Robot est en activité/pause, on y touche pas" << ""; + } + return true; + } + return false; +} + +void Motor::wait(int timeout) +{ + asserv_.wait(timeout); +} + +/// Récupère le File Descriptor +int Motor::getFd(void) +{ + return asserv_.getFd(); +} + +bool Motor::jackState(void) +{ + pinUpdated_ = false; +/* asserv_.statInPort(8); /// XXX La période est défini comment?? (en dur, et c'est 8 période d'asserv) + while(!pinUpdated_) + { + sync(); + } + /// On fait fermer sa gueule à l'AVR sur l'epineux sujet du port d'entrée + asserv_.statInPort(); */ + int temp = pinState_ & 0x0040; // XXX Ouais à tester + if(temp == 0x0040) // si le jack est retiré + return true; + else + return false; +} + +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) +{ +} + +void Motor::receivePosX (double xPos) +{ + posX_ = xPos; +} + +void Motor::receivePosY (double yPos) +{ + posY_ = yPos; +} + +void Motor::receivePosA (double aPos) +{ + posA_ = aPos; +} + +void Motor::receiveSpeedStat (int leftError, int leftInt, int rightError, + int rightInt) +{ +} + +void Motor::receivePwm (double leftPwm, double rightPwm) +{ +} + +void Motor::receiveTiming (int motorTimer4, + int motorTimer3, int motorTimer2, + int motorTimer1, int motorTimer0) +{ +} + +void Motor::receiveInPort (int port) +{ + pinState_ = port; + pinUpdated_ = true; +} + +void Motor::receiveSharp (int sharp1, int sharp2, int sharp3) +{ +} + +void Motor::receiveTazState(int state, int subState) +{ +} + +void Motor::done (void) +{ + //log_("done") << "Passage dans done" << ""; + doneDone_ = true; +} diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh new file mode 100644 index 0000000..b527525 --- /dev/null +++ b/i/marvin/src/motor/motor.hh @@ -0,0 +1,128 @@ +#ifndef motor_hh +#define motor_hh +// motor.hh +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#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_; + double posA_; + 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_; + /// Etat du port d'entrée + int pinState_; + bool pinUpdated_; + + public: + /// Constructeur + Motor (const Config & config); + /// Initialise les moteurs + void init(void); + ///Arrête les moteurs + void stop(void); + /// Renvoie la position X + double getX(void); + /// Renvoie la position Y + double getY(void); + /// Renvoie l'angle A + double getA(void); + /// On set les positions dans l'avr + void setPosition(double x, double y, double a); + /// Ammène le robot à la position x,y + void goTo(double x, double y, double a); + /// Recale le robot + void recalage(void); + /// Indique si les moteurs sont occupé ou non + bool idle (void); + /// Execute un déplacement linéaire + void linearMove(double d); + /// Execute une rotation(argument en radian) + void rotation(double newA); + /// set de la vitesse des moteurs + void setMaxSpeed(int maxLspeed, int maxRspeed); + /// set de l'acceleration des moteurs + void setAccel(int accel); + /// récupère la vitesse linéaire maximum + int getMaxLSpeed(void); + /// Syncronisation + bool sync(void); + /// On attend... + void wait(int timeout); + /// Récupère le File Descriptor + int getFd(void); + /// 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); + void receivePosY (double yPos); + void receivePosA (double aPos); + void receiveSpeedStat (int leftError, int leftInt, int rightError, + int rightInt); + void receivePwm (double leftPwm, double rightPwm); + // Défini les valeurs de la PWM + void setPwm(int leftPwm = 0, int rightPwm = 0); + void receiveTiming (int motorTimer4, + int motorTimer3, int motorTimer2, + int motorTimer1, int motorTimer0); + void receiveInPort (int port); + void receiveSharp (int sharp1, int sharp2, int sharp3); + void receiveTazState(int state, int subState); // XXX Vérifier les formats + void done (void); +}; +#endif // motor.hh diff --git a/i/marvin/src/motor/test_motor.cc b/i/marvin/src/motor/test_motor.cc new file mode 100644 index 0000000..137e355 --- /dev/null +++ b/i/marvin/src/motor/test_motor.cc @@ -0,0 +1,163 @@ +// test_motor.cc +// robert - programme du robot 2005 {{{ +// +// Copyright (C) 2005 Nicolas Haller +// +// Robot APB Team/Efrei 2005. +// Web: http://assos.efrei.fr/robot/ +// Email: robot AT efrei DOT fr +// +// This program is free software; you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation; either version 2 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program; if not, write to the Free Software +// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. +// +// }}} + +#include "config/config.hh" +#include "motor/motor.hh" +#include "timer/timer.hh" + +#include + +/// Affiche un memo de suntaxe. + void +syntax (void) +{ + std::cout << "test_motor - test la classe motor.\n" + "Syntaxe : test_motor <...>\n" + " s envois une commande\n" + " w attend pendant un nombre de millisecondes\n" + " ? affiche cet écran d'aide\n" + << std::endl; +} + + int +main (int argc, char **argv) +{ + try + { + int i; + if (argc < 2) + { + syntax (); + return 1; + } + Config config(argc, argv); + Motor motor(config); + i = 1; + while (i < argc) + { + switch (argv[i][0]) + { + case 's': + { + switch(argv[++i][0]) + { + case 'z': + motor.init(); + break; + case 's': + motor.stop(); + break; + case 'P': + std::cout << "position:\n" << + "X: " << motor.getX() << + " Y: " << motor.getY() << + " A: " << motor.getA() << std::endl; + break; + case 'g': + if(++i + 2 > argc) + throw std::runtime_error("syntax error"); + motor.goTo(strtod(argv[i], 0), + strtod(argv[i + 1], 0), + strtod(argv[i + 2], 0)); + i += 2; + break; + case 'r': + motor.recalage(); + break; + case 'I': + std::cout << "Moteur idle? " + << (motor.idle() ? "true" : "false") + << std::endl; + break; + case 'l': + if(++i > argc) + throw std::runtime_error("syntax error"); + motor.linearMove(strtod(argv[i],0)); + break; + case 'a': + if(++i > argc) + throw std::runtime_error("syntax error"); + motor.rotation(strtod(argv[i], 0)); + break; + case 'J': + 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 'A': + if(++i > argc) + 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(); + } + 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) + { + motor.wait (stop - t); + t = Timer::getProgramTime (); + motor.sync(); + } + break; + } + case '?': + syntax (); + return 0; + default: + throw std::runtime_error ("syntax error"); + } + i++; + } + } + catch (const std::exception &e) + { + std::cerr << e.what () << std::endl; + syntax (); + return 1; + } + return 0; +} -- cgit v1.2.3