summaryrefslogtreecommitdiff
path: root/i/marvin/src/ai/ai.cc
diff options
context:
space:
mode:
authorhaller2006-05-25 22:14:43 +0000
committerhaller2006-05-25 22:14:43 +0000
commitbf03b37b06877b55e4f23b627647a6b156ed664e (patch)
tree47aa544812fa1a1eed22666179bd21a7e1ee3807 /i/marvin/src/ai/ai.cc
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
Diffstat (limited to 'i/marvin/src/ai/ai.cc')
-rw-r--r--i/marvin/src/ai/ai.cc102
1 files changed, 99 insertions, 3 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)
+{
+}