Newer
Older
#include "strategieV2.h"
#include "leds.h"
#include "positionPlusAngle.h"
#include "asservissement.h"
Alexandre Manoury
committed
#include "memory.h"
#include "actionGoTo.h"
#include "recalibrerOdometrie.h"
Alexandre Manoury
committed
#include "commandAllerA.h"
Alexandre Manoury
committed
#include "commandAllerEnArcA.h"
#include "commandTournerVers.h"
Victor Dubois
committed
Victor Dubois
committed
#include "remote.h"
#endif
Alexandre Manoury
committed
#include "tourelle.h"
#include "tirette.h"
Alexandre Manoury
committed
#include <QDebug>
//#include <iostream>
#ifndef NULL
#define NULL 0
#endif
StrategieV2* StrategieV2::strategie = NULL;
int StrategieV2::updateCount = 0;
Command* StrategieV2::currentCommand = NULL;
MediumLevelAction* StrategieV2::currentAction = NULL;
int StrategieV2::actionsCount = 0;
MediumLevelAction* StrategieV2::actionsToDo[32];
Victor Dubois
committed
SharpSensor** StrategieV2::sharps;
bool StrategieV2::hasToGoBase = false;
bool StrategieV2::hasJustAvoided = false;
bool StrategieV2::mustDeleteAction = false;
int StrategieV2::glassGathered = 0;
int StrategieV2::timeSinceLastRecalibration = 0;
bool StrategieV2::somethingDetected = false;
bool StrategieV2::sharpsToCheck[SharpSensor::END_SHARP_NAME];
int StrategieV2::robotBloque = 0;
bool StrategieV2::enTrainDeRecalibrerOdometrie = false;
int StrategieV2::timer = 0;
MediumLevelAction* StrategieV2::evitement = NULL;
int StrategieV2::timeToRestart = 0;
#ifdef ROBOTHW
//Tourelle* StrategieV2::tourelle = NULL;
//int StrategieV2::hysteresisTourelle = 0;
StrategieV2::StrategieV2(bool yellow)
if(strategie == 0)
{
StrategieV2::setYellow(yellow);
timeToRestart = 0;
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++)
sharpsToCheck[i] = false;
#if defined(STM32F40_41xxx) || defined(STM32F10X_MD) // H405
//actionsToDo[0] = (MediumLevelAction*) new KrabiJunior2016(yellow);
actionsToDo[0] = (MediumLevelAction*) new Krabi2016(yellow);
currentAction = actionsToDo[actionsCount];
Sensors* sensors = Sensors::getSingleton();
sharps = sensors->getSharpSensorsList();
}
StrategieV2::~StrategieV2()
{
//dtor
}
long StrategieV2::getTimeSpent()
{
return updateCount * 5;
}
void StrategieV2::resetTime()
{
updateCount = 0;
}
void StrategieV2::update()
{
/*#ifndef NO_REMOTE
if (!currentAction)
{
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
}
if (StrategieV2::strategie == NULL)
return;
if(currentAction)
currentAction->updateTime(90*1000-updateCount*5);
if (updateCount < 0)
{
updateCount = 50000;
}
if(updateCount > 18200 && updateCount < 18210)
//if(updateCount > 3000 && updateCount < 3010)
{
//On tire
CanonFilet::getSingleton()->shoot();
#ifndef ROBOTHW
qDebug() << "CanonFilet::getSingleton()->shoot();";
#endif
}
#endif
#ifndef ROBOTHW
//qDebug() << updateCount;
#endif
// if (updateCount > 677)//684)
// {
// updateCount++;
// updateCount--;
// }
/*if ((updateCount / 100) % 2)
{
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
}
//if (updateCount > 6000 && updateCount < 10000) // attendre 15 secondes
/*{
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
if (actionsCount >= 1)
{
updateCount = 20000;
}
/*if (robotB1loque > 1000)
return;*/
/*
enableSharp(SharpSensor::FRONT_LEFT);
enableSharp(SharpSensor::FRONT_RIGHT);
enableSharp(SharpSensor::FRONT_SIDE_LEFT);
enableSharp(SharpSensor::FRONT_SIDE_RIGHT);
enableSharp(SharpSensor::BACK_LEFT);
enableSharp(SharpSensor::BACK_MIDDLE);
enableSharp(SharpSensor::BACK_RIGHT);
*/
// check sensors:
AnalogSensor::startConversion();
//
//allumerLED2();
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++) // update tous les sharps
{
sharps[i]->updateValue();
}
//sharps[9]->updateValue();
bool allume = false;
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++)
{
if(Odometrie::odometrie->getPos().getPosition().getY()>1600)//Si on est en train de faire les claps
{
allume = false;//true;//Sharps desactivés
}
else{
allume = true;
}
}
float values[10] = {0};
bool detected[10] = {false};
values[i] = sharps[i]->getValue().f;
detected[i] = sharps[i]->getValue().b;
/*if (i!=5 && i!=7 && detected[i])
allume = true;*/
}
//allume = false;
//bool allume = false;
//if (sharps[9]->getValue().b)
//{
//allume = true;
//}
//Remote::getSingleton()->update();
/*if (Remote::getSingleton()->isRemoteMode())
{
updateCount = 10;
Led::setOff(1);
}
else
{*/
Led::setOn(1);
if (allume)
{
Asservissement::asservissement->setCommandSpeeds(NULL);
Asservissement::asservissement->update();
allumerLED2();
#ifndef NO_REMOTE
//Remote::getSingleton()->log("Detect !");
#endif
#ifndef NO_REMOTE
//Remote::getSingleton()->log("No !");
#endif
}
//return;
//somethingDetected = Sensors::getSensors()->sharpDetect();
//allumerLED2();
//allume = true;//(updateCount%30==0);
//allume = false;
if(timeToRestart)
{
timeToRestart--;
}
if(timeToRestart == 1)//Dernière boucle d'évitement avant de repartir
{
if (currentAction)
{
//Pour changer de trajectoire, décommenter les lignes suivantes
currentAction->collisionAvoided();
actionsToDo[actionsCount]->collisionAvoided();
// currentCommand->collisionAvoided();
currentAction->update();
Position pos = Odometrie::odometrie->getPos().getPosition();
addTemporaryAction(new ActionGoTo(pos, true));
//On arrête le robot
if (currentCommand)
currentCommand->resetSpeeds();
else if (allume || timeToRestart) // Si un des sharp voit un adversaire, ou qu'on doit être arrêté suite à une détection
timeToRestart = 400;
hasJustAvoided = true;
//Asservissement::asservissement->setCommandSpeeds(NULL); // On s'arrête
//allumerLED();
// if (true) // if (canStillDoAction)
// {
// }
hasJustAvoided = false;
somethingDetected = false;
//
// tentative d'évitement :
//Position pos = Odometrie::odometrie->getPos().getPosition();
//addTemporaryAction(new ActionGoTo(pos, true));
Asservissement::asservissement->setCommandSpeeds(NULL);
Asservissement::asservissement->resetAsserv();
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
//Asservissement::asservissement->setCommandSpeeds(currentCommand);
/*
float currentAngle = wrapAngle(Odometrie::odometrie->getPos().getAngle());
Position pos = Odometrie::odometrie->getPos().getPosition();
Position newPos = pos + Position(-200*cos(currentAngle), -200*sin(currentAngle));
Position newPos2 = pos + Position(200*cos(currentAngle), 200*sin(currentAngle));
bool hasAvoided = false;
if (newPos.getX() > 300 && newPos.getX() < 2700 && newPos.getY() > 300 && newPos.getY() < 1800)
{
Position vect2 = newPos - Position(1500,2000);
if (vect2.getNorme() > 600)
{
bool avance = false;
bool bouge = false;
if (sharpDetects(SharpSensor::FRONT_LEFT) || sharpDetects(SharpSensor::FRONT_RIGHT) || sharpDetects(SharpSensor::FRONT_SIDE_LEFT) || sharpDetects(SharpSensor::FRONT_SIDE_RIGHT))
{
bouge = true;
avance = false;
}
if (sharpDetects(SharpSensor::BACK_LEFT) || sharpDetects(SharpSensor::BACK_RIGHT) || sharpDetects(SharpSensor::BACK_MIDDLE))
{
bouge = false;
}
if (bouge)
{
hasAvoided = true;
addTemporaryAction(new ActionGoTo(newPos, !avance));
#ifndef ROBOTHW
qDebug() << "newPos" << updateCount << "\n";
#endif
Marteaux::enfoncerBasDroit();
}
}
}
if (newPos2.getX() > 300 && newPos2.getX() < 2700 && newPos2.getY() > 300 && newPos2.getY() < 1800 && !hasAvoided)
{
Position vect2 = newPos2 - Position(1500,2000);
if (vect2.getNorme() > 600)
{
bool avance = false;
bool bouge = false;
if (sharpDetects(SharpSensor::FRONT_LEFT) || sharpDetects(SharpSensor::FRONT_RIGHT) || sharpDetects(SharpSensor::FRONT_SIDE_LEFT) || sharpDetects(SharpSensor::FRONT_SIDE_RIGHT))
{
bouge = true;
avance = false;
}
if (sharpDetects(SharpSensor::BACK_LEFT) || sharpDetects(SharpSensor::BACK_RIGHT) || sharpDetects(SharpSensor::BACK_MIDDLE))
{
bouge = false;
}
if (bouge)
{
hasAvoided = true;
addTemporaryAction(new ActionGoTo(newPos2, avance));
#ifndef ROBOTHW
qDebug() << "newPos2" << updateCount << "\n";
#endif
Marteaux::enfoncerBasDroit();
}
}
}
if (!hasAvoided)
{
allumerLED();
return;
}// */
// }
// else
// {
//// if (hasJustAvoided == true)
//// {
//// hasJustAvoided = false;
// if (currentCommand)
// currentCommand->resetSpeeds();
//// }
// }
//allumerLED2();
//currentCommand->update();
//std::cout << "updating action" << std::endl;
/*if (abs(Odometrie::odometrie->getVitesseLineaire()) < 0.0001 && abs(Odometrie::odometrie->getVitesseLineaire()) < 0.0001 && abs(Asservissement::asservissement->getLinearSpeed()) > 0.1)//0.01*abs(Asservissement::asservissement->getLinearSpeed())) // *abs())
{
//robotBloque++;// ++;
//std::cout << robotBloque << "\n";
}*/
if (currentAction->update() == -1 || (robotBloque > 50))// && !enTrainDeRecalibrerOdometrie))
if (robotBloque > 1000) // si le robot est bloqué 2 secondes
{
// on recule de 20 cm
Asservissement::asservissement->setCommandSpeeds(NULL);
Asservissement::asservissement->resetAsserv();
allumerLED();
//StrategieV2::addTemporaryAction(new ActionGoTo(Odometrie::odometrie->getPos().getPosition(), mustGoBack));
}
robotBloque = 0;
//std::cout << "Changing action" << std::endl;
//Position currentPos = Odometrie::odometrie->getPos().getPosition();
// delete the current command
//if (currentCommand != NULL)
// must go to base :
{
currentAction = new RamenerVerres(Position(300,currentPos.getY()));
hasToGoBase = false;
mustDeleteAction = true;
/*else if (timeSinceLastRecalibration % 3 == 2)
{
currentAction = new RecalibrerOdometrie();
mustDeleteAction = true;
timeSinceLastRecalibration++;
}*/
//else
//{
if (hasToStopAfterAction)
{
hasToStopAfterAction = false;
Asservissement::asservissement->stop();
}
if (mustDeleteAction) // temporary action
mustDeleteAction = false;
Asservissement::asservissement->setCommandSpeeds(NULL);
Asservissement::asservissement->resetAsserv();
return;
}
else
{
currentAction = actionsToDo[actionsCount];
}
if(!timeToRestart)
{
if (currentCommand)
currentCommand->update();
Asservissement::asservissement->setCommandSpeeds(currentCommand);
}
Command* StrategieV2::setCurrentGoal(Position goal, bool goBack, float maxSpeed, Angle precisionAngle, float stopAtDistance)
{
if (currentCommand != NULL)
Alexandre Manoury
committed
// if (actionsCount == 0)
// currentCommand = new CommandAllerA(goal, goBack, maxSpeed/2);
// else
currentCommand = new CommandAllerA(goal, goBack, maxSpeed, 0.0f, precisionAngle, stopAtDistance);
Asservissement::asservissement->setCommandSpeeds(currentCommand);
//StrategieV2::sharpsToCheck[SharpSensor::BACK_LEFT] = true;
//StrategieV2::sharpsToCheck[SharpSensor::LEFT_BACK] = true;
StrategieV2::sharpsToCheck[SharpSensor::BACK_RIGHT] = true;
StrategieV2::sharpsToCheck[SharpSensor::FRONT_LEFT] = false;
//StrategieV2::sharpsToCheck[SharpSensor::RIGHT_BACK] = true;
{
StrategieV2::sharpsToCheck[SharpSensor::FRONT_LEFT] = true;
StrategieV2::sharpsToCheck[SharpSensor::BACK_RIGHT] = false;
//StrategieV2::sharpsToCheck[SharpSensor::FRONT_RIGHT] = true;
//StrategieV2::sharpsToCheck[SharpSensor::LEFT_FRONT] = true;
//StrategieV2::sharpsToCheck[SharpSensor::RIGHT_FRONT] = true;
}*/
#endif
Alexandre Manoury
committed
return currentCommand;
}
Command* StrategieV2::setCurrentGoalSmooth(Position goal, Position nextGoal, float smoothFactor, bool goBack, float maxSpeed, Angle precisionAngle)
{
if (currentCommand != NULL)
delete currentCommand;
CommandAllerA* command = new CommandAllerA(goal, goBack, maxSpeed, 0.0f, precisionAngle);
command->smoothMovement(nextGoal, smoothFactor);
currentCommand = command;
Asservissement::asservissement->setCommandSpeeds(currentCommand);
StrategieV2::emptySharpsToCheck();
return currentCommand;
Alexandre Manoury
committed
Command* StrategieV2::setCurrentGoal(Position goal, Position center, float vitesse, bool goBack, Angle /*precisionAngle*/)
{
if (currentCommand != NULL)
currentCommand = new CommandAllerEnArcA(goal, center, vitesse*5, goBack);
Asservissement::asservissement->setCommandSpeeds(currentCommand);
Alexandre Manoury
committed
return currentCommand;
Alexandre Manoury
committed
void StrategieV2::stop()
{
if (currentCommand != NULL)
delete currentCommand;
currentCommand = NULL;
}
Alexandre Manoury
committed
Command* StrategieV2::lookAt(Position pos, float maxSpeed)
{
if (currentCommand != NULL)
delete currentCommand;
currentCommand = new CommandTournerVers(pos, maxSpeed); // create the command
Asservissement::asservissement->setCommandSpeeds(currentCommand); // apply it
StrategieV2::emptySharpsToCheck();
Alexandre Manoury
committed
return currentCommand;
Alexandre Manoury
committed
Command* StrategieV2::lookAt(Angle a, float maxSpeed)
{
if (currentCommand != NULL)
delete currentCommand;
currentCommand = new CommandTournerVers(a, maxSpeed); // create the command
Asservissement::asservissement->setCommandSpeeds(currentCommand); // apply it
Alexandre Manoury
committed
return currentCommand;
void StrategieV2::addTemporaryAction(MediumLevelAction* action, bool stopAfter)
{
currentAction = action;
mustDeleteAction = true; // this is a temporary action that needs to be deleted
}
void StrategieV2::setJustAvoided(bool avoided)
{
hasJustAvoided = avoided;
}
bool StrategieV2::getJustAvoided()
{
return hasJustAvoided;
}
bool StrategieV2::willCollide()
{
somethingDetected = true;
Asservissement::asservissement->setCommandSpeeds(NULL); // stoppe le robot
Asservissement::asservissement->resetAsserv();
void StrategieV2::setYellow(bool yellow)
void StrategieV2::gatherGlass()
{
glassGathered++;
if (glassGathered > 0)
{
glassGathered = 0;
hasToGoBase = true;
}
}
void StrategieV2::setEnTrainDeRecalibrer(bool recalibre)
{
enTrainDeRecalibrerOdometrie = recalibre;
}
void StrategieV2::emptySharpsToCheck()
{
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++)
sharpsToCheck[i] = false;
}
void StrategieV2::enableSharp(SharpSensor::SharpName name)
{
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++)
if (sharps[i]->getName() == name)
sharpsToCheck[i] = true;
}
void StrategieV2::enableSharpsGroup(bool front)
{
emptySharpsToCheck();
if (front)
{
#if defined(STM32F40_41xxx)
enableSharp(SharpSensor::FRONT_LEFT);
enableSharp(SharpSensor::FRONT_RIGHT);
enableSharp(SharpSensor::FRONT_MIDDLE);
#else
enableSharp(SharpSensor::FRONT_LEFT);
enableSharp(SharpSensor::FRONT_RIGHT);
enableSharp(SharpSensor::FRONT_LEFT_SIDE);
enableSharp(SharpSensor::FRONT_RIGHT_SIDE);
#endif
}
else
{
#if defined(STM32F40_41xxx)
enableSharp(SharpSensor::BACK_LEFT);
enableSharp(SharpSensor::BACK_RIGHT);
enableSharp(SharpSensor::BACK_MIDDLE);
#else
enableSharp(SharpSensor::BACK_LEFT);
enableSharp(SharpSensor::BACK_RIGHT);
enableSharp(SharpSensor::BACK_LEFT_SIDE);
enableSharp(SharpSensor::BACK_RIGHT_SIDE);
void StrategieV2::setCommand(Command* command)
{
currentCommand = command;
}
bool StrategieV2::sharpDetects(SharpSensor::SharpName name)
{
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++)
if (sharps[i]->getName() == name && sharpsToCheck[i] == true && !tourneSurSoiMeme)
return sharps[i]->getValue().b;
}
void StrategieV2::setTourneSurSoiMeme(bool tourne)
{
tourneSurSoiMeme = tourne;
}
SharpSensor** StrategieV2::getSensors()
{
return sharps;
}
void StrategieV2::paint(QPainter* p)
{
actionsToDo[actionsCount]->paint(p);
}
bool* StrategieV2::getSharpsToCheck()
{
return sharpsToCheck;
}