Newer
Older
Guillaume Buret
committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
#include "cubeDebut.h"
#include "strategieV2.h"
#include "mediumLevelAction.h"
#include "command.h"
#include "position.h"
#ifndef ROBOTHW
#include <QDebug>
#endif
CubeDebut::CubeDebut(){}
CubeDebut::CubeDebut(Position goalPosition):MediumLevelAction(goalPosition)
{
goalPosition = this->goalPosition;
}
CubeDebut::~CubeDebut(){}
Etape::EtapeType CubeDebut::getType()
{
return Etape::CUBE_DEBUT;
}
int CubeDebut::update()
{
if (status == 0) //Début
{
StrategieV2::setCurrentGoal(this->getGoalPosition(), false, VITESSE_LINEAIRE_MAX, -100.0, 100.f);
#ifndef ROBOTHW
qDebug() << "Poussage des cubes devant la zone de depart";
#endif
status++;
}
else if (status == 1) {
if (Command::isNear(this->getGoalPosition(), 100.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 au bercail";
#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 bercail";
#endif
status++;
}
}
else if (status == 3) {
#ifndef ROBOTHW
qDebug() << "Etape poussage des cubes finie";
#endif
status = -1;
}
return status;
}