summaryrefslogtreecommitdiff
path: root/2005/i/robert/src/motor/motor.cc
diff options
context:
space:
mode:
Diffstat (limited to '2005/i/robert/src/motor/motor.cc')
-rw-r--r--2005/i/robert/src/motor/motor.cc75
1 files changed, 46 insertions, 29 deletions
diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc
index f693bc3..5152dc5 100644
--- a/2005/i/robert/src/motor/motor.cc
+++ b/2005/i/robert/src/motor/motor.cc
@@ -29,7 +29,8 @@
/// Constructeur
Motor::Motor (const Config & config)
- :asserv_(config, *this), log_("Motor"),idle_(true), pinUpdated_(false)
+ :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");
@@ -54,7 +55,6 @@ void Motor::init (void)
void Motor::stop(void)
{
asserv_.setSpeed();
- while(!idle()){} ///XXX ca pue
}
double Motor::getX(void)
@@ -83,13 +83,12 @@ void Motor::goTo(double x, double y, double a)
angle = -angle;
// 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 ();
+ rotation(angle);
}
void Motor::recalage(void)
@@ -113,13 +112,52 @@ void Motor::linearMove(double d)
void Motor::rotation(double newA)
{
asserv_.angularMove(newA);
- log_ ("rotation") << "Valeur" << newA;
+ log_ ("rotation") << "Valeur" << newA;
idle_ = false;
}
bool Motor::sync(void)
{
- return asserv_.sync();
+ log_("sync") << "Entré dans sync" << "";
+ if(asserv_.sync())
+ {
+ log_("sync") << "Toutes les commandes ont été envoyés et aquitté" << "";
+ if(doneDone_)
+ {
+ if(!f0Sended_)
+ {
+ log_("sync") << "On envoie un F0" << "";
+ asserv_.finishAck();
+ f0Sended_ = true;
+ doneDone_ = false;
+ }
+ else
+ {
+ if(doneDone_)
+ f0Sended_ = false;
+ else
+ {
+ doneDone_ = f0Sended_ = false;
+ if (fOrdre_.empty())
+ {
+ idle_ = true;
+ log_("sync") << "Rien à foutre" << "Idle";
+ }
+ else
+ {
+ log_("sync") << "Execution de la prochaine instruction de la pile" << "";
+ if(fOrdre_.front().ordre_) // Linear Move
+ linearMove(fOrdre_.front().arg_);
+ else
+ rotation(fOrdre_.front().arg_);
+ fOrdre_.pop();
+ }
+ }
+ }
+ }
+ return true;
+ }
+ return false;
}
void Motor::wait(int timeout)
@@ -183,10 +221,6 @@ void Motor::receivePosA (double aPos)
void Motor::receiveSpeedStat (int leftError, int leftInt, int rightError,
int rightInt)
{
- if(leftInt == 0 && rightInt == 0) // XXX c'est bon ca?
- idle_ = true;
- else
- idle_ = false;
}
void Motor::receivePwm (double leftPwm, double rightPwm)
@@ -215,23 +249,6 @@ void Motor::receiveTazState(int state, int subState)
void Motor::done (void)
{
- 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" << "";
+ log_("done") << "Passage dans done" << "";
+ doneDone_ = true;
}