From 526a70ea2f2505d512b6567ebf052ad0b65c2319 Mon Sep 17 00:00:00 2001 From: dufourj Date: Fri, 26 May 2006 22:02:30 +0000 Subject: 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. --- i/marvin/runtime/rc/config | 6 ++-- i/marvin/src/ai/ai.cc | 84 +++++++++++++++++++++++-------------------- i/marvin/src/asserv/asserv.cc | 2 +- i/marvin/src/es/es.cc | 16 +++++++-- i/marvin/src/es/es.hh | 2 ++ n/es-2006/src/main.c | 3 +- 6 files changed, 68 insertions(+), 45 deletions(-) diff --git a/i/marvin/runtime/rc/config b/i/marvin/runtime/rc/config index 9b977a8..d7eb03f 100644 --- a/i/marvin/runtime/rc/config +++ b/i/marvin/runtime/rc/config @@ -19,7 +19,7 @@ asserv.l_invert_pwm = true asserv.r_invert_pwm = false asserv.step_per_mm = 75.78789091394133410940 # Stats. -asserv.pos_report = 0 +asserv.pos_report = 10 asserv.in_port_report = 0 asserv.counter_stat = 0 asserv.speed_stat = 0 @@ -40,8 +40,8 @@ es.rvb_sensors_max_ov = 250 # Ratio for the algorithm that choose if the color is green or not es.rvb_sniff_ref_ratio = 3 # Limits for the algorithm of choosing between a black or white ball -es.rvb_sniff_green_limit = 3700 -es.rvb_sniff_clear_limit = 4000 +es.rvb_sniff_green_limit = 4300 +es.rvb_sniff_clear_limit = 4300 # Stats of RVB Sensor raw data es.rvb_sensor_mask_stat = 0xFFFF es.rvb_sensor_stat = 0 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 + /// 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; } diff --git a/i/marvin/src/asserv/asserv.cc b/i/marvin/src/asserv/asserv.cc index 9298c3c..48c2ad5 100644 --- a/i/marvin/src/asserv/asserv.cc +++ b/i/marvin/src/asserv/asserv.cc @@ -324,7 +324,7 @@ Asserv::receive (char command, const Proto::Frame &frame) break; case 'X': if (proto_.decode (frame, "DDD", x, y, a)) - receiver_.receivePos (stepToMm (x), stepToMm (y), avrToRad (a)); + receiver_.receivePos (stepToMm (x / 256), stepToMm (y / 256), avrToRad (a)); break; case 'S': if (proto_.decode (frame, "BB", t, a)) diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 03fa74b..66a8fc9 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -316,6 +316,7 @@ Es::deposeBalle(void) barilletDebutLancement(); log_ ("Es::Barillet", Log::debug) << "DROP!! "; frontTurbineIsFull_ = false; + wait (400); barilletLancement(); } @@ -497,6 +498,7 @@ void Es::receive(char command, const Proto::Frame & frame) case 4: // A ball has been by the sensor at the front of the barillet enableAllSensors (true); + setRVBBallStat (5); break; } shutUp (); @@ -539,10 +541,13 @@ void Es::receive(char command, const Proto::Frame & frame) case 'B': if (proto_.decode (frame, "bb", stat1 /* Rear */, stat2 /* Front */)) { - //log_ ("Stats RVB Ball", Log::debug) << "[" << decodeColor - // (stat1) << "] [" << decodeColor (stat2) << "]"; +// log_ ("Stats RVB Ball", Log::debug) << "[" << decodeColor +// (stat1) << "] [" << decodeColor (stat2) << "]"; updateAnalysisSensor (stat1, rearBallRVB_, thresholdBallSensors_); updateAnalysisSensor (stat2, frontBallRVB_, thresholdBallSensors_); + log_ ("Ball") << "Devant : " << (colorSeen (frontBallRVB_) == + whiteColor_ ? "white" : "black") << " Arr : " << (colorSeen + (rearBallRVB_) == whiteColor_ ? "white" : "black"); } break; /* Others */ @@ -585,6 +590,11 @@ void Es::receive(char command, const Proto::Frame & frame) } } +void Es::setRVBHoleStat (int freq) +{ + proto_.send ('D', "b", freq); +} + /// Decode a color into a string std::string Es::decodeColor (int color) @@ -657,6 +667,7 @@ Es::newBallFront(void) barilletLancement(); log_ ("Es::Barillet", Log::debug) << "gobage balle par l'avant."; enableAllSensors (false); + setRVBBallStat (0); } /// Analyse une balle arrivant par derrière @@ -696,6 +707,7 @@ Es::newBallRear(void) } log_ ("Es::Barillet", Log::debug) << "gobage balle par l'arriere."; enableAllSensors (false); + setRVBBallStat (0); } /// Update system for one sensor diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh index 94bb68d..0a53ff3 100644 --- a/i/marvin/src/es/es.hh +++ b/i/marvin/src/es/es.hh @@ -134,6 +134,8 @@ class Es : public Proto::Receiver void setRVBSniffFrontStat (int freq); /// Enable all the sensors or just the 4 and 1 near the ground void enableAllSensors (bool enable); + // + void setRVBHoleStat (int freq); /// Set frequency of jack, selectoul printed out function /// Use this function with frequency 0 to disable this stat. diff --git a/n/es-2006/src/main.c b/n/es-2006/src/main.c index 334d77f..e377c55 100644 --- a/n/es-2006/src/main.c +++ b/n/es-2006/src/main.c @@ -372,7 +372,8 @@ main (void) { /* Re init stats system for this sensor */ sniff_rvb_hole_stats = sniff_rvb_hole_enable; - proto_send1b ('D', sniff_rvb_analysis_ball (5)); + proto_send1b + ('D', sniff_rvb_analysis_color (5, RVB_SNIFF_ALL_COLORS)); } else sniff_rvb_hole_stats = 1; -- cgit v1.2.3