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.cc302
1 files changed, 58 insertions, 244 deletions
diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc
index e9c0937..af3d29f 100644
--- a/i/marvin/src/motor/motor.cc
+++ b/i/marvin/src/motor/motor.cc
@@ -1,7 +1,7 @@
// motor.cc
-// robert - programme du robot 2005 {{{
+// marvin - programme du robot 2006. {{{
//
-// Copyright (C) 2005 Nicolas Haller
+// Copyright (C) 2006 Nicolas Schodet
//
// Robot APB Team/Efrei 2005.
// Web: http://assos.efrei.fr/robot/
@@ -22,290 +22,104 @@
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//
// }}}
-
#include "motor.hh"
-#include "config/config.hh"
-
-#include <cmath>
-/// Constructeur
+/// Constructor.
Motor::Motor (void)
- : asserv_(*this), log_("Motor"),idle_(true), doneDone_(false),
- f0Sended_(false), pinUpdated_(false)
-{
- // Get Config
- Config &config = Config::getInstance ();
- // 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_ (*this), seq_ (0), finish_ (true), x_ (0.0), y_ (0.0), a_ (0.0)
{
- asserv_.linearMove(d);
- //log_ ("linearMove") << "Valeur" << d;
- idle_ = false;
}
-void Motor::rotation(double newA)
+/// Linear and angular move. T(mm) is the linear distance. A(mm) is the
+/// angular distance which means the arc length.
+void
+Motor::move (double t, double a)
{
- asserv_.angularMove(newA);
- //log_ ("rotation") << "Valeur" << newA;
- idle_ = false;
+ nextSeq ();
+ finish_ = false;
+ asserv_.speedTo (t, a, seq_);
}
-void Motor::setMaxSpeed(int maxLspeed, int maxRspeed)
+/// Rotate (rad).
+void
+Motor::rotate (double a)
{
- asserv_.setMaxSpeed(maxLspeed, maxRspeed);
+ nextSeq ();
+ finish_ = false;
+ asserv_.speedAngle (a, seq_);
}
-void Motor::setAccel(int accel)
+/// Find a hole.
+void
+Motor::findHole (void)
{
- 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;
+ nextSeq ();
+ finish_ = false;
+ asserv_.findHole (seq_);
}
-bool
-Motor::wait(int timeout)
+/// Stop now.
+void
+Motor::stop (void)
{
- asserv_.wait(timeout);
- return sync ();
+ finish_ = true;
+ asserv_.speed (0, 0);
}
-/// Récupère le File Descriptor
-int Motor::getFd(void)
+/// Next seq number.
+void
+Motor::nextSeq (void)
{
- return asserv_.getFd();
+ seq_++;
+ if (seq_ > 250)
+ seq_ = 1;
}
-bool Motor::jackState(void)
+/// Implement Asserv::Receiver callbacks.
+void
+Motor::receiveAck (int seq)
{
- 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;
+ if (seq != seq_)
+ std::cout << "spurious ack received" << std::endl;
+ asserv_.ack (seq);
+ finish_ = true;
}
-bool Motor::colorState (void)
+void
+Motor::receiveCounterStat (int l, int r)
{
- 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::receivePos (double x, double y, double a)
{
+ x_ = x;
+ y_ = y;
+ a_ = a;
}
-void Motor::receivePosX (double xPos)
+void
+Motor::receiveSpeedStat (int t, int a)
{
- posX_ = xPos;
}
-void Motor::receivePosY (double yPos)
+void
+Motor::receivePosStat (int te, int ti, int ae, int ai)
{
- posY_ = yPos;
}
-void Motor::receivePosA (double aPos)
+void
+Motor::receivePwmStat (int l, int r)
{
- posA_ = aPos;
}
-void Motor::receiveSpeedStat (int leftError, int leftInt, int rightError,
- int rightInt)
+void
+Motor::receiveTimerStat (const int *t, int tn)
{
}
-void Motor::receivePwm (double leftPwm, double rightPwm)
+void
+Motor::receiveInPort (unsigned int port)
{
}
-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;
-}