summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authordufourj2006-05-26 08:12:24 +0000
committerdufourj2006-05-26 08:12:24 +0000
commit56e637ec77fc6c220009663dcc35712e399bfd5d (patch)
treed3ebab75458fdce937169fcd2931283e0c1bd3ee
parentdd7faf987331073d71248bd84f5a113ad9629a29 (diff)
Général :
- commit du programme d'après premier match.
-rw-r--r--i/marvin/src/ai/ai.cc63
-rw-r--r--i/marvin/src/ai/ai.hh2
-rw-r--r--i/marvin/src/ai/test_ai.cc2
-rw-r--r--i/marvin/src/es/es.cc30
-rw-r--r--i/marvin/src/es/es.hh6
-rw-r--r--i/marvin/src/motor/Makefile.defs2
-rw-r--r--i/marvin/src/motor/motor.cc5
-rw-r--r--i/marvin/src/motor/motor.hh2
-rw-r--r--n/asserv/src/asserv/eeprom.avr.c4
-rw-r--r--n/asserv/src/asserv/main.c3
-rw-r--r--n/asserv/src/asserv/traj.c78
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
@@ -133,7 +133,6 @@ void
Ai::prepare (void)
{
es_.lcdPrint("");
- es_.lcdPrint("");
es_.lcdPrint("Prepa match...");
es_.lcdPrint("Inserer Jack");
es_.setOthersStat (10);
@@ -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 aprs 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 prpare le robot
prepare();
/// Faire des crpes...
@@ -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 dchire 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 <cmath>
+
/// 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 <avr/eeprom.h>
/* 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;
+ }
+ }
}
}