diff --git a/stm32/src/strategie/cabine.cpp b/stm32/src/strategie/cabine.cpp index 0679224cf952c38e16e19cd33dc151d56b5f127a..3fdb28003819a7cc519d34d6f064711636da7e02 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 7a3e4b65ca221e9207adebc3947ba311d082c258..b548842da0c2da95f0da2873420c45660b0b652e 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 03bc24a6959a707ba1406fc78960e631337ba1d2..4e2f6aea8f2f8f37cb5887c5943aeb05cdae59d8 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);