summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorhaller2006-05-25 22:14:43 +0000
committerhaller2006-05-25 22:14:43 +0000
commitbf03b37b06877b55e4f23b627647a6b156ed664e (patch)
tree47aa544812fa1a1eed22666179bd21a7e1ee3807
parent1aff85cbaa8174f4aef9ded81c2156c8f1c5a508 (diff)
* Constification de getFd dans la classe Serial et ses clients
* Fonction afterMatch dans ai * Fonction motorSmartFindHole * Déclaration de la fonction motorDeblock * Ajout d'info sur le lcd lors de l'init * Utilisation des capteurs barillet
-rw-r--r--i/marvin/src/ai/ai.cc102
-rw-r--r--i/marvin/src/ai/ai.hh8
-rw-r--r--i/marvin/src/ai/test_ai.cc5
-rw-r--r--i/marvin/src/asserv/asserv.cc2
-rw-r--r--i/marvin/src/asserv/asserv.hh2
-rw-r--r--i/marvin/src/es/es.cc10
-rw-r--r--i/marvin/src/motor/motor.cc11
-rw-r--r--i/marvin/src/motor/motor.hh4
-rw-r--r--i/marvin/src/proto/proto.cc2
-rw-r--r--i/marvin/src/proto/proto.hh2
-rw-r--r--i/marvin/src/serial/serial.hh2
11 files changed, 134 insertions, 16 deletions
diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc
index 53f191e..0d24afa 100644
--- a/i/marvin/src/ai/ai.cc
+++ b/i/marvin/src/ai/ai.cc
@@ -132,6 +132,10 @@ Ai::waitJack (bool out)
void
Ai::prepare (void)
{
+ es_.lcdPrint("");
+ es_.lcdPrint("");
+ es_.lcdPrint("Prepa match...");
+ es_.lcdPrint("Inserer Jack");
es_.setOthersStat (10);
while (!update ());
// XXX We should check if the jack is not already in
@@ -139,7 +143,11 @@ Ai::prepare (void)
waitJack (false);
// We reference all the color
referenceSensors ();
+ // Set actual position
+ motor_.getAsserv().setPos(0,0,0);
es_.barilletInit();
+ es_.lcdPrint("Virer le jack...");
+ es_.lcdPrint("PRET POUR MATCH");
while (!update ());
// We first wait for the jack to be put inside
waitJack (true);
@@ -152,6 +160,27 @@ Ai::prepare (void)
while (!update ());
}
+/// Some after after a match
+void
+Ai::afterMatch(void)
+{
+ // On attend de mettre le jack après le match
+ es_.lcdPrint("FIN DU MATCH");
+ es_.lcdPrint("Inserer jack...");
+ while (!update());
+ waitJack(false);
+ // On vide le barillet
+ es_.barilletEmpty();
+ es_.lcdPrint("Vidange barillet");
+ while (!update());
+ waitJack(true);
+ // On init les cartes histoire de
+ es_.reset();
+ motor_.reset();
+ es_.lcdPrint("Power off!!");
+ while (!update());
+}
+
/// Reference sensors
void
Ai::referenceSensors (void)
@@ -188,7 +217,7 @@ void Ai::progHomoloRobal(void)
motorMove(500, 0);
// temporisation (100);
std::cout << "Find hole !" << std::endl;
- motorFindHole ();
+ motorBasicFindHole ();
/// On tourne XXX
/// motorRotate(-M_PI/4);
@@ -214,7 +243,7 @@ void Ai::progHomoloRobal(void)
motorMove(300, 0);
std::cout << "Find hole !" << std::endl;
- motorFindHole ();
+ motorBasicFindHole ();
es_.dropWhiteBall();
while (!update ());
temporisation (3000);
@@ -225,6 +254,15 @@ void Ai::progHomoloRobal(void)
/// Tourner 3 fois en chantant du Mickael Jackson
/// Again....
/// Fin du match
+ afterMatch();
+}
+
+void
+Ai::progMatch(void)
+{
+ /// On prépare le robot
+ prepare();
+ /// Faire des crêpes...
}
void
@@ -250,7 +288,7 @@ Ai::motorRotate (double d)
}
void
-Ai::motorFindHole (void)
+Ai::motorBasicFindHole (void)
{
motor_.findHole ();
do
@@ -259,3 +297,61 @@ Ai::motorFindHole (void)
}
while (!motor_.finish ());
}
+
+/// Renvoie une couleur définie dans es ou -1 si on s'est mangé un mur ou 0 si
+/// distanceOut
+int
+Ai::motorSmartFindHole (double distOut)
+{
+ /// On sauvegarde la position actuelle
+ double bx, x, by, y, ba, a;
+ double dist;
+ motor_.getPosition(bx, by, ba);
+ /// on lance le robal à la recherche d'un tru
+ motor_.findHole();
+ do
+ {
+ update();
+ /// On regarde si on a pas atteint la distOut
+ motor_.getPosition(x, y, a);
+ /// XXX C'est propre ça?
+ dist = sqrt(((x - bx)*(x - bx)) + ((y - by)*(y - by)));
+ if (dist > distOut)
+ return 0;
+ /// on regarde si les capteurs voit un trou
+ if(es_.colorSeen(es_.leftFrontRVB_) == es_.redColor_ ||
+ es_.colorSeen(es_.leftFrontRVB_) == es_.blueColor_)
+ {
+ // Si c'est le cas on repositionne le robot
+ motor_.stop();
+ while(!update());
+ // XXX Vérifier l'angle
+ motorRotate(0.21);
+ motor_.findHole();
+ }
+ else if(es_.colorSeen(es_.rightFrontRVB_) == es_.redColor_ ||
+ es_.colorSeen(es_.rightFrontRVB_) == es_.blueColor_)
+ {
+ // Si c'est le cas on repositionne le robot
+ motor_.stop();
+ while(!update());
+ // XXX Vérifier l'angle
+ motorRotate(-0.21);
+ motor_.findHole();
+ }
+ }
+ while (!motor_.finish() || motor_.blocked());
+ /// On regarde si on a blocké
+ if(motor_.blocked())
+ return -1;
+ if(motor_.finish())
+ return es_.colorSeen(es_.holeRVB_);
+ /// XXX Le return qui ne doit jamais arriver normalement...
+ return 0;
+}
+
+/// Procedure to unblock the robal
+void
+Ai::motorDeblock (void)
+{
+}
diff --git a/i/marvin/src/ai/ai.hh b/i/marvin/src/ai/ai.hh
index f0143ec..b7b0833 100644
--- a/i/marvin/src/ai/ai.hh
+++ b/i/marvin/src/ai/ai.hh
@@ -68,12 +68,18 @@ class Ai
bool sync (void);
/// Init things for a match.
void prepare (void);
+ /// Some after after a match
+ void afterMatch(void);
/// Reference sensors
void referenceSensors (void);
/// programme d'homologation du robal
void progHomoloRobal(void);
+ /// programme de match du robal
+ void progMatch(void);
void motorMove (int d, int a);
void motorRotate (double d);
- void motorFindHole (void);
+ void motorBasicFindHole (void);
+ int motorSmartFindHole (double distOut);
+ void motorDeblock(void);
};
#endif // ai_hh
diff --git a/i/marvin/src/ai/test_ai.cc b/i/marvin/src/ai/test_ai.cc
index 27d32a1..dd51b7d 100644
--- a/i/marvin/src/ai/test_ai.cc
+++ b/i/marvin/src/ai/test_ai.cc
@@ -61,6 +61,9 @@ class TestAi : public Tester
"Initialise le robal(void)");
interpreter.add("prepare", Interpreter::memFunc(ai_, &Ai::prepare),
"Lance la procédure pré-Match");
+ interpreter.add("afterMatch", Interpreter::memFunc(ai_,
+ &Ai::afterMatch),
+ "Lance la procédure post match");
interpreter.add("stop", Interpreter::memFunc(ai_, &Ai::stop),
"Stop le robal(void)");
interpreter.add("tempo", Interpreter::memFunc(ai_,
@@ -74,6 +77,8 @@ class TestAi : public Tester
interpreter.add("homologation", Interpreter::memFunc(ai_,
&Ai::progHomoloRobal),
"Programme d'homologation du robal");
+ interpreter.add("match", Interpreter::memFunc(ai_, &Ai::progMatch),
+ "Programme de match qui déchire du robal");
interpreter.add ("_postcall",
Interpreter::memFunc (*this, &TestAi::postcall));
}
diff --git a/i/marvin/src/asserv/asserv.cc b/i/marvin/src/asserv/asserv.cc
index 5726dbf..9298c3c 100644
--- a/i/marvin/src/asserv/asserv.cc
+++ b/i/marvin/src/asserv/asserv.cc
@@ -77,7 +77,7 @@ Asserv::wait (int timeout/*-1*/)
/// Récupère le File Descriptor
int
-Asserv::getFd (void)
+Asserv::getFd (void) const
{
return proto_.getFd ();
}
diff --git a/i/marvin/src/asserv/asserv.hh b/i/marvin/src/asserv/asserv.hh
index 0d9f5a2..ddbc104 100644
--- a/i/marvin/src/asserv/asserv.hh
+++ b/i/marvin/src/asserv/asserv.hh
@@ -69,7 +69,7 @@ class Asserv : public Proto::Receiver
/// Attend que toute les émissions soit terminées.
bool wait (int timeout = -1);
/// Récupère le File Descriptor
- int getFd (void);
+ int getFd (void) const;
/// Reset the board and send parameters.
void reset (void);
/// Set PWM.
diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc
index 86bbb3b..8c4c6fb 100644
--- a/i/marvin/src/es/es.cc
+++ b/i/marvin/src/es/es.cc
@@ -617,27 +617,27 @@ Es::newBallFront(void)
switch (positionBarillet_)
{
case avant0:
- stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:white;
+ stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:black;
positionBarillet_ = avant4;
log_ ("Es::Barillet", Log::debug) << "trou 0:" << ((stockBarillet[0] == white)?"white":"pas white");
break;
case avant1:
- stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:white;
+ stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black;
positionBarillet_ = avant0;
log_ ("Es::Barillet", Log::debug) << "trou 1:" << ((stockBarillet[1] == white)?"white":"pas white");
break;
case avant2:
- stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:white;
+ stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black;
positionBarillet_ = avant1;
log_ ("Es::Barillet", Log::debug) << "trou 2:" << ((stockBarillet[2] == white)?"white":"pas white");
break;
case avant3:
- stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:white;
+ stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black;
positionBarillet_ = avant2;
log_ ("Es::Barillet", Log::debug) << "trou 3:" << ((stockBarillet[3] == white)?"white":"pas white");
break;
case avant4:
- stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:white;
+ stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black;
positionBarillet_ = avant3;
log_ ("Es::Barillet", Log::debug) << "trou 4:" << ((stockBarillet[4] == white)?"white":"pas white");
break;
diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc
index d2a7250..d3fadfc 100644
--- a/i/marvin/src/motor/motor.cc
+++ b/i/marvin/src/motor/motor.cc
@@ -102,11 +102,20 @@ Motor::stop (void)
/// get the file descriptor
int
-Motor::getFd(void)
+Motor::getFd(void) const
{
return asserv_.getFd();
}
+/// get the position of robal
+void
+Motor::getPosition(double & x, double & y, double & a) const
+{
+ x = x_;
+ y = y_;
+ a = a_;
+}
+
/// Next seq number.
void
Motor::nextSeq (void)
diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh
index 68da820..f1c9843 100644
--- a/i/marvin/src/motor/motor.hh
+++ b/i/marvin/src/motor/motor.hh
@@ -62,9 +62,11 @@ class Motor : public Asserv::Receiver
/// Stop now.
void stop (void);
/// get the file descriptor
- int getFd(void);
+ int getFd(void) const;
/// Reset.
void reset (void);
+ /// get the position of robal
+ void getPosition(double & x, double & y, double & a) const;
private:
/// Next seq number.
inline void nextSeq (void);
diff --git a/i/marvin/src/proto/proto.cc b/i/marvin/src/proto/proto.cc
index 216a345..50e363d 100644
--- a/i/marvin/src/proto/proto.cc
+++ b/i/marvin/src/proto/proto.cc
@@ -221,7 +221,7 @@ Proto::wait (int timeout/*-1*/)
}
/// Récupère le File Descriptor
-int Proto::getFd(void)
+int Proto::getFd(void) const
{
return serial_.getFd();
}
diff --git a/i/marvin/src/proto/proto.hh b/i/marvin/src/proto/proto.hh
index 8979a3d..340b1fb 100644
--- a/i/marvin/src/proto/proto.hh
+++ b/i/marvin/src/proto/proto.hh
@@ -108,7 +108,7 @@ class Proto : public NonCopyable
/// fonction bloquante, n'utiliser que pour les tests.
bool wait (int timeout = -1);
/// Récupère le File Descriptor
- int getFd(void);
+ int getFd(void) const;
public:
/// Les clients de Proto doivent dériver de Receiver.
diff --git a/i/marvin/src/serial/serial.hh b/i/marvin/src/serial/serial.hh
index 23feb42..d66971e 100644
--- a/i/marvin/src/serial/serial.hh
+++ b/i/marvin/src/serial/serial.hh
@@ -58,7 +58,7 @@ class Serial
/// millisecondes.
bool wait (int timeout = -1) { return sb_->wait (timeout); }
/// Récupère le descripteur.
- int getFd (void) { return sb_->getFd (); }
+ int getFd (void) const { return sb_->getFd (); }
};
#endif // serial_hh