summaryrefslogtreecommitdiff
path: root/i/marvin/src/ai/ai.cc
diff options
context:
space:
mode:
authordufourj2006-05-26 22:02:30 +0000
committerdufourj2006-05-26 22:02:30 +0000
commit526a70ea2f2505d512b6567ebf052ad0b65c2319 (patch)
tree743dc77b9f594d5b096169d38df3847bf9a879f3 /i/marvin/src/ai/ai.cc
parente9a0712596a4d481ba4ecd5f4a25d55da462aaaa (diff)
Asserv :
- activation des stats pour savoir la position ; - correction d'un facteur de 256 ; Es : - correction du bug pour la détection de couleurs du trou. Ai : - amélioration de l'algo.
Diffstat (limited to 'i/marvin/src/ai/ai.cc')
-rw-r--r--i/marvin/src/ai/ai.cc84
1 files changed, 46 insertions, 38 deletions
diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc
index 373fe2a..c9be04a 100644
--- a/i/marvin/src/ai/ai.cc
+++ b/i/marvin/src/ai/ai.cc
@@ -25,6 +25,8 @@
#include "config/config.hh"
#include "timer/timer.hh"
+#include <cmath>
+
/// Fonction callback useless
static void
callback (void)
@@ -260,22 +262,22 @@ void
Ai::progMatch(void)
{
double dummy, aStart, aEnd;
+
+ // Init of cards
init ();
-// bool isFinish;
/// On prépare le robot
prepare();
/// Faire des crêpes...
/// Init du barillet
es_.barilletDebutLancement();
while (!update ());
- temporisation (1000);
- es_.barilletLancement();
- while (!update ());
log_ ("match") << "Totem activator";
es_.totemActivator (true);
/// En avant guingamp
log_ ("match") << "Eloignement du bord";
motorMove(480,0);
+ es_.barilletLancement();
+ while (!update ());
/// On tourner de -90°
log_ ("match") << "Petite rotation de 90°";
motorMove(216,-216);
@@ -286,23 +288,11 @@ Ai::progMatch(void)
log_ ("match") << "Un mur ! Reculons";
motorMove (-60, 0);
}
-// motorMove(200, 0);
/// On tourne bizarrement mais de 90°
log_ ("match") << "On tourne à 90° en deux fois";
motorMove(-36,36);
motorMove(180, 180);
/// On avance un peu
-// log_ ("match") << "On recule un peu";
-// motorMove (-110, 0);
-// // On tourne de 90°
-// log_ ("match") << "On tourne vers le totem";
-// motorMove (216, 216);
-// log_ ("match") << "On avance";
-// motorMove (950, 0);
-// log_ ("match") << "Petite rotation de 90° droite";
-// motorMove (316, -316);
-// motorMove (800, 0);
-// motorBasicFindHole ();
log_ ("match") << "On avance de l'autre coté";
motorMove (1420, 0);
log_ ("match") << "On recule un peu";
@@ -314,33 +304,43 @@ Ai::progMatch(void)
/* Here, we are in front of all our holes */
int rotationSens = 1;
- const double distanceMax[2] = { -1600, -200 };
- double distanceCur;
+ const double distanceMax[2] = { -1600., 200. };
+ // Force a turn back !
+ double distanceCur, remainingDist;
+ int ret;
- motor_.getPosition (dummy, distanceCur, aStart);
+ /* Save angle */
+ motor_.getPosition (dummy, dummy, aStart);
do
{
- // TODO compute remaining value
- // alors une petite blague : Comment les alsacien aime-t-il les oeufs
- // ?
- // réponse : Hopla !!!! (Tb) :-)
- log_ ("match") << "Start loop for find hole";
-
+ log_ ("match") << "Start loop for finding holes";
+ // Save Y position each time
motor_.getPosition (dummy, distanceCur, dummy);
- int ret = motorSmartFindHole (distanceCur - distanceMax[rotationSens % 2]);
+ // Look for an hole !
+ if (rotationSens % 2)
+ remainingDist = std::abs (distanceMax[1] - distanceCur);
+ else
+ remainingDist = std::abs (distanceMax[0] - distanceCur);
+ // Protection again stupid computing
+ if (remainingDist > 0.)
+ ret = motorSmartFindHole (remainingDist);
+ else
+ // Force a turn back !
+ ret = 0;
switch (ret)
{
case 0:
+ // Here we should turn back in the other way
log_ ("match") << "Trop long, demi tour !";
if (rotationSens % 2)
motor_.move (432, -432);
else
motor_.move (432, 432);
rotationSens++;
- // Distance dépassé
- // Here we should turn back in the other way
+ /* Compute the new angle */
+ motor_.getPosition (dummy, dummy, aStart);
break;
case -2:
/// La couleur est unknown
@@ -353,11 +353,10 @@ Ai::progMatch(void)
bool retDrop;
//if (((ret == es_.blueColor_) && es_.isColorModeBlue () )
// || ((ret != es_.blueColor_) && !es_.isColorModeBlue ()))
- if(true)
+ if (true)
{
- log_ ("match") << "C'est notre couleur!";
// If it is our colour, put a white ball,
- log_ ("match") << "Drop blanc!";
+ log_ ("match") << "C'est notre couleur -> drop white !";
retDrop = es_.dropWhiteBall();
while (!update ());
if (retDrop)
@@ -365,12 +364,14 @@ Ai::progMatch(void)
log_ ("match") << "Tempo & depot!";
temporisation (3000);
es_.deposeBalle ();
+ temporisation (1000);
while (!update ());
}
}
else
{
- log_ ("match") << "Drop noir!";
+ // otherwise, put a black one.
+ log_ ("match") << "Pas notre couleur, drop noir!";
retDrop = es_.dropBlackBall();
while (!update ());
if (retDrop)
@@ -378,20 +379,20 @@ Ai::progMatch(void)
log_ ("match") << "Tempo & depot!";
temporisation (3000);
es_.deposeBalle ();
+ temporisation (1000);
while (!update ());
- // otherwise, put a black one.
}
}
+ // Put ourself back in the right direction
+ motor_.getPosition (dummy, dummy, aEnd);
+ log_ ("match") << "Remise en place de l'angle : " << aStart << " end " << aEnd;
+ motor_.rotate (aStart - aEnd);
}
break;
default:
log_ ("match") << "Hooooo on a default!!!!";
break;
}
- motor_.getPosition (dummy, dummy, aEnd);
- log_ ("match") << "Remise en place de l'angle! " << aStart - aEnd;
- motor_.rotate (aStart - aEnd);
- motor_.getPosition (dummy, dummy, aStart);
} while (true);
afterMatch();
@@ -432,6 +433,11 @@ Ai::motorBasicFindHole (void)
update ();
}
while (!motor_.finish ());
+ es_.enableAllSensors (true);
+ es_.setRVBHoleStat (2);
+ temporisation (500);
+ log_ ("FindHole") << "We see" << es_.colorSeen (es_.holeRVB_) ;
+ es_.setRVBHoleStat (0);
}
/// Renvoie une couleur définie dans es ou -1 si on s'est mangé un mur ou 0 si
@@ -487,8 +493,10 @@ Ai::motorSmartFindHole (double distOut)
if(motor_.finish())
{
es_.enableAllSensors (true);
- temporisation (100);
+ es_.setRVBHoleStat (2);
+ temporisation (500);
int ret = es_.colorSeen(es_.holeRVB_);
+ es_.setRVBHoleStat (0);
es_.enableAllSensors (false);
return ret;
}