Newer
Older
#include "tapis.h"
#include "strategieV2.h"
#include "mediumLevelAction.h"
#include "command.h"
#include "position.h"
Tapis::Tapis(){}
Tapis::Tapis(Position position):MediumLevelAction(position) //si cote est true on veut poser le tapis droit, si non c'est le tapis gauche
Cecile Bouette
committed
if (position == Position(1000, 750))
Cecile Bouette
committed
this->positionLookAt = Position(0, 300);
Cecile Bouette
committed
if (position == Position(1450, 750))
Cecile Bouette
committed
this->positionLookAt = Position(3000, 200);
}
Tapis::~Tapis(){}
Etape::EtapeType Tapis::getType()
{
return Etape::TAPIS;
}
int Tapis::update()
{
if (status == 0)
{
qDebug() << "tapis";
status++;
}
else if (status == 1)
{
StrategieV2::setCurrentGoal(goalPosition, goBack);
status++;
}
else if (status == 2)
{
if (Command::isNear(goalPosition))
{
StrategieV2::lookAt(positionLookAt);
status++;
}
}
else if (status == 3)
{
if (Command::isLookingAt(positionLookAt))
{
//ouvrir le bras
Cecile Bouette
committed
qDebug() << "On ouvre le bras";
BrasTapis::getSingleton(cote)->ouvrirBras();
status++;
}
}
Cecile Bouette
committed
else if ((status <= 23) && (status > -1 )) //On attend que les bras du robot s'ouvrent : 50ms par incrémentation du status
{
status++;
}
Cecile Bouette
committed
else if (status == 24)
{
//lacher les tapis
Cecile Bouette
committed
qDebug() << "On ouvre les pinces pour lacher les tapis";
BrasTapis::getSingleton(cote)->ouvrirPince();
Cecile Bouette
committed
else if ((status <= 44) && (status > -1 )) //On attend que la pince s'ouvre
{
status++;
}
Cecile Bouette
committed
else if (status == 45)
Cecile Bouette
committed
qDebug() << "On ferme les Pinces";
BrasTapis::getSingleton(cote)->fermerPince();
status++;
}
Cecile Bouette
committed
else if ((status <= 65) && (status > -1 )) //On attend que la pince se ferme
{
status++;
}
Cecile Bouette
committed
else if (status == 66)
{
//fermer bras
qDebug() << "On ferme le bras";
BrasTapis::getSingleton(cote)->fermerBras();
status++;
}
Cecile Bouette
committed
else if ((status <= 87) && (status > -1)) //On attend que les bras se ferment.
{
status++;
}
Cecile Bouette
committed
else if (status == 88)
{
#ifndef ROBOTHW
qDebug() << "Etape tapis finie";
#endif
Cecile Bouette
committed
status = -1;
}
return status;
}