From aadf5eea6b2fb123541ce2683fa31d477a33d8bb Mon Sep 17 00:00:00 2001
From: Guillaume Buret <guillaume.buret@telecom-bretagne.eu>
Date: Thu, 24 Mar 2016 14:21:21 +0100
Subject: [PATCH] ajout d'une etape cabine sur le graphe

---
 stm32/src/strategie/cabine.cpp    | 36 ++++++++++++++++++++++++++++---
 stm32/src/strategie/cubeDebut.cpp |  3 +++
 stm32/src/strategie/krabi2016.cpp | 23 +++++++++++++-------
 3 files changed, 51 insertions(+), 11 deletions(-)

diff --git a/stm32/src/strategie/cabine.cpp b/stm32/src/strategie/cabine.cpp
index 0679224c..3fdb2800 100644
--- a/stm32/src/strategie/cabine.cpp
+++ b/stm32/src/strategie/cabine.cpp
@@ -10,9 +10,9 @@
 
 Cabine::Cabine(){}
 
-Cabine::Cabine(Position position):MediumLevelAction(position)
+Cabine::Cabine(Position goalPosition):MediumLevelAction(goalPosition)
 {
-
+    goalPosition = this->goalPosition;
 }
 
 Cabine::~Cabine(){}
@@ -27,11 +27,41 @@ int Cabine::update()
 
     if (status == 0) //Début
     {
+        StrategieV2::setCurrentGoal(this->getGoalPosition(), false, VITESSE_LINEAIRE_MAX, -100.0, 200.f);
 #ifndef ROBOTHW
-        qDebug() << "Ouverture cabine";
+        qDebug() << "On se prepare a pousser une porte";
 #endif
      status++;
     }
 
+    else if (status == 1) {
+        if (Command::isNear(this->getGoalPosition(), 200.0f)) // le second paramètre est la distance a l'objectif
+        {
+            // après avoir poussé les cubes on revient en marche arrière
+            StrategieV2::setCurrentGoal(Position(600, 900), true, VITESSE_LINEAIRE_MAX, -100.0, 10.f);
+#ifndef ROBOTHW
+        qDebug() << "On revient en marche arriere au point ou on prend son elan";
+#endif
+            status++;
+        }
+    }
+
+    else if (status == 2) {
+        if (Command::isNear(Position(600, 900), 10.0f)) // le second paramètre est la distance a l'objectif
+        {
+#ifndef ROBOTHW
+        qDebug() << "On est revenu au point ou on prend son elan";
+#endif
+            status++;
+        }
+    }
+
+    else if (status == 3) {
+#ifndef ROBOTHW
+        qDebug() << "Etape fermeture de cabine finie";
+#endif
+        status = -1;
+    }
+
     return status;
 }
diff --git a/stm32/src/strategie/cubeDebut.cpp b/stm32/src/strategie/cubeDebut.cpp
index 7a3e4b65..b548842d 100644
--- a/stm32/src/strategie/cubeDebut.cpp
+++ b/stm32/src/strategie/cubeDebut.cpp
@@ -47,6 +47,9 @@ int CubeDebut::update()
     }
 
     else if (status == 2) {
+
+        // A faire, ne pas hardcoder la position
+
         if (Command::isNear(Position(600, 900), 10.0f)) // le second paramètre est la distance a l'objectif
         {
 #ifndef ROBOTHW
diff --git a/stm32/src/strategie/krabi2016.cpp b/stm32/src/strategie/krabi2016.cpp
index 9342c680..af82e0df 100644
--- a/stm32/src/strategie/krabi2016.cpp
+++ b/stm32/src/strategie/krabi2016.cpp
@@ -23,32 +23,39 @@ Krabi2016::Krabi2016(bool isYellow) : StrategieV3(isYellow)
 
     /** Points de passage **/
     int wa = Etape::makeEtape(Position(600,  900, true));
-    int wb = Etape::makeEtape(Position(680,  740, true));
+    int wb = Etape::makeEtape(Position(680,  700, true));
+    int wc = Etape::makeEtape(Position(1000,  500, true));
+    int wd = Etape::makeEtape(Position(400,  500, true));
 
     // On crée l'étape "pousse les cubes du début"
-    int c1 = Etape::makeEtape(new CubeDebut(Position(900, 900, true)));
+    int cd1 = Etape::makeEtape(new CubeDebut(Position(900, 900, true)));
 
 
     /** Actions **/
     // Zone de construction
-    int zc1 = Etape::makeEtape(new ZoneConstruction(Position(980, 920, true), benne));
-    int zc2 = Etape::makeEtape(new ZoneConstruction(Position(1050, 1050, true), benne));
+    int zc1 = Etape::makeEtape(new ZoneConstruction(Position(1000, 600, true), benne));
+    int zc2 = Etape::makeEtape(new ZoneConstruction(Position(1050, 600, true), benne));
 
 
     // Pieds
     int pa = Etape::makeEtape(new RamasserPied(Position(970,  260, true)));
     int pb = Etape::makeEtape(new RamasserPied(Position(1200, 260, true)));
 
-    // Etc.
+    // Cabines de plage
+    int cp1 = Etape::makeEtape(new Cabine(Position(250, 50, true)));
+    int cp2 = Etape::makeEtape(new Cabine(Position(500, 50, true)));
 
 
     /** Liens **/
     // [WIP]
     Etape::get(start)   ->addVoisin(wa);
     Etape::get(wa)      ->addVoisin(wb, zc2);
-    Etape::get(wa)      ->addVoisin(zc1);
-    Etape::get(c1)      ->addVoisin(wa);
-    Etape::get(wb)      ->addVoisin(zc2);
+    Etape::get(wc)      ->addVoisin(zc1);
+    Etape::get(cd1)      ->addVoisin(wa);
+    Etape::get(cp1)      ->addVoisin(wd);
+    Etape::get(cp2)      ->addVoisin(wd);
+    Etape::get(wc)      ->addVoisin(zc2);
+    Etape::get(wd)      ->addVoisin(wb);
     Etape::get(pa)      ->addVoisin(wb, zc2);
     Etape::get(pb)      ->addVoisin(pa, zc2);
 
-- 
GitLab