summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorhaller2005-05-04 02:32:11 +0000
committerhaller2005-05-04 02:32:11 +0000
commit1c34d302ede3696d9df925195d358185d4504fc5 (patch)
tree4cd08a484d86b569c4606d2b0e03d3e5eb34f096
parent626e59a6ab3e8efc91ac4808dd21ad69f4411366 (diff)
Les F c'est mal
-rw-r--r--2005/i/robert/src/motor/motor.cc75
-rw-r--r--2005/i/robert/src/motor/motor.hh4
-rw-r--r--2005/i/robert/src/motor/test_motor.cc3
3 files changed, 53 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;
}
diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh
index b6d649f..6080850 100644
--- a/2005/i/robert/src/motor/motor.hh
+++ b/2005/i/robert/src/motor/motor.hh
@@ -55,6 +55,10 @@ class Motor : public Asserv::Receiver
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_;
diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc
index c3f5aa9..e57ff53 100644
--- a/2005/i/robert/src/motor/test_motor.cc
+++ b/2005/i/robert/src/motor/test_motor.cc
@@ -111,9 +111,11 @@ main (int argc, char **argv)
while(!motor.idle())
{
motor.wait(-1);
+ motor.sync();
}
break;
}
+ motor.sync();
break;
}
@@ -128,6 +130,7 @@ main (int argc, char **argv)
{
motor.wait (stop - t);
t = Timer::getProgramTime ();
+ motor.sync();
}
break;
}