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();
#ifdef ROBOTHW
// tourelle = new Tourelle(TIM6, 0);//TIM parameter is not implemented yet
// tourelle = new Tourelle();
//tourelle->setZoneCritique(10, 27000);
#endif
}
StrategieV2::~StrategieV2()
{
//dtor
}
long StrategieV2::getTimeSpent()
{
return updateCount * 5;
}
void StrategieV2::resetTime()
{
updateCount = 0;
}
void StrategieV2::update()
{
#ifndef NO_REMOTE
Remote::getSingleton()->update();
if (!currentAction)
{
Asservissement::asservissement->setCommandSpeeds(NULL);
return;
}
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
//Tourelle* tourelle = new Tourelle(TIM6, 0);
/*
uint8_t resultZoneCritique = tourelle->setZoneCritique(10, 27000);
uint16_t resultAngle = tourelle->calculAngle(0);
// objectDetectionInstant[0] = 0;
// resultAngle = tourelle->calculAngle(0);
// objectDetectionInstant[0] = 10;
// resultAngle = tourelle->calculAngle(0);
// objectDetectionInstant[0] = 20;
// resultAngle = tourelle->calculAngle(0);
// objectDetectionInstant[0] = 40;
// resultAngle = tourelle->calculAngle(0);
bool resultUpdate = tourelle->update();
resultUpdate = tourelle->update();
nombreObjetDetecte = 1;
resultUpdate = tourelle->update();
*/
if (StrategieV2::strategie == NULL)
return;
if(currentAction)
currentAction->updateTime(90*1000-updateCount*5);
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
#ifdef ROBOTHW
// if (updateCount % 6 == 2)
// {
// EXTI_GenerateSWInterrupt(EXTI_Line2);
// }
// if (updateCount % 6 == 3)
// {
// EXTI_GenerateSWInterrupt(EXTI_Line3);
// }
//On met à jour l'indicateur de "on doit rallentir"
// bool returnTourelle = tourelle->updateSimple();
// if(returnTourelle)
// {
// //allumerLED();
// hysteresisTourelle = 100;// On ralenti pendant 0,5s mini
// }
// else if (hysteresisTourelle > 0)
// {
// hysteresisTourelle--;
// }
//
// //On donne l'ordre de ralentir, si besoin
// currentCommand->limitSpeed(hysteresisTourelle);
#endif
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();
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
//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;
}