summaryrefslogtreecommitdiff
path: root/i/marvin/src
diff options
context:
space:
mode:
Diffstat (limited to 'i/marvin/src')
-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
8 files changed, 85 insertions, 27 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 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 <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);