Newer
Older
#include "strategieV2.h"
#include "leds.h"
#include "positionPlusAngle.h"
#include "asservissement.h"
#include "memory.h"
#include "actionGoTo.h"
#include "recalibrerOdometrie.h"
#include "commandAllerA.h"
#include "ramasserVerres.h"
#include "eteindreBougies.h"
#include "marteaux.h"
#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];
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::isBlue = false;
bool StrategieV2::sharpsToCheck[SharpSensor::END_SHARP_NAME];
int StrategieV2::robotBloque = 0;
bool StrategieV2::enTrainDeRecalibrerOdometrie = false;
int StrategieV2::timer = 0;
MediumLevelAction* StrategieV2::evitement = NULL;
bool StrategieV2::tourneSurSoiMeme = false;
/*actionsToDo[0] = new RamasserVerreV2(Position(900,550));
actionsToDo[1] = new RamasserVerreV2(Position(1050,800));
actionsToDo[2] = new RamasserVerreV2(Position(900,1050));
actionsToDo[3] = new RamasserVerreV2(Position(1200,1050));
actionsToDo[4] = new RamasserVerreV2(Position(1200,550));
actionsToDo[5] = new RamasserVerreV2(Position(1350,800));
actionsToDo[6] = new RamasserVerreV2(Position(1650,800));
actionsToDo[7] = new RamasserVerreV2(Position(1800,1050));
actionsToDo[8] = new RamasserVerreV2(Position(1800,550));
actionsToDo[9] = new RamasserVerreV2(Position(1950,800));
actionsToDo[10] = new RamasserVerreV2(Position(2100,550));
actionsToDo[11] = new RamasserVerreV2(Position(2100,1050));*/
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++)
{
sharpsToCheck[i] = false;
}
//actionsToDo[0] = new Evitement();
//actionsToDo[0] = new ActionGoTo(Position(1000,230), false);
int decalage = 0;
actionsToDo[0] = new EteindreBougies(isBlue); // action d'éteindre les bougies
//actionsToDo[1] = new ActionGoTo(Position(400,1000), true);
//actionsToDo[1] = new RecalibrerOdometrie(); //EteindreBougies(isBlue); // action d'éteindre les bougies
actionsToDo[1] = new RamasserVerres(); //new ActionGoTo(Position(300,1300,StrategieV2::getIsBlue()), true); // //action de ramasser les verres
//actionsToDo[0] = new ActionGoTo(Position(1200,550), false);//RamasserVerreV2(Position(900,550));
//actionsToDo[1] = new ActionGoTo(Position(1200,700), false); // (400,230
/*actionsToDo[2] = new ActionGoTo(Position(350,550), false); // (400,230
//actionsToDo[2] = new ActionGoTo(Position(2600,400), false);
actionsToDo[3] = new ActionGoTo(Position(2600,550), false);
actionsToDo[4] = new ActionGoTo(Position(300, 550), false);
actionsToDo[5] = new ActionGoTo(Position(2600,550), true);
actionsToDo[6] = new ActionGoTo(Position(2600,800), false);
actionsToDo[7] = new ActionGoTo(Position(300, 800), false);
actionsToDo[8] = new ActionGoTo(Position(2600,800), true);
actionsToDo[9] = new ActionGoTo(Position(2600,1070), false);
actionsToDo[10] = new ActionGoTo(Position(300,1070), false);*/
//currentAction = new RecalibrerOdometrie(new LimitSwitchSensor(LimitSwitchSensor::BACK_LEFT, GPIO_Pin_14, GPIOD), new LimitSwitchSensor(LimitSwitchSensor::BACK_RIGHT, GPIO_Pin_1, GPIOD));
currentAction = actionsToDo[actionsCount];
//currentCommand = new CommandAllerA(currentAction->getGoalPosition());
//Asservissement::setCommand(currentCommand);
//currentAction = new RecalibrerOdometrie();
//hasJustAvoided = true;
StrategieV2::strategie = this;
uint8_t channels[10] = {9,13,8,11,5,10,4,12,14,15};
uint16_t* data = AnalogSensor::initialiserADC(10, channels);
int nbSharp = 10;
sharps[0] = new SharpSensor(SharpSensor::FRONT_LEFT, 9, data, 2000.0); // front left 9
sharps[1] = new SharpSensor(SharpSensor::FRONT_RIGHT, 13, data); // front side right 13
sharps[2] = new SharpSensor(SharpSensor::FRONT_SIDE_LEFT, 8, data); // front side left 8
sharps[3] = new SharpSensor(SharpSensor::FRONT_SIDE_RIGHT, 11, data); // avant side droite 11
sharps[4] = new SharpSensor(SharpSensor::BACK_LEFT, 5, data); // ARRIERE gauche 5
sharps[5] = new SharpSensor(SharpSensor::BACK_MIDDLE, 10, data, 2500.); // back middle 10
sharps[6] = new SharpSensor(SharpSensor::NONE, 4, data); //
sharps[7] = new SharpSensor(SharpSensor::ELEVATOR_TOP, 12, data); // capteur haut ascenseur 12
sharps[8] = new SharpSensor(SharpSensor::ELEVATOR_DOWN, 14, data); // capteur bas ascenseur 14
sharps[9] = new SharpSensor(SharpSensor::BACK_RIGHT, 15, data, 2000.0); //
emptySharpsToCheck();
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);
}
StrategieV2::~StrategieV2()
{
//dtor
}
void StrategieV2::update()
{
if (StrategieV2::strategie == NULL)
return;
updateCount++;
if (updateCount < 0)
{
updateCount = 50000;
}
if ((updateCount / 100) % 2)
{
allumerLED2();
eteindreLED();
}
else
{
eteindreLED2();
{
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
}
//if (updateCount > 6000 && updateCount < 10000) // attendre 15 secondes
/*{
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
}*/
if (actionsCount >= 2)
return;
if (robotBloque > 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);
*/
//std::cout << "update" << std::endl;
// check sensors:
//allumerLED2();
for (int i = 0; i < SharpSensor::END_SHARP_NAME; i++) // update tous les sharps
if (sharpsToCheck[i] && !tourneSurSoiMeme && i != SharpSensor::BACK_RIGHT)
//if (sharps[9]->getValue().b)
//{
//allume = true;
//}
if (allume) {
allumerLED();
}
else {
eteindreLED();
}
//return;
//somethingDetected = Sensors::getSensors()->sharpDetect();
//allumerLED2();
if (allume)
Asservissement::asservissement->setCommandSpeeds(NULL);
//allumerLED();
/*
if (true) // if (canStillDoAction)
{
if (currentAction)
{
currentAction->collisionAvoided();
currentAction->update();
}
}
hasJustAvoided = false;
somethingDetected = false;
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
// tentative d'évitement :
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_LEFT))
{
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));
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_LEFT))
{
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));
Marteaux::enfoncerBasDroit();
}
}
}*/
//if (!hasAvoided)
return;
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.00001 && abs(Asservissement::asservissement->getLinearSpeed()) > 0.1)//0.01*abs(Asservissement::asservissement->getLinearSpeed())) //*abs())
robotBloque = 0;// ++;
else
robotBloque = 0;
if (currentAction->update() == -1 || (robotBloque > 1000))// && !enTrainDeRecalibrerOdometrie))
if (robotBloque > 1000) // si le robot est bloqué 2 secondes
{
// on recule de 20 cm
Asservissement::asservissement->setCommandSpeeds(NULL);
currentCommand = NULL;
actionsCount = 2;
bool mustGoBack = true;
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)
//delete currentCommand;
//if (mustDeleteAction && currentAction != NULL)
//delete currentAction;
// 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 (mustDeleteAction) // temporary action
mustDeleteAction = false;
else
actionsCount++;
if (actionsCount == 2)
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
}
else
{
currentAction = actionsToDo[actionsCount];
}
//timeSinceLastRecalibration++;
//}
//delete action;
}
if (currentCommand)
currentCommand->update();
Asservissement::asservissement->setCommandSpeeds(currentCommand);
// updateCount ++;
eteindreLED2();
eteindreLED();
void StrategieV2::setCurrentGoal(Position goal, bool goBack, float maxSpeed)
{
if (currentCommand != NULL)
if (actionsCount == 0)
currentCommand = new CommandAllerA(goal, goBack, maxSpeed/2);
else
currentCommand = new CommandAllerA(goal, goBack, maxSpeed);
Asservissement::asservissement->setCommandSpeeds(currentCommand);
StrategieV2::emptySharpsToCheck();
if (goBack)
{
StrategieV2::sharpsToCheck[SharpSensor::BACK_LEFT] = true;
StrategieV2::sharpsToCheck[SharpSensor::BACK_MIDDLE] = true;
StrategieV2::sharpsToCheck[SharpSensor::BACK_RIGHT] = true;
}
else
{
StrategieV2::sharpsToCheck[SharpSensor::FRONT_LEFT] = true;
StrategieV2::sharpsToCheck[SharpSensor::FRONT_RIGHT] = true;
StrategieV2::sharpsToCheck[SharpSensor::FRONT_SIDE_LEFT] = true;
StrategieV2::sharpsToCheck[SharpSensor::FRONT_SIDE_RIGHT] = true;
}
}
void StrategieV2::setCurrentGoal(Position goal, Position center, float vitesse, bool goBack)
{
if (currentCommand != NULL)
delete currentCommand;
currentCommand = new CommandAllerEnArcA(goal, center, vitesse*5, goBack);
Asservissement::asservissement->setCommandSpeeds(currentCommand);
}
void StrategieV2::lookAt(Position pos)
{
if (currentCommand != NULL)
delete currentCommand;
currentCommand = new CommandTournerVers(pos); // create the command
Asservissement::asservissement->setCommandSpeeds(currentCommand); // apply it
}
void StrategieV2::addTemporaryAction(MediumLevelAction* action)
{
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
}
void StrategieV2::setIsBlue(bool blue)
{
isBlue = blue;
}
bool StrategieV2::getIsBlue()
{
return isBlue;
}
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::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;
}