summaryrefslogtreecommitdiff
path: root/i/marvin/src/motor
diff options
context:
space:
mode:
authorhaller2005-11-17 11:10:17 +0000
committerhaller2005-11-17 11:10:17 +0000
commitda4a77ff094a0dc74414ec1de2cac08e8b7b60f4 (patch)
tree402c39246c164af44648fd98d2ea85500d58df58 /i/marvin/src/motor
parent164d8870bc058450e6967a594dd9033d088316af (diff)
Importation brut du code récupérable de robert
Diffstat (limited to 'i/marvin/src/motor')
-rw-r--r--i/marvin/src/motor/Makefile.defs10
-rw-r--r--i/marvin/src/motor/motor.cc306
-rw-r--r--i/marvin/src/motor/motor.hh128
-rw-r--r--i/marvin/src/motor/test_motor.cc163
4 files changed, 607 insertions, 0 deletions
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 <cmath>
+
+/// 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<int>("motor.pStatPosition");
+ pStatMotor_ = config.get<int>("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 <queue>
+
+/// 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<ordre> 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 <iostream>
+
+/// Affiche un memo de suntaxe.
+ void
+syntax (void)
+{
+ std::cout << "test_motor - test la classe motor.\n"
+ "Syntaxe : test_motor <...>\n"
+ " s <cmd> <args...> envois une commande\n"
+ " w <ms> 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;
+}