From 56e637ec77fc6c220009663dcc35712e399bfd5d Mon Sep 17 00:00:00 2001 From: dufourj Date: Fri, 26 May 2006 08:12:24 +0000 Subject: Général : - commit du programme d'après premier match. --- i/marvin/src/ai/ai.cc | 63 +++++++++++++++++++++++++++++++--------- i/marvin/src/ai/ai.hh | 2 +- i/marvin/src/ai/test_ai.cc | 2 ++ i/marvin/src/es/es.cc | 30 ++++++++++++------- i/marvin/src/es/es.hh | 6 ++++ i/marvin/src/motor/Makefile.defs | 2 +- i/marvin/src/motor/motor.cc | 5 +++- i/marvin/src/motor/motor.hh | 2 ++ 8 files changed, 85 insertions(+), 27 deletions(-) (limited to 'i') diff --git a/i/marvin/src/ai/ai.cc b/i/marvin/src/ai/ai.cc index ac06157..19f3cca 100644 --- a/i/marvin/src/ai/ai.cc +++ b/i/marvin/src/ai/ai.cc @@ -132,7 +132,6 @@ Ai::waitJack (bool out) void Ai::prepare (void) { - es_.lcdPrint(""); es_.lcdPrint(""); es_.lcdPrint("Prepa match..."); es_.lcdPrint("Inserer Jack"); @@ -146,8 +145,8 @@ Ai::prepare (void) // Set actual position motor_.getAsserv().setPos(0,0,0); es_.barilletInit(); - es_.lcdPrint("Virer le jack..."); - es_.lcdPrint("PRET POUR MATCH"); + es_.lcdPrint("Barillet pret ?"); + es_.lcdPrint("Virer le jack !"); while (!update ()); // We first wait for the jack to be put inside waitJack (true); @@ -166,7 +165,7 @@ Ai::afterMatch(void) { // On attend de mettre le jack après le match es_.lcdPrint("FIN DU MATCH"); - es_.lcdPrint("Inserer jack..."); +// es_.lcdPrint("Inserer jack..."); while (!update()); waitJack(false); // On vide le barillet @@ -260,7 +259,8 @@ void Ai::progHomoloRobal(void) void Ai::progMatch(void) { - bool isFinish; + init (); +// bool isFinish; /// On prépare le robot prepare(); /// Faire des crêpes... @@ -270,21 +270,56 @@ Ai::progMatch(void) temporisation (1000); es_.barilletLancement(); while (!update ()); + log_ ("match") << "Totem activator"; + es_.totemActivator (true); /// En avant guingamp + log_ ("match") << "Eloignement du bord"; motorMove(480,0); /// On tourner de -90° - motorMove(432,432); + log_ ("match") << "Petite rotation de 90°"; + motorMove(216,-216); /// On dit bonjour au mur - if(motorMove(900, 0)) - motorMove(200, 0); + log_ ("match") << "Va à l'opposé"; + if (!motorMove (1000, 0)) + { + log_ ("match") << "Un mur ! Reculons"; + motorMove (-70, 0); + } +// motorMove(200, 0); /// On tourne bizarrement mais de 90° - motorMove(-72,-72); - motorMove(360,-360); + log_ ("match") << "On tourne à 90° en deux fois"; + motorMove(-36,36); + motorMove(190, 190); /// On avance un peu - motorMove(150,0); - // On tourne de 90° - motorMove(432,-432); - motorMove(1400,0); +// 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 (1320, 0); + log_ ("match") << "On recule un peu"; + motorMove (-420, 0); + log_ ("match") << "On tourne"; + motorMove (150, 150); + motorBasicFindHole (); + es_.dropWhiteBall(); + while (!update ()); + temporisation (3000); + es_.deposeBalle (); + while (!update ()); + motorBasicFindHole (); + es_.dropWhiteBall(); + while (!update ()); + temporisation (3000); + es_.deposeBalle (); + afterMatch(); } diff --git a/i/marvin/src/ai/ai.hh b/i/marvin/src/ai/ai.hh index b7b0833..e3f7e19 100644 --- a/i/marvin/src/ai/ai.hh +++ b/i/marvin/src/ai/ai.hh @@ -76,7 +76,7 @@ class Ai void progHomoloRobal(void); /// programme de match du robal void progMatch(void); - void motorMove (int d, int a); + bool motorMove (int d, int a); void motorRotate (double d); void motorBasicFindHole (void); int motorSmartFindHole (double distOut); diff --git a/i/marvin/src/ai/test_ai.cc b/i/marvin/src/ai/test_ai.cc index dd51b7d..6a7d09f 100644 --- a/i/marvin/src/ai/test_ai.cc +++ b/i/marvin/src/ai/test_ai.cc @@ -79,6 +79,8 @@ class TestAi : public Tester "Programme d'homologation du robal"); interpreter.add("match", Interpreter::memFunc(ai_, &Ai::progMatch), "Programme de match qui déchire du robal"); + interpreter.add("findHoleBasic", Interpreter::memFunc(ai_, &Ai::motorBasicFindHole), + "Find a hole, basic version"); interpreter.add ("_postcall", Interpreter::memFunc (*this, &TestAi::postcall)); } diff --git a/i/marvin/src/es/es.cc b/i/marvin/src/es/es.cc index 8c4c6fb..7b0505b 100644 --- a/i/marvin/src/es/es.cc +++ b/i/marvin/src/es/es.cc @@ -617,27 +617,27 @@ Es::newBallFront(void) switch (positionBarillet_) { case avant0: - stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[0] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = avant4; log_ ("Es::Barillet", Log::debug) << "trou 0:" << ((stockBarillet[0] == white)?"white":"pas white"); break; case avant1: - stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[1] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = avant0; log_ ("Es::Barillet", Log::debug) << "trou 1:" << ((stockBarillet[1] == white)?"white":"pas white"); break; case avant2: - stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[2] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = avant1; log_ ("Es::Barillet", Log::debug) << "trou 2:" << ((stockBarillet[2] == white)?"white":"pas white"); break; case avant3: - stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[3] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = avant2; log_ ("Es::Barillet", Log::debug) << "trou 3:" << ((stockBarillet[3] == white)?"white":"pas white"); break; case avant4: - stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[4] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = avant3; log_ ("Es::Barillet", Log::debug) << "trou 4:" << ((stockBarillet[4] == white)?"white":"pas white"); break; @@ -659,27 +659,27 @@ Es::newBallRear(void) switch (positionBarillet_) { case arriere0: - stockBarillet[0] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[0] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = arriere4; log_ ("Es::Barillet", Log::debug) << "trou 0"; break; case arriere1: - stockBarillet[1] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[1] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = arriere0; log_ ("Es::Barillet", Log::debug) << "trou 1"; break; case arriere2: - stockBarillet[2] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[2] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = arriere1; log_ ("Es::Barillet", Log::debug) << "trou 2"; break; case arriere3: - stockBarillet[3] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[3] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = arriere2; log_ ("Es::Barillet", Log::debug) << "trou 3"; break; case arriere4: - stockBarillet[4] = colorSeen(frontBallRVB_)==whiteColor_?white:black; + stockBarillet[4] = ((colorSeen(frontBallRVB_))==whiteColor_)?white:white; positionBarillet_ = arriere3; log_ ("Es::Barillet", Log::debug) << "trou 4"; break; @@ -719,3 +719,13 @@ Es::colorSeen (int sensor_num) { return seenColors_[sensor_num]; } + +/// Totems activator +void +Es::totemActivator (bool out) +{ + // Right + setServoPos (2, out ? 0 : 200); + // Left + setServoPos (4, out ? 200 : 0); +} diff --git a/i/marvin/src/es/es.hh b/i/marvin/src/es/es.hh index 8abcca4..94bb68d 100644 --- a/i/marvin/src/es/es.hh +++ b/i/marvin/src/es/es.hh @@ -185,6 +185,12 @@ class Es : public Proto::Receiver bool barilletIsFull(void); /// What color do you see my lord ? int colorSeen (int sensor_num); + /// Totems activator + void totemActivator (bool out); + /// Front sensor + bool frontSensor (void) { return front_sensor_; } + /// Clear sensor + void clearFrontSensor (void) { front_sensor_ = false; } private: /// Decode a color into a string diff --git a/i/marvin/src/motor/Makefile.defs b/i/marvin/src/motor/Makefile.defs index 1efcd13..ce79e10 100644 --- a/i/marvin/src/motor/Makefile.defs +++ b/i/marvin/src/motor/Makefile.defs @@ -1,5 +1,5 @@ PROGRAMS += test_motor -motor_OBJECTS = motor.o $(asserv_OBJECTS) +motor_OBJECTS = motor.o $(asserv_OBJECTS) $(log_OBJECTS) test_motor_OBJECTS = test_motor.o $(motor_OBJECTS) $(tester_OBJECTS) diff --git a/i/marvin/src/motor/motor.cc b/i/marvin/src/motor/motor.cc index d3fadfc..32d94eb 100644 --- a/i/marvin/src/motor/motor.cc +++ b/i/marvin/src/motor/motor.cc @@ -24,10 +24,12 @@ // }}} #include "motor.hh" +#include + /// Constructor. Motor::Motor (void) : asserv_ (*this), seq_ (0), finish_ (true), blocked_ (false), - x_ (0.0), y_ (0.0), a_ (0.0) + x_ (0.0), y_ (0.0), a_ (0.0), log_ ("Motor") { } @@ -151,6 +153,7 @@ Motor::receivePos (double x, double y, double a) x_ = x; y_ = y; a_ = a; + log_ ("Pos") << "x " << x << " y " << y << " a " << a / (2 * M_PI) * 360; } void diff --git a/i/marvin/src/motor/motor.hh b/i/marvin/src/motor/motor.hh index f1c9843..548c7bd 100644 --- a/i/marvin/src/motor/motor.hh +++ b/i/marvin/src/motor/motor.hh @@ -25,6 +25,7 @@ // // }}} #include "asserv/asserv.hh" +#include "log/log.hh" /// Handle movements. class Motor : public Asserv::Receiver @@ -35,6 +36,7 @@ class Motor : public Asserv::Receiver bool finish_; bool blocked_; double x_, y_, a_; + Log log_; public: /// Constructor. Motor (void); -- cgit v1.2.3