add missing files

stm32/include/asservissement/commandSSA.h

+#ifndef COMMAND_SSA_H
+#define COMMAND_SSA_H
+
+#include "positionPlusAngle.h"
+#include "math.h"
+
+#ifdef USE_IOSTREAM
+#include <iostream>
+#endif
+
+// DEG TO RAD
+#define M_PI 3.14159265358979323846
+#define DEGTORAD(a) (a/180.*M_PI)
+
+#define PREC_DISTANCE 30.0f
+#define PREC_ANGLE DEGTORAD(5.f)
+
+/**@brief Classe abstraite généralisant l'utilisation des commandes */
+class Command
+{
+    public:
+
+        /**@brief Constructeur par défaut. Il permet d'attribuer les commandes à l'asservissement qui fera en sorte que les commandes soient exécutées */
+        Command(PositionPlusAngle* position);
+
+        /**@brief Destructeur de la classe */
+        virtual ~Command();
+
+        /** @brief Indique si le robot est près de la position pos
+            @return true s'il est à moins de precision */
+        static bool isNear(Position pos, float precision = PREC_DISTANCE);
+
+        /** @brief Indique si le robot est proche de l'angle a
+            @return true s'il est à moins de precision */
+        static bool isLookingAt(Angle a, float precision = PREC_ANGLE);
+
+        /** @brief Indique si le robot regarde vers la position pos
+            @return true s'il est à moins de precision */
+        static bool isLookingAt(Position pos, float precision = PREC_ANGLE);
+
+
+    protected:
+        bool fromSmoothMovement;
+
+        float linSpeed, angSpeed;
+
+        bool mFinished;
+
+    private:
+        static PositionPlusAngle* currentPosition;
+
+        /** @brief Indique si le robot doit s'arrêter imédiatement ou non. */
+        static bool stop;
+
+        /** @brief Indique si la vitesse du robot doit être limitée. */
+        static bool limiter;
+
+        static bool previousWasSmooth;
+};
+
+#endif //COMMAND_SSA_H

stm32/include/strategie/strategieV2_SSA.h

+#ifndef STRATEGIEV2_SSA_H_INCLUDED
+#define STRATEGIEV2_SSA_H_INCLUDED
+
+#include "commandSSA.h"
+#include "mediumLevelAction.h"
+
+#include "strategiev3.h"
+#include "goldo2018.h"
+#include "serialComStrat.h"
+#include "serialMessage.h"
+
+#define VITESSE_LINEAIRE_MAX 100
+#define VITESSE_ANGULAIRE_MAX 100
+
+
+class StrategieV2
+{
+    public:
+        StrategieV2(bool yellow = false);
+        virtual ~StrategieV2();
+
+        static void update();
+        static Command* setCurrentGoal(Position goal, bool goBack = false, float maxSpeed = VITESSE_LINEAIRE_MAX, Angle precisionAngle = -100.00, float stopAtDistance = 0.f);
+        static void stop();
+        static Command *lookAt(Position pos, float maxSpeed = VITESSE_ANGULAIRE_MAX);
+        static Command* lookAt(Angle a, float maxSpeed = VITESSE_ANGULAIRE_MAX);
+        static void setJustAvoided(bool value);
+        static bool getJustAvoided();
+        static bool isYellow();
+        static void setYellow(bool yellow);
+        static int sendNewMission();
+        static void addTemporaryAction(MediumLevelAction* action, bool stopAfter = false);
+        static void collisionAvoided();
+
+    private:
+        static bool yellow;
+        static MediumLevelAction* actionsToDo[32];
+        static MediumLevelAction* currentAction;
+        static StrategieV2* strategie;
+        static StrategieV3* strat;
+        static SerialComStrat *serialComStrat;
+        static bool mustDeleteAction, hasToStopAfterAction;
+        static int actionsCount;
+};
+
+#endif // STRATEGIEV2_SSA_H_INCLUDED
+

