summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--i/marvin/runtime/rc/config6
-rw-r--r--i/marvin/src/ai/ai.cc84
-rw-r--r--i/marvin/src/asserv/asserv.cc2
-rw-r--r--i/marvin/src/es/es.cc16
-rw-r--r--i/marvin/src/es/es.hh2
-rw-r--r--n/es-2006/src/main.c3
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 <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;
}
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;