summaryrefslogtreecommitdiff
path: root/i
diff options
context:
space:
mode:
authordufourj2006-05-26 22:02:30 +0000
committerdufourj2006-05-26 22:02:30 +0000
commit526a70ea2f2505d512b6567ebf052ad0b65c2319 (patch)
tree743dc77b9f594d5b096169d38df3847bf9a879f3 /i
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')
-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
5 files changed, 66 insertions, 44 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 prpare le robot
prepare();
/// Faire des crpes...
/// 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
- // ?
- // rponse : 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 dpass
- // 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 dfinie 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 derrire
@@ -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.