summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorhaller2005-05-03 23:30:57 +0000
committerhaller2005-05-03 23:30:57 +0000
commit8c8042f7a644eabae5fe20135d64c7258b73e8d0 (patch)
tree7f348cd1a5e0bc52d96c1ed17ccd4054c041040b
parentccf18730f3ecfe2cce1fdc02b949b757b42dcb16 (diff)
Correction de bug
Implémentation d'une file pour goTo
-rw-r--r--2005/i/robert/src/motor/motor.cc68
-rw-r--r--2005/i/robert/src/motor/motor.hh14
-rw-r--r--2005/i/robert/src/motor/test_motor.cc19
3 files changed, 79 insertions, 22 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" << "";
}
diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh
index 1ab092a..b6d649f 100644
--- a/2005/i/robert/src/motor/motor.hh
+++ b/2005/i/robert/src/motor/motor.hh
@@ -28,14 +28,26 @@
#include "config/config.hh"
#include "asserv/asserv.hh"
+#include <queue>
+
/// Gère les moteurs de déplacement du robot
class Motor : public Asserv::Receiver
{
+ struct ordre
+ {
+ // Ordre (true pour linéaire, false pour rotation
+ bool ordre_;
+ double arg_;
+ // Constructeur
+ ordre(bool ordre, double arg) { ordre_ = ordre; arg_ = arg; }
+ };
private:
/// Communication avec l'asservissement
Asserv asserv_;
/// Système de log
Log log_;
+ /// Pile de commande
+ std::queue<ordre> fOrdre_;
/// position
double posX_;
double posY_;
@@ -79,6 +91,8 @@ class Motor : public Asserv::Receiver
void wait(int timeout);
/// Retoure l'état du jack (false entrée et true sortie)
bool jackState(void);
+ /// Retourne la couleur selectionné
+ bool colorState (void);
/// déclaration des fonctions de receiver
void receiveCounter (double lMotor, double rMotor);
void receivePosX (double xPos);
diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc
index 9ccd4b8..c3f5aa9 100644
--- a/2005/i/robert/src/motor/test_motor.cc
+++ b/2005/i/robert/src/motor/test_motor.cc
@@ -72,8 +72,8 @@ main (int argc, char **argv)
case 'P':
std::cout << "position:\n" <<
"X: " << motor.getX() <<
- "Y: " << motor.getY() <<
- "A: " << motor.getA() << std::endl;
+ " Y: " << motor.getY() <<
+ " A: " << motor.getA() << std::endl;
break;
case 'g':
if(++i + 2 > argc)
@@ -88,7 +88,9 @@ main (int argc, char **argv)
break;
case 'I':
std::cout << "Moteur idle? "
- << motor.idle() << std::endl;
+ << (motor.idle() ? "true" : "false")
+ << std::endl;
+ break;
case 'l':
if(++i > argc)
throw std::runtime_error("syntax error");
@@ -100,7 +102,16 @@ main (int argc, char **argv)
motor.rotation(strtod(argv[i], 0));
break;
case 'J':
- std::cout << "Etat du Jack: " << (motor.jackState() ? "Retiré" : "Dedans");
+ std::cout << "Etat du Jack: " << (motor.jackState() ? "Retiré" : "Dedans") << std::endl;
+ break;
+ case 'C':
+ std::cout << "Couleur Sélectionné: " << (motor.colorState() ? "Rouge" : "Vert") << std::endl;
+ break;
+ case 'L':
+ while(!motor.idle())
+ {
+ motor.wait(-1);
+ }
break;
}
break;