summaryrefslogtreecommitdiff
path: root/i/marvin/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src/motor/motor.cc')
-rw-r--r--i/marvin/src/motor/motor.cc306
1 files changed, 306 insertions, 0 deletions
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;
+}