stm32/src/asservissement/commandSSA.cpp

+#include "commandSSA.h"
+
+PositionPlusAngle* Command::currentPosition;
+
+Command::Command(PositionPlusAngle* position)
+{
+    this->currentPosition = position;
+}
+
+Command::~Command()
+{}
+
+bool Command::isNear(Position pos, float precision)
+{
+    Vec2d vect = pos - currentPosition->getPosition();
+
+    return (vect.getNorme() < (int) precision);
+}
+
+bool Command::isLookingAt(Angle a, float precision)
+{
+    Angle diff = a - currentPosition->angle;
+
+    while(diff >= M_PI)
+        diff -= 2.*M_PI;
+    while(diff < -M_PI)
+        diff += 2.*M_PI;
+
+    return (fabs(diff) < precision);
+}
+
+bool Command::isLookingAt(Position pos, float precision)
+{
+    Vec2d delta = pos - currentPosition->getPosition();
+    Angle angleVise = atan2(delta.getY(),delta.getX());
+    Angle diff = angleVise - currentPosition->angle;
+
+    while(diff >= M_PI)
+        diff -= 2.*M_PI;
+    while(diff < -M_PI)
+        diff += 2.*M_PI;
+
+    return (fabs(diff) < precision);
+}

stm32/src/strategie/strategieV2_SSA.cpp

