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 ++ n/asserv/src/asserv/eeprom.avr.c | 4 ++- n/asserv/src/asserv/main.c | 3 ++ n/asserv/src/asserv/traj.c | 78 +++++++++++++++++++++++++--------------- 11 files changed, 141 insertions(+), 56 deletions(-) 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); diff --git a/n/asserv/src/asserv/eeprom.avr.c b/n/asserv/src/asserv/eeprom.avr.c index 3abba92..dcd567c 100644 --- a/n/asserv/src/asserv/eeprom.avr.c +++ b/n/asserv/src/asserv/eeprom.avr.c @@ -25,7 +25,7 @@ #include /* Change the eeprom key each time you change eeprom format. */ -#define EEPROM_KEY 0x45 +#define EEPROM_KEY 0x44 #define EEPROM_START 256 /* +AutoDec */ @@ -54,6 +54,7 @@ eeprom_read_params (void) pos_alpha_kd = eeprom_read_word (p16++); pos_e_sat = eeprom_read_word (p16++); pos_int_sat = eeprom_read_word (p16++); + pos_blocked = eeprom_read_word (p16++); } /* Write parameters to eeprom. */ @@ -78,6 +79,7 @@ eeprom_write_params (void) eeprom_write_word (p16++, pos_alpha_kd); eeprom_write_word (p16++, pos_e_sat); eeprom_write_word (p16++, pos_int_sat); + eeprom_write_word (p16++, pos_blocked); } /* Clear eeprom parameters. */ diff --git a/n/asserv/src/asserv/main.c b/n/asserv/src/asserv/main.c index 63fd580..943c2e8 100644 --- a/n/asserv/src/asserv/main.c +++ b/n/asserv/src/asserv/main.c @@ -405,6 +405,9 @@ proto_callback (uint8_t cmd, uint8_t size, uint8_t *args) case c ('I', 3): pos_int_sat = v8_to_v16 (args[1], args[2]); break; + case c ('b', 3): + pos_blocked = v8_to_v16 (args[1], args[2]); + break; case c ('w', 3): /* Set PWM direction. * - b: inverse left direction. diff --git a/n/asserv/src/asserv/traj.c b/n/asserv/src/asserv/traj.c index eb0ef92..d60f7c9 100644 --- a/n/asserv/src/asserv/traj.c +++ b/n/asserv/src/asserv/traj.c @@ -31,6 +31,9 @@ uint8_t traj_mode; /** Holes filter. */ uint8_t traj_holes_filter; +/** Holes ok filter. */ +uint8_t traj_holes_ok; + /* Find a hole in the table. * French: On avance, et si on voit un "pas vert" à gauche, on tourne à * gauche. Pareil pour la droite. Au final, on centre le "pas vert" entre deux @@ -73,6 +76,7 @@ traj_holes (void) { traj_holes_filter = 255; traj_mode = 12; + traj_holes_ok = 0; } } else if (traj_mode == 12) @@ -84,39 +88,57 @@ traj_holes (void) traj_mode = 11; } /* Determine speed. */ - speed = traj_mode <= 11 ? speed_theta_max : speed_theta_slow; - speed *= 256; - if ((in & 0xf) == 0xc) - { - /* Stop. */ - speed_theta_cons = 0; - speed_alpha_cons = 0; - main_sequence_finish = main_sequence; - traj_mode = 13; - } - else if (in & 1) - { - /* Left. */ - speed_theta_cons = 0; - speed_alpha_cons = speed; - } - else if (in & 2) - { - /* Right. */ - speed_theta_cons = 0; - speed_alpha_cons = -speed; - } - else if (in & 8) + if (traj_mode <= 11) { - /* Back. */ - speed_theta_cons = -speed; + /* Cruise mode. */ + speed = speed_theta_max; + speed *= 256; + speed_theta_cons = speed; speed_alpha_cons = 0; } else { - /* Front. */ - speed_theta_cons = speed; - speed_alpha_cons = 0; + speed = speed_theta_slow; + speed *= 256; + if ((in & 0xf) == 0xc || (in & 0x7) == 0x7) + { + traj_holes_ok++; + if (traj_holes_ok >= 32) + { + /* Stop. */ + speed_theta_cons = 0; + speed_alpha_cons = 0; + main_sequence_finish = main_sequence; + traj_mode = 13; + } + } + else + { + if (in & 1) + { + /* Left. */ + speed_theta_cons = 0; + speed_alpha_cons = speed; + } + else if (in & 2) + { + /* Right. */ + speed_theta_cons = 0; + speed_alpha_cons = -speed; + } + else if (in & 8) + { + /* Back. */ + speed_theta_cons = -speed; + speed_alpha_cons = 0; + } + else + { + /* Front. */ + speed_theta_cons = speed; + speed_alpha_cons = 0; + } + } } } -- cgit v1.2.3