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.cc68
1 files changed, 50 insertions, 18 deletions
diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc
index b743d1d..f693bc3 100644
--- a/2005/i/robert/src/motor/motor.cc
+++ b/2005/i/robert/src/motor/motor.cc
@@ -34,6 +34,9 @@ Motor::Motor (const Config & config)
// 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;
}
/// Initialise les moteurs
@@ -41,13 +44,9 @@ void Motor::init (void)
{
// on reset la carte
asserv_.reset();
- // On stop le moteur
- stop();
// on règle le rafraichissement des positions
asserv_.statPosition(pStatPosition_);
asserv_.statMotor(pStatMotor_);
- // on remet les position à 0 XXX à 0? dns ai en dur
- posX_ = posY_ = posA_ = 0;
log_ ("initialisation") << "Etat" << "Terminée";
}
@@ -77,20 +76,20 @@ 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_)); // XXX A vérifier
+ 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 tourne
- rotation(angle); // XXX est-ce que ca attend quelque chose avant de passer à la commande suivant ou bien...
- log_ ("goTo") << "Etat" << "Rotation" << "angle" << angle;
- // XXX while (!idle) { sync();}
- //On avance
- linearMove(dist);
- log_("goTo") << "Etat" << "linearMove" << "distance" << dist;
- // rotation vers l'angle a
- rotation(a);
- log_("goTo") << "Etat" << "Dernière rotation" << "angle" << a;
+ // 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 ();
}
void Motor::recalage(void)
@@ -145,6 +144,23 @@ bool Motor::jackState(void)
asserv_.statInPort();
}
+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)
{
}
@@ -199,7 +215,23 @@ void Motor::receiveTazState(int state, int subState)
void Motor::done (void)
{
- idle_ = true;
- asserv_.finishAck();
- log_ ("done") << "Action précédente" << "Terminée";
+ 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" << "";
}