Newer
Older
#include "strategiev3.h"
#include "strategieV2.h"
#include "actionGoTo.h"
#include "leds.h"
#include "odometrie.h"
#include "asservissement.h"
#include "dijkstra.h"
#include "etape.h"
#ifndef ROBOTHW
#include <QDebug>
#endif
//#include <iostream>
StrategieV3::StrategieV3(bool /*isYellow*/) : MediumLevelAction()
{
this->avoiding = false;
this->etapeEnCours = 0;
this->statusStrat=1;
this->enTrainEviterReculant = false;
this->enTrainEviterAvancant = false;
this->millisecondesRestantes = 90 * 1000;
#ifndef ROBOTHW
colorLiaisonsEtapes = QColor("blue");
colorEtapeGoal = QColor("red");
colorEtapesIntermediaires = QColor("yellow");
colorEtapes = QColor("orange");
colorTexteEtapes = QColor("black");
colorEtapesRobotVu = QColor("black");
#endif
}
int StrategieV3::update()
{
//this->actionEtape[this->etapeEnCours]->reset();
//this->actionGoto[this->etapeEnCours].reset();
//Si on est en train d'éviter, on revient à l'étape précédente, et on marque l'étape comme à éviter
if(this->avoiding)
{
Arnaud Cadot
committed
#ifndef ROBOTHW
Arnaud Cadot
committed
#endif
this->tableauEtapesTotal[this->etapeEnCours]->robotVu();
//this->tableauEtapesTotal[this->etapeEnCours]->setState(-2);
this->tableauEtapesTotal[etapeEnCours]->getParent()->setParent(this->tableauEtapesTotal[this->etapeEnCours]);
this->etapeEnCours = this->tableauEtapesTotal[etapeEnCours]->getParent()->getNumero();
//On recalcul les distances par rapport à l'étape où l'on vient d'arriver
dijkstra->setEtapeCourante(this->etapeEnCours);
if(this->enTrainEviterReculant)
{
this->enTrainEviterReculant = false;
this->enTrainEviterAvancant = true;
tableauEtapesTotal[etapeEnCours]->getActionGoTo()->setGoBack(false);
//actionEtape[etapeEnCours]->setGoBack(false);
}
else
{
this->enTrainEviterReculant = true;
this->enTrainEviterAvancant = false;
tableauEtapesTotal[etapeEnCours]->getActionGoTo()->setGoBack(true);
//actionEtape[etapeEnCours]->setGoBack(true);
}
StrategieV2::addTemporaryAction(tableauEtapesTotal[etapeEnCours]->getActionGoTo());
//StrategieV2::addTemporaryAction(actionEtape[etapeEnCours]);
//dijkstra->setEtapeCourante((this->tableauEtapesTotal[this->etapeEnCours]->getParent()->getNumero()));
if(dijkstra->run() != 0)
{
// Si run renvoit autre chose que 0, c'est que l'étape en cours a changée.
// Cela arrive pour débloquer le robot
//Etape* ancienneEtape = this->tableauEtapesTotal[this->etapeEnCours];
//this->etapeEnCours = this->tableauEtapesTotal[this->etapeEnCours]->getParent()->getNumero();
/*this->actionEtape[this->etapeEnCours]->reset();
this->actionGoto[this->etapeEnCours].reset();*/
}
//On retourne à l'étape intermédiaire précédente, en marche arrière
this->avoiding = false;
Arnaud Cadot
committed
#ifndef ROBOTHW
Arnaud Cadot
committed
#endif
//On reset toute les directions à aller en marche avant
for(int i = 0 ; i < this->nombreEtapes ; i++)
{
/*actionGoto[i].setGoBack(false);
actionEtape[i]->setGoBack(false);*/
}
this->enTrainEviterReculant = false;
this->enTrainEviterAvancant = false;
if(this->statusStrat==2)//Si on vient d'arriver à une étape intermédiare
{
this->updateIntermedaire();
}
else//Sinon, statusStrat==1, et il faut donc choisir un nouvel objectif
{
// Si on n'était pas en train d'éviter
if(!(enTrainEviterReculant || enTrainEviterAvancant))
{
//L'objectif qu'on vient de remplir est maintenant un simple point de passage
//this->tableauEtapesTotal[this->etapeEnCours]->finir();//Vieux, on utilise maintenant updateStock dans la strat de l'année en cours
//Idem pour les autres étapes correspondant au même objectif
for(int etapeLiee = 0 ; etapeLiee < this->tableauEtapesTotal[this->etapeEnCours]->getNombreEtapesLieesParFinirEtape() ; etapeLiee++)
int numeroEtapeLiee = this->tableauEtapesTotal[this->etapeEnCours]->getEtapesLieesParFinirEtape()[etapeLiee];
this->tableauEtapesTotal[numeroEtapeLiee]->finir();
//Mise à jour du stock et l'objectif qu'on vient de remplir est maintenant un simple point de passage
//On est maintenant arrivé à l'étape de fin de l'action (en général c'est la même étape, mais pas toujours, ex : les claps de 2015)
this->etapeEnCours = this->tableauEtapesTotal[this->etapeEnCours]->getNumeroEtapeFinAction() == -1
: this->tableauEtapesTotal[this->etapeEnCours]->getNumeroEtapeFinAction();
}
int score = 0;
bool resteDesChosesAFaire = updateScores();
// S'il n'y a plus d'objectif dans tout le graphe, on se replit vers une position où on ne bloque pas l'adversaire.
// Sinon, il y a risque de prendre un avertissement pour anti-jeu (évité de peu pour le premier match de Krabi 2014)
if(!resteDesChosesAFaire)
{
for(int i = 0 ; i < this->nombreEtapes ; i++)
{
}
resteDesChosesAFaire = updateScores();
//S'il n'y a VRAIMENT plus rien à faire
if(!resteDesChosesAFaire)
{
//Si on est au garage, on s'arrête
if(this->etapeEnCours == this->numeroEtapeGarage)
{
this->statusStrat=-1;//Plus rien à faire, on passe à l'action suivante de stratégieV2
#ifndef ROBOTHW
qDebug() << "Fin de StrategieV3";
#endif
return this->statusStrat;
}
else
{
//Sinon on y va
this->tableauEtapesTotal[this->numeroEtapeGarage]->setScore(1000);
//On recalcul les distances par rapport à l'étape où l'on vient d'arriver
dijkstra->setEtapeCourante(this->etapeEnCours);
if(dijkstra->run() != 0)
{
// Si run renvoit autre chose que 0, c'est que l'étape en cours a changée.
// Cela arrive pour débloquer le robot
//Etape* ancienneEtape = this->tableauEtapesTotal[this->etapeEnCours];
//this->etapeEnCours = this->tableauEtapesTotal[this->etapeEnCours]->getParent()->getNumero();
tableauEtapesTotal[this->etapeEnCours]->getAction()->reset();
//On sélectionne l'objectif le plus prometteur : pas trop loin et qui rapporte
int meilleurEtape = -1;
int scoreMaxi = -100000;
int scoreTypeEtape = 0;
for(int i = 0 ; i < this->nombreEtapes ; i++)
scoreTypeEtape = this->tableauEtapesTotal[i]->getScore();
// score = modificateurTemporel*(10000-this->tableauEtapesTotal[i]->getDistance() + scoreTypeEtape);
score = (10000-this->tableauEtapesTotal[i]->getDistance() + scoreTypeEtape);
if((scoreMaxi < score) && scoreTypeEtape && (this->tableauEtapesTotal[i]->getDistance() != -1))
if(meilleurEtape==-1)
{
if(this->etapeEnCours == this->numeroEtapeGarage)
{
this->statusStrat=-1;//Plus rien à faire, on passe à l'action suivante de stratégieV2
return this->statusStrat;
}
else
{
meilleurEtape = this->numeroEtapeGarage;
}
this->goal = meilleurEtape;
this->statusStrat = 2;//Jusqu'à preuve du contraire, la prochaine étape est une étape intermédiaire
this->updateIntermedaire();//On y va
return this->statusStrat;
}
void StrategieV3::resetEverything(){
for(int i = 0 ; i < 10 ; i++){
void StrategieV3::collisionAvoided(){
this->avoiding = true;
}
void StrategieV3::updateIntermedaire()
{
//Note : le parent d'une étape est l'étape le rapprochant le plus de l'étape d'origine
//Ainsi, le parent du parent du parent... de n'importe quelle étape est l'étape d'origine
//(sauf peut être le parent de l'étape d'origine, mais on s'en fout
#ifndef ROBOTHW
qDebug() << "updateIntermedaire\n";
#endif
int etapeOuOnVientDArriver = this->etapeEnCours;
this->etapeEnCours = this->goal;
Alexandre Manoury
committed
this->nextStep = -1;
// Si la prochaine étape est le goal, alors au prochain update il faudra trouver un nouvel objectif -> status = 1;
if(((this->tableauEtapesTotal[this->etapeEnCours]->getParent()->getNumero())) == etapeOuOnVientDArriver)
qDebug() << "la prochaine etape est le goal\n" << etapeOuOnVientDArriver;
this->statusStrat = 1;
}
//On cherche l'etape suivant vers l'etape - but
while(((this->tableauEtapesTotal[this->etapeEnCours]->getParent()->getNumero())) != etapeOuOnVientDArriver)
Alexandre Manoury
committed
this->nextStep = this->etapeEnCours;
this->etapeEnCours = ((this->tableauEtapesTotal[this->etapeEnCours]->getParent()->getNumero()));
StrategieV2::addTemporaryAction(tableauEtapesTotal[this->etapeEnCours]->getAction());
}
else
{
//On ajoute l'action d'aller en ligne droite vers cette étape intermédiaire
Alexandre Manoury
committed
#ifdef SMOOTH_MOVE
tableauEtapesTotal[this->etapeEnCours]->getActionGoTo()->setNextGoal(tableauEtapesTotal[this->nextStep]->getPosition());
Alexandre Manoury
committed
#endif
StrategieV2::addTemporaryAction(tableauEtapesTotal[this->etapeEnCours]->getActionGoTo());
}
}
#ifndef ROBOTHW
void StrategieV3::paint(QPainter* p)
{
for(int numeroEtape = 0 ; numeroEtape<this->nombreEtapes ; numeroEtape++)
this->tableauEtapesTotal[numeroEtape]->getPosition().getX(),
this->tableauEtapesTotal[numeroEtape]->getPosition().getY());
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
// Affichage des étapes
p->setPen(colorEtapesIntermediaires);//"orange"));
p->setBrush(colorEtapesIntermediaires);//"orange"));
p->setOpacity(1.0f);
p->drawEllipse(position,10,10);
// Etape - but en surbrillance
if(numeroEtape == this->goal)
{
p->setPen(colorEtapeGoal);
p->setBrush(colorEtapeGoal);
p->setOpacity(1.0f);
p->drawEllipse(position,30,30);
}
// Etape actuelle en surbrillance
if(numeroEtape == this->etapeEnCours)
{
p->setPen(colorEtapesIntermediaires);
p->setBrush(colorEtapesIntermediaires);
p->setOpacity(1.0f);
p->drawEllipse(position,25,25);
}
// Etapes où on a vu un robot
if(this->tableauEtapesTotal[numeroEtape]->aEviter())
{
p->setPen(colorEtapesRobotVu);
p->setBrush(colorEtapesRobotVu);
p->setOpacity(1.0f);
p->drawEllipse(position,20,20);
}
//Affichage du numéro des étapes
QFont font;
font.setPixelSize(50);
p->setFont(font);
p->setOpacity(1);
p->setPen(colorTexteEtapes);
p->setBrush(colorTexteEtapes);
p->drawText(position, QString::number(this->tableauEtapesTotal[numeroEtape]->getNumero()));
for(int numeroChild = 0 ; numeroChild < this->tableauEtapesTotal[numeroEtape]->getNbChildren() ; numeroChild++)
{
// Affichages de liaisons
// QPoint positionChild = QPoint(
// this->tableauEtapesTotal[numeroEtape]->getChildren()[numeroChild]->getPosition().getX(),
// -(this->tableauEtapesTotal[numeroEtape]->getChildren()[numeroChild]->getPosition().getY()));
// Affichage des demi-liaisons, pour mieux voir les liens mono-directionnels
QPoint positionChild = QPoint(
(this->tableauEtapesTotal[numeroEtape]->getChildren()[numeroChild]->getPosition().getX()+this->tableauEtapesTotal[numeroEtape]->getPosition().getX())/2,
(this->tableauEtapesTotal[numeroEtape]->getChildren()[numeroChild]->getPosition().getY()+this->tableauEtapesTotal[numeroEtape]->getPosition().getY())/2);
p->setOpacity(0.5f);
p->setPen(colorLiaisonsEtapes);
p->setBrush(colorLiaisonsEtapes);
p->drawLine(position.x(), position.y(),positionChild.x(),positionChild.y());
p->drawEllipse(position,10,10);
}
//Affichage du type d'étape
QPoint positionTypeEtape = QPoint(
this->tableauEtapesTotal[numeroEtape]->getPosition().getX(),
this->tableauEtapesTotal[numeroEtape]->getPosition().getY() + 50);
p->setOpacity(0.5);
p->setPen(colorTexteEtapes);
p->setBrush(colorTexteEtapes);
p->drawText(positionTypeEtape, Etape::getShortNameType(this->tableauEtapesTotal[numeroEtape]->getEtapeType()));
void StrategieV3::startDijkstra() {
this->dijkstra = new Dijkstra(tableauEtapesTotal, this->nombreEtapes);
tableauEtapesTotal[0]->setParent(tableauEtapesTotal[0]);// Evite de planter si on detecte dès la première boucle (dans le simu)
}
void StrategieV3::updateStock(){
switch(this->tableauEtapesTotal[this->etapeEnCours]->getEtapeType()){
case Etape::DEPART :
this->tableauEtapesTotal[this->etapeEnCours]->setEtapeType(Etape::POINT_PASSAGE);
break;
default:
this->tableauEtapesTotal[this->etapeEnCours]->setEtapeType(Etape::POINT_PASSAGE);
bool StrategieV3::updateScores() {
//Mise à jour du score correspondant à chaque étape
//CETTE PARTIE EST A AMELIORER
//on pourrait prendre en compte :
//le fait le transporter un feu, ce qui active les objectifs de dépose done
//le temps que prend chaque action
//le temps restant done
//la trajectoire de notre autre robot
//...
//C'est aussi utilisé pour savoir si on est dans un sous-graphe sans objectif,
//et qu'il faut oublier qu'on a vu des robots
int scoreTypeEtape = 0;
//float modificateurTemporel = 1.f;
bool resteDesChosesAFaire = false;
for(int i = 0 ; i < this->nombreEtapes ; i++)
{
scoreTypeEtape=this->getScoreEtape(i);
if(scoreTypeEtape)
{
resteDesChosesAFaire = true;
}
qDebug() << "Etape " << i << " Score: " << scoreTypeEtape;
this->tableauEtapesTotal[i]->setScore(scoreTypeEtape);