Newer
Older
Guillaume Buret
committed
#include "cabine.h"
#include "strategieV2.h"
#include "mediumLevelAction.h"
#include "command.h"
#include "position.h"
#ifndef ROBOTHW
#include <QDebug>
#endif
Cabine::Cabine(){}
Cabine::Cabine(Position goalPosition, Position wd_position):MediumLevelAction(goalPosition)
Guillaume Buret
committed
{
goalPosition = this->goalPosition;
position_depart = wd_position;
Guillaume Buret
committed
}
Cabine::~Cabine(){}
Etape::EtapeType Cabine::getType()
{
return Etape::CABINE;
}
int Cabine::update()
{
if (status == 0) //Début
{
StrategieV2::setCurrentGoal(this->getGoalPosition(), false, VITESSE_LINEAIRE_MAX, -100.0, 200.f);
Guillaume Buret
committed
#ifndef ROBOTHW
qDebug() << "On se prepare a pousser une porte";
Guillaume Buret
committed
#endif
Guillaume Buret
committed
}
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_depart, 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_depart, 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;
}
Guillaume Buret
committed
return status;
}