summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--2005/i/robert/src/ai/ai.cc136
-rw-r--r--2005/i/robert/src/ai/ai.hh2
-rw-r--r--2005/i/robert/src/ai/test_ai.cc18
-rw-r--r--2005/i/robert/src/asserv/test_asserv.cc4
-rw-r--r--2005/i/robert/src/es/es.cc20
-rw-r--r--2005/i/robert/src/es/es.hh8
-rw-r--r--2005/i/robert/src/es/test_es.cc9
-rw-r--r--2005/i/robert/src/motor/motor.cc4
-rw-r--r--2005/i/robert/src/motor/motor.hh2
-rw-r--r--2005/i/robert/src/motor/test_motor.cc16
-rw-r--r--2005/i/robert/src/proto/proto.cc16
-rw-r--r--2005/i/robert/src/proto/proto.hh2
12 files changed, 129 insertions, 108 deletions
diff --git a/2005/i/robert/src/ai/ai.cc b/2005/i/robert/src/ai/ai.cc
index 25445e8..341f260 100644
--- a/2005/i/robert/src/ai/ai.cc
+++ b/2005/i/robert/src/ai/ai.cc
@@ -96,12 +96,6 @@ void Ai::stop(void)
} while (!motor_.idle ());
}
-/// Attend des données.
-void Ai::wait (int t)
-{
- motor_.wait(t);
-}
-
/// Fonction de temporisation
void Ai::temporisation(int t)
{
@@ -203,7 +197,7 @@ void Ai::ventouses (void)
/// Attend une mise à jour
bool Ai::update (void)
{
- scheduler_.schedule(500, true);
+ scheduler_.schedule(100, true);
bool retour = sync ();
// On vérifie que le match n'est pas fini
if (Timer::getRoundTime () > roundDuration_)
@@ -215,12 +209,7 @@ bool Ai::update (void)
bool
Ai::sync (void)
{
- bool motor = motor_.sync();
- bool es = es_.sync();
- if (es && motor)
- return true;
- else
- return false;
+ return es_.sync () && motor_.sync ();
}
/// Reset la PWM à zéro.
@@ -292,12 +281,33 @@ Ai::runProtectFond (void)
void Ai::runVisionLine(void)
{
- initTest();
+ initTest ();
+ int i = 1;
+ bool sensHorraire = true;
do
{
- scanVision(true);
- basic(100);
- }while(motor_.getY() < 1700);
+ // On recherche une quille et on y va si besoin
+ scanVision (true);
+ std::cout << "Petite pause d'1 seconde" << std::endl;
+ temporisation (1000);
+ // On continue à tourner
+ rotation (((2. * M_PI) / 8.) * i);
+ if (sensHorraire)
+ i++;
+ else
+ i--;
+ if (i == 9)
+ {
+ sensHorraire = !sensHorraire;
+ i -= 2;
+ }
+ if (i == -1)
+ {
+ sensHorraire = !sensHorraire;
+ i += 2;
+ }
+ }
+ while (true);
}
// Pour les Tests.
void
@@ -317,7 +327,6 @@ Ai::runTest (void)
//stop ();
// On restaure la vitesse
temporisation (400);
- init ();
std::cout << "On restaure la vitesse à 8" << std::endl;
setAccel (96);
setMaxSpeed (8, -1);
@@ -384,7 +393,7 @@ void Ai::initMatch (void)
temporisation(tempoDebutMatch_);
}/*}}}*/
-// Parcourt la ligne du fond, en aveugle pour relever des quilles et les poser derrière soit
+// Parcourt la ligne du fond, en aveugle pour relever des quilles et les poser derrière soit/*{{{*/
void
Ai::parcourtFond (void)
{
@@ -430,17 +439,27 @@ Ai::parcourtFond (void)
}
std::cout << "Fini : " << Timer::getRoundTime () << std::endl;
}
-
+/*}}}*/
// *** Fonctions de vision ***
// On scan tout autour du robot
void Ai::scanVision (bool followLine) /// XXX mettre un argument
{
+ // XXX Cet algo n'est pas bon : c'est trop compliqué de retenir la
+ // position des quilles.
+ // Il faut prendre des photos jusquà qu'on trouve une quille.
+ // La liste doit être purgé à chaque déplacement car elle ne fonctionne
+ // qu'en position relative, à moins d'arriver à les stoquer en position
+ // absolue.
+ // Il serai interessant de stoquer la position (x, y, et orientation) et
+ // de maintenir cette liste et de virer les quilles que l'on a relever.
+ // XXX
std::cout << "On commence le scannage de quiles" << std::endl;
double angleDepart = motor_.getA();
std::vector<std::vector<Zone> > skittles;
for (int i = 0; i < (followLine ? 1 : 4) ; i++)
{
+ temporisation (1000);
std::cout << "On prend la photo" << std::endl;
// On prend la photo
oVision_.takeShoot();
@@ -471,14 +490,16 @@ void Ai::analyseSkittles(std::vector<std::vector<Zone> > skittlesBrut)
// On regarde zone par zone
for(std::vector<Zone>::iterator it2 = it->begin(); it2 != it->end(); it2++)
{
- if (!it2->alone)
- continue;
- if (it2->vertical)
- continue;
- if (it2->partial) // XXX to manage
- continue;
- if (it2->small)
- continue;
+ std::cout << "On regarde si on a une quille qui a les bonnes "
+ "propriétés" << std::endl;
+// if (!it2->alone)
+// continue;
+// if (it2->vertical)
+// continue;
+// if (it2->partial) // XXX to manage
+// continue;
+// if (it2->small)
+// continue;
std::cout << "On a une quille une gagnante !" << std::endl;
goodSkittles_.push_back(*it2);
}
@@ -489,7 +510,8 @@ void Ai::analyseSkittles(std::vector<std::vector<Zone> > skittlesBrut)
// On remonte la ze quille la meilleur détecté par la cam
void Ai::remonteWithCam(void)
{
- int x, y , angle, dist;
+ int x, y , dist;
+ double angle;
std::cout << "remonteWithCam" << std::endl;
if (goodSkittles_.size () > 0)
{
@@ -500,30 +522,38 @@ void Ai::remonteWithCam(void)
oVision_.getLoc(zeSkittles, x, y, angle, dist);
std::cout << "x : " << x << ", y : " << y <<
", a : " << angle << ", d : " << dist << "." << std::endl;
- // On enlève la partie avant du robot du au calibrage
- dist -= 90;
- // On pivote de l'angle demandée
- // XXX position relative par rapport à la prise de photo
- std::cout << "Rotation" << std::endl;
- rotation(motor_.getA() + angle);
- std::cout << "Pas Max speed" << std::endl;
-// setMaxSpeed(2,1);
- std::cout << "basic dist/d" << std::endl;
- basic(dist);
- std::cout << "Ascenceur +" << std::endl;
- ascenceur(true);
-// basic (-dist - 5);
-// rotation(M_PI);
-// rotation(M_PI / 2);
-// basic(30); // XXX vérifier valeur
- ventouses();
- std::cout << "Max speed" << std::endl;
- setMaxSpeed(8, 4);
- std::cout << "basic - 30" << std::endl;
- basic(-30); // XXX Vérifier valeur
- std::cout << "Ascenceur -" << std::endl;
- ascenceur(false);
-// rotation(3 * M_PI / 4);
+ if (dist < 600)
+ {
+ // On enlève la partie avant du robot du au calibrage
+ // dist -= 90;
+ // On pivote de l'angle demandée
+ // XXX position relative par rapport à la prise de photo
+ std::cout << "Rotation" << std::endl;
+ rotation(motor_.getA() + angle);
+// std::cout << "Max speed 2, -1" << std::endl;
+// setMaxSpeed(3, -1);
+ std::cout << "basic dist" << std::endl;
+ basic(dist);
+ std::cout << "Ascenceur up" << std::endl;
+ ascenceur(true);
+ // basic (-dist - 5);
+ // rotation(M_PI);
+ // rotation(M_PI / 2);
+ // basic(30); // XXX vérifier valeur
+ std::cout << "On temporise de 1500 ms" << std::endl;
+ temporisation (1500);
+ std::cout << "On relache les ventouses" << std::endl;
+ ventouses();
+ std::cout << "On temporise de 500 ms" << std::endl;
+ temporisation (1000);
+// std::cout << "Max speed 4, -1" << std::endl;
+// setMaxSpeed(4, -1);
+ std::cout << "On recule de la distance parcouru" << std::endl;
+ basic(-dist); // XXX Vérifier valeur
+ std::cout << "Ascenceur down" << std::endl;
+ ascenceur(false);
+ }
+ // XXX Here, we should check if the skittle is up or not
}
}
diff --git a/2005/i/robert/src/ai/ai.hh b/2005/i/robert/src/ai/ai.hh
index bf89910..b4a3c8d 100644
--- a/2005/i/robert/src/ai/ai.hh
+++ b/2005/i/robert/src/ai/ai.hh
@@ -63,8 +63,6 @@ class Ai
void init (void);
/// Arrète le robot.
void stop (void);
- /// Attend des données (préférez plutôt temporisation).
- void wait (int t);
/// Fonction de temporisation
void temporisation (int t);
// Attend le jack entré (false) ou sorti (true).
diff --git a/2005/i/robert/src/ai/test_ai.cc b/2005/i/robert/src/ai/test_ai.cc
index 129ed44..c1cae62 100644
--- a/2005/i/robert/src/ai/test_ai.cc
+++ b/2005/i/robert/src/ai/test_ai.cc
@@ -219,20 +219,10 @@ main (int argc, char **argv)
break;
}
case 'w':
- {
- int stop, t;
- if (i + 1 >= argc)
- throw std::runtime_error ("syntax error");
- stop = atoi (argv[++i]) + Timer::getProgramTime ();
- t = Timer::getProgramTime ();
- while (t < stop)
- {
- ai_.wait (stop - t);
- t = Timer::getProgramTime ();
- ai_.update();
- }
- break;
- }
+ if (i + 1 >= argc)
+ throw std::runtime_error ("syntax error");
+ ai_.temporisation (atoi (argv[++i]));
+ break;
case '?':
syntax ();
return 0;
diff --git a/2005/i/robert/src/asserv/test_asserv.cc b/2005/i/robert/src/asserv/test_asserv.cc
index 228748c..6d4e672 100644
--- a/2005/i/robert/src/asserv/test_asserv.cc
+++ b/2005/i/robert/src/asserv/test_asserv.cc
@@ -311,7 +311,8 @@ main (int argc, char **argv)
break;
}
}
- asserv.sync();
+ while (!asserv.wait ())
+ ;
break;
}
case 'w':
@@ -325,7 +326,6 @@ main (int argc, char **argv)
{
asserv.wait (stop - t);
t = Timer::getProgramTime ();
- asserv.sync();
}
break;
}
diff --git a/2005/i/robert/src/es/es.cc b/2005/i/robert/src/es/es.cc
index d1e8e76..9c32e9b 100644
--- a/2005/i/robert/src/es/es.cc
+++ b/2005/i/robert/src/es/es.cc
@@ -66,7 +66,16 @@ bool Es::ascIsIdle(void)
bool Es::obsTropPres(void)
{
- if(distGauche_ > teleSeuil_ || distDroite_ > teleSeuil_)
+ if (distGauche_ > teleSeuil_ || distDroite_ > teleSeuil_
+ || obsBasTropPres ())
+ return true;
+ else
+ return false;
+}
+bool
+Es::obsBasTropPres (void)
+{
+ if (distBas_ > teleSeuilBas_)
return true;
else
return false;
@@ -89,9 +98,10 @@ void Es::ventouses(void)
proto_.send('v',"b",pVentouses_);
}
-void Es::wait(int timeout)
+bool
+Es::wait(int timeout /*-1*/)
{
- proto_.wait(timeout);
+ return proto_.wait(timeout);
}
/// Récupère le File Descriptor
@@ -114,6 +124,7 @@ void Es::loadConfig(const Config & config)
pStatTele_ = config.get<int>("es.pStatTele");
teleSeuil_ = config.get<int>("es.teleSeuil");
+ teleSeuilBas_ = config.get <int> ("es.teleSeuilBas");
}
void Es::receive(char command, const Proto::Frame & frame)
@@ -137,8 +148,7 @@ void Es::receive(char command, const Proto::Frame & frame)
ascFailure_ = true;
break;
case 'C': // retour télémètre
- proto_.decode(frame,"ww", distGauche_, distDroite_);
-// log_("Télémètre") << "Valeur gauche" << distGauche_ << "Valeur Droite" << distDroite_;
+ proto_.decode(frame,"www", distGauche_, distDroite_, distBas_);
break;
}
diff --git a/2005/i/robert/src/es/es.hh b/2005/i/robert/src/es/es.hh
index bccfcf6..df5aaec 100644
--- a/2005/i/robert/src/es/es.hh
+++ b/2005/i/robert/src/es/es.hh
@@ -59,6 +59,10 @@ class Es : public Proto::Receiver
int distDroite_;
/// Seuil du télémètre
int teleSeuil_;
+ /// Distance du télémêtre du bas (quille dans l'ascenceur)
+ int distBas_;
+ /// Seuil du télémêtre du bas
+ int teleSeuilBas_;
/// Système de log
Log log_;
@@ -75,6 +79,8 @@ class Es : public Proto::Receiver
bool ascIsIdle(void);
/// Renvoie si l'obstacle le plus proche est trop proche
bool obsTropPres(void);
+ /// Renvoie la présence d'un obstacle devant le capteur du bas
+ bool obsBasTropPres (void);
/// Fait monter l'ascenceur
void monterAsc(void);
/// Fait descendre l'ascenceur
@@ -82,7 +88,7 @@ class Es : public Proto::Receiver
/// Désactive les ventouses pour X secondes XXX vérifier la durées arg/config
void ventouses(void);
/// On attend ...
- void wait(int timeout);
+ bool wait(int timeout = -1);
/// Récupère le File Descriptor
int getFd(void);
/// Charge les paramètre du fichier de config
diff --git a/2005/i/robert/src/es/test_es.cc b/2005/i/robert/src/es/test_es.cc
index 630edc6..7e49762 100644
--- a/2005/i/robert/src/es/test_es.cc
+++ b/2005/i/robert/src/es/test_es.cc
@@ -93,10 +93,10 @@ main (int argc, char **argv)
std::cout << "Le prochain obstacle est " << (es.obsTropPres() ? "TROP PRES." : "LOIN.") << std::endl;
break;
}
- do
- {
- es.sync();
- }while(!es.ascIsIdle());
+
+ while (!es.wait (-1) || !es.ascIsIdle())
+ ;
+
break;
}
case 'w':
@@ -110,7 +110,6 @@ main (int argc, char **argv)
{
es.wait (stop - t);
t = Timer::getProgramTime ();
- es.sync();
}
break;
}
diff --git a/2005/i/robert/src/motor/motor.cc b/2005/i/robert/src/motor/motor.cc
index c292e9c..7e78aff 100644
--- a/2005/i/robert/src/motor/motor.cc
+++ b/2005/i/robert/src/motor/motor.cc
@@ -206,9 +206,11 @@ bool Motor::sync(void)
return false;
}
-void Motor::wait(int timeout)
+bool
+Motor::wait(int timeout)
{
asserv_.wait(timeout);
+ return sync ();
}
/// Récupère le File Descriptor
diff --git a/2005/i/robert/src/motor/motor.hh b/2005/i/robert/src/motor/motor.hh
index b527525..ad67f84 100644
--- a/2005/i/robert/src/motor/motor.hh
+++ b/2005/i/robert/src/motor/motor.hh
@@ -100,7 +100,7 @@ class Motor : public Asserv::Receiver
/// Syncronisation
bool sync(void);
/// On attend...
- void wait(int timeout);
+ bool wait(int timeout = -1);
/// Récupère le File Descriptor
int getFd(void);
/// Retoure l'état du jack (false entrée et true sortie)
diff --git a/2005/i/robert/src/motor/test_motor.cc b/2005/i/robert/src/motor/test_motor.cc
index 137e355..f01e299 100644
--- a/2005/i/robert/src/motor/test_motor.cc
+++ b/2005/i/robert/src/motor/test_motor.cc
@@ -112,20 +112,9 @@ main (int argc, char **argv)
throw std::runtime_error("syntax error");
motor.setAccel(strtol(argv[0], 0, 10));
break;
- case 'L':
- while(!motor.idle())
- {
- motor.wait(-1);
- motor.sync();
- }
- break;
- }
- motor.sync();
- while(!motor.idle())
- {
- motor.wait(-1);
- motor.sync();
}
+ while(!motor.wait () || !motor.idle ())
+ ;
break;
}
@@ -140,7 +129,6 @@ main (int argc, char **argv)
{
motor.wait (stop - t);
t = Timer::getProgramTime ();
- motor.sync();
}
break;
}
diff --git a/2005/i/robert/src/proto/proto.cc b/2005/i/robert/src/proto/proto.cc
index a7ada32..3569a47 100644
--- a/2005/i/robert/src/proto/proto.cc
+++ b/2005/i/robert/src/proto/proto.cc
@@ -56,11 +56,10 @@ Proto::close(void)
bool
Proto::sync(void)
{
- bool reGet;
// Récupération de la frame
- while (reGet = getFrame())
+ while (getFrame())
{
- //log_ ("recv") << "frame" << currentFrame_;
+// log_ ("recv") << "frame" << currentFrame_;
// Si la frame est un aquittement
if(currentFrame_ == frameQueue_.front())
{
@@ -96,7 +95,6 @@ Proto::send (const Frame & frame, bool reliable)
}
else
sendFrame(frame);
- // XXX sync();
}
@@ -197,7 +195,8 @@ bool
Proto::wait (int timeout/*-1*/)
{
// On apelle serial_.wait avec un timeout toujours inf ou égal à proto::timeout_
- return serial_.wait (timeout < timeout_ ? timeout : timeout_);
+ serial_.wait (timeout < timeout_ && timeout != -1 ? timeout : timeout_);
+ return sync ();
}
/// Récupère le File Descriptor
@@ -241,7 +240,6 @@ Proto::getFrame(void)
currentFrame_.command = receivedChar;
revState_ = 2;
}
- // On vérifie que le match n'est pas fini
break;
case 2:
d = hex2digit (receivedChar);
@@ -270,7 +268,7 @@ Proto::getFrame(void)
{
// On renvoie en mettant le compteur à 0, la commande sera
// renvoyer de retour à sync
- //log_("Erreur de reception") << "commande inconnue" << "";
+// log_("Erreur de reception") << "commande inconnue" << "";
tLastSend_ = 0;
revState_ = 0;
return false;
@@ -283,7 +281,7 @@ Proto::getFrame(void)
void
Proto::sendFrame(const Frame & frame)
{
- //log_ ("send") << "frame" << frame;
+// log_ ("send") << "frame" << frame;
// envoyer le bang
serial_.putchar('!');
@@ -292,7 +290,7 @@ Proto::sendFrame(const Frame & frame)
// Envoyer les arguments
for(std::vector<uint8_t>::const_iterator it = frame.args.begin();
- it != frame.args.end(); it++)
+ it != frame.args.end(); ++it)
{
serial_.putchar(digit2hex(*it >> 4));
serial_.putchar(digit2hex(*it & 0x0f));
diff --git a/2005/i/robert/src/proto/proto.hh b/2005/i/robert/src/proto/proto.hh
index 11b1ae3..47ef9a0 100644
--- a/2005/i/robert/src/proto/proto.hh
+++ b/2005/i/robert/src/proto/proto.hh
@@ -55,7 +55,7 @@ class Proto : public NonCopyable
//Date du dernier envoie
int tLastSend_;
//Temps entre les ré-émission
- static const int timeout_ = 500;
+ static const int timeout_ = 100;
/// Frame en cours de réception
Frame currentFrame_;