+#include "strategieV2_SSA.h"
+#include "positionPlusAngle.h"
+#include "goldo2018.h"
+//#include "actionGoTo.h"
+//#include "commandAllerA.h"
+
+#ifdef QT_GUI
+    #include <QDebug>
+#endif
+//#include <iostream>
+
+#ifndef NULL
+#define NULL 0
+#endif
+
+StrategieV2* StrategieV2::strategie = NULL;
+MediumLevelAction* StrategieV2::actionsToDo[32];
+StrategieV3* StrategieV2::strat;
+SerialComStrat* StrategieV2::serialComStrat;
+bool StrategieV2::mustDeleteAction = false;
+bool StrategieV2::hasToStopAfterAction = false;
+int StrategieV2::actionsCount = 0;
+MediumLevelAction* StrategieV2::currentAction = NULL;
+
+bool StrategieV2::yellow = true;
+
+StrategieV2::StrategieV2(bool yellow)
+{
+    if(strategie == 0)
+    {
+        StrategieV2::setYellow(yellow);
+
+    #if defined(STM32F40_41xxx) || defined(STM32F10X_MD) // H405
+        //actionsToDo[0] = (MediumLevelAction*) new KrabiJunior2016(yellow);
+    #elif defined(GOLDO2018) // Goldo 2018
+        actionsToDo[0] = (MediumLevelAction*) new Goldo2018(yellow);
+    #else // Krabi 2016
+        actionsToDo[0] = (MediumLevelAction*) new Krabi2016(yellow);
+    #endif
+
+        currentAction = actionsToDo[0];
+
+        StrategieV2::strategie = this;
+    }
+}
+
+StrategieV2::~StrategieV2()
+{
+    //dtor
+}
+
+
+void StrategieV2::update()
+{
+    if (StrategieV2::strategie == NULL)
+        return;
+
+#ifndef ROBOTHW
+    //qDebug() << updateCount;
+#endif
+
+    // Update the current action
+    if(currentAction->update() == -1) {
+        // If it is finished
+
+        if (mustDeleteAction) {// temporary action
+            mustDeleteAction = false;
+        }
+        else {
+            actionsCount++;
+        }
+
+        if (actionsCount == 2)
+        {
+            // Mission over
+            return;
+        }
+        else
+        {
+            // Go back to the non-temporary actions (i.e. the main strat)
+            currentAction = actionsToDo[actionsCount];
+        }
+    }
+
+/*    if (actionsCount >= 1)
+    {
+        updateCount = 20000;
+        return;
+    }
+
+        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();
+            }
+            timeToRestart--;
+        }
+
+        else if (allume || timeToRestart) // Si un des sharp voit un adversaire, ou qu'on doit être arrêté suite à une détection
+        {
+            if(!timeToRestart)//Début de l'évitement
+            {
+                timeToRestart = 400;
+                hasJustAvoided = true;
+                hasJustAvoided = false;
+                somethingDetected = false;
+                //
+                // tentative d'évitement :
+                //Position pos = Odometrie::odometrie->getPos().getPosition();
+                //addTemporaryAction(new ActionGoTo(pos, true));
+
+            }
+    }
+        if (mustDeleteAction) // temporary action
+            mustDeleteAction = false;
+        else
+            actionsCount++;
+
+        currentAction = actionsToDo[actionsCount];
+    }
+    if(!timeToRestart)
+    {
+        if (currentCommand)
+            currentCommand->update();
+        //Asservissement::asservissement->setCommandSpeeds(currentCommand);
+    }
+    */
+}
+
+/**
+ * Update the strategy, and send the new mission
+ * @return result, the strategie's update result
+ */
+int StrategieV2::sendNewMission() {
+    Position goal = strat->getEtapeEnCours()->getPosition();
+    int mission_type = strat->getEtapeEnCours()->getEtapeType();
+
+    #ifdef USE_IOSTREAM
+    std::cout << "goal: " << goal.Print() << std::endl;
+    std::cout << "type: " << mission_type << std::endl;
+    #endif // USE_IOSTREAM
+
+    PositionPlusAngle* goalWithAngle = new PositionPlusAngle(goal, 0);
+//    serialComStrat->sendNewMission(*goalWithAngle, mission_type);
+}
+
+Command* StrategieV2::setCurrentGoal(Position goal, bool goBack, float maxSpeed, Angle precisionAngle, float stopAtDistance)
+{
+
+//    StrategieV2::setCurrentGoal(this->getGoalPosition(), false, VITESSE_LINEAIRE_MAX, -100.0, 200.f);
+
+    PositionPlusAngle* goalWithAngle = new PositionPlusAngle(goal, 0);
+    serialComStrat->sendNewMission(goalWithAngle, SerialMessage::NEW_MISSION_NO_ANGLE, goBack, false);
+    delete goalWithAngle;
+}
+
+void StrategieV2::stop()
+{
+}
+
+Command* StrategieV2::lookAt(Position pos, float maxSpeed)
+{
+
+    //Asservissement::asservissement->setCommandSpeeds(currentCommand); // apply it
+  //  StrategieV2::emptySharpsToCheck();
+
+}
+Command* StrategieV2::lookAt(Angle a, float maxSpeed)
+{
+    PositionPlusAngle* goalWithAngle = new PositionPlusAngle(Position(0, 0), a);
+    serialComStrat->sendNewMission(goalWithAngle, SerialMessage::NEW_MISSION, false, true);
+//    currentCommand = new CommandTournerVers(a, maxSpeed); // create the command
+
+    //Asservissement::asservissement->setCommandSpeeds(currentCommand); // apply it
+//    StrategieV2::emptySharpsToCheck();
+}
+
+void StrategieV2::setJustAvoided(bool avoided)
+{
+//    hasJustAvoided = avoided;
+}
+bool StrategieV2::getJustAvoided()
+{
+//    return hasJustAvoided;
+}
+
+bool StrategieV2::isYellow()
+{
+    return yellow;
+}
+
+void StrategieV2::setYellow(bool yellow)
+{
+    StrategieV2::yellow = yellow;
+}
+
+void StrategieV2::addTemporaryAction(MediumLevelAction* action, bool stopAfter)
+{
+    currentAction = action;
+    mustDeleteAction = true; // this is a temporary action that needs to be deleted
+//    hasToStopAfterAction = stopAfter;
+}
+
+void StrategieV2::collisionAvoided() {
+    currentAction->collisionAvoided();
+    actionsToDo[actionsCount]->collisionAvoided();
+}