diff --git a/simulation/Makefile.am b/simulation/Makefile.am
index 05565af3e96fc4c5be548c53908fce93cb3cdecc..8ba7c6cc5de1570184b7922e5a338533f6d13f35 100644
--- a/simulation/Makefile.am
+++ b/simulation/Makefile.am
@@ -10,6 +10,7 @@ src/simul/main_window.cpp \
src/simul/table.cpp \
src/simul/robot.cpp \
src/asservissement.cpp \
+src/command.cpp \
src/Position.cpp \
src/PositionPlusAngle.cpp \
src/distance.cpp \
diff --git a/stm32/include/Angle.h b/stm32/include/Angle.h
index 1f8edb30426be5eefd502b3990182e50675824fd..38138ef91b5218a68e3e67b746cbe323a627aabd 100755
--- a/stm32/include/Angle.h
+++ b/stm32/include/Angle.h
@@ -21,6 +21,7 @@ class Angle
bool presqueEgales(Angle &ang);
Angle(const Angle &original);
float getValueInRadian();
+ Angle wrap();
};
diff --git a/stm32/include/asservissement.h b/stm32/include/asservissement.h
index e44e4011907a336d26c1b1ab8c64258245f217a0..f84184262e79862e908a4569a17868ebe56ab753 100755
--- a/stm32/include/asservissement.h
+++ b/stm32/include/asservissement.h
@@ -12,17 +12,12 @@
#include "capteurs.h"
#endif
#include <stdint.h>
+#include "command.h"
class Strategie;
class Asservissement{
public:
- Distance vitesse_lineaire_a_atteindre;
- Angle vitesse_angulaire_a_atteindre;
- Distance vitesse_lineaire_max;
- Angle vitesse_angulaire_max;
- Distance acceleration_lineaire;
- Angle acceleration_angulaire;
float linearDutySent, angularDutySent;
PIDFilterDistance pid_filter_distance;
PIDFilterAngle pid_filter_angle;
@@ -34,25 +29,15 @@ class Asservissement{
unsigned int buffer_collision;
unsigned int nb_echantillons_buffer_collision;
- Position destination;
- Angle destAngle;
- bool en_mouvement;
- bool direction;
- bool stop;
public:
Strategie* strategie;
short toto;
Odometrie* odometrie;
- Asservissement(Odometrie* _odemetrie);
+ Command* command;
+ Asservissement(Odometrie* _odemetrie, class Command* command);
static Asservissement * asservissement;
static const uint16_t nb_ms_between_updates;
void update(void);
- void goTo(Position position, bool stop=true);
- void goToDirection(Angle angle);
- void recule(Distance distance);
- void tourne(Angle angle);
- Distance getVitesseLineaireAfterTrapeziumFilter(Distance vitesse_lineaire_atteinte, Distance distance_restante, Angle angle_restant);
- Angle getVitesseAngulaireAfterTrapeziumFilter(Angle vitesse_angulaire_atteinte, Angle angle_restant);
};
#endif // ASSERVISSEMENT_H_INCLUDED
diff --git a/stm32/include/command.h b/stm32/include/command.h
new file mode 100644
index 0000000000000000000000000000000000000000..9b094c7e2418c72267d2b29b2fc2da733e052f72
--- /dev/null
+++ b/stm32/include/command.h
@@ -0,0 +1,41 @@
+#include "Angle.h"
+#include "PositionPlusAngle.h"
+
+class Command
+{
+private:
+ float vitesse_lineaire_a_atteindre;
+ Angle vitesse_angulaire_a_atteindre;
+ float vitesse_lineaire_max;
+ Angle vitesse_angulaire_max;
+ float acceleration_lineaire;
+ Angle acceleration_angulaire;
+
+ Position destination;
+ Angle destAngle;
+ bool en_mouvement;
+ bool direction;
+ bool stop;
+public:
+ Command();
+ void update(PositionPlusAngle positionPlusAngleActuelle, Angle vitesse_angulaire_atteinte, float vitesse_lineaire_atteinte);
+
+ void goTo(Position position, bool stop=true);
+ void goToDirection(Angle angle);
+ void recule(Distance distance);
+ void tourne(Angle angle);
+ Distance getVitesseLineaireAfterTrapeziumFilter(Distance vitesse_lineaire_atteinte, Distance distance_restante, Angle angle_restant);
+ Angle getVitesseAngulaireAfterTrapeziumFilter(Angle vitesse_angulaire_atteinte, Angle angle_restant);
+
+ float getLinearSpeed()
+ {
+ return vitesse_lineaire_a_atteindre;
+ }
+
+ Angle getAngularSpeed()
+ {
+ return vitesse_angulaire_a_atteindre;
+ }
+
+};
+
diff --git a/stm32/include/simul/robot.h b/stm32/include/simul/robot.h
index d02e09f2ec1008f8de07eb256c2ce21de362cf80..a77838eae2a327f6e3bf32641f194cc7d2b18eed 100644
--- a/stm32/include/simul/robot.h
+++ b/stm32/include/simul/robot.h
@@ -17,7 +17,7 @@ public:
PositionPlusAngle pos;
PositionPlusAngle deriv;
boost::circular_buffer<PositionPlusAngle> olds;
- class Asservissement* asservissement;
+ class Command* command;
class OdoRobot* odometrie;
class Strategie* strategie;
diff --git a/stm32/include/strategie.h b/stm32/include/strategie.h
index c968d7f00efcf8769e316742ff9235ef824c6855..f908d2f893924043f7c74dd10781fba0121de122 100755
--- a/stm32/include/strategie.h
+++ b/stm32/include/strategie.h
@@ -7,12 +7,13 @@
#include <stdint.h>
-class Asservissement;
+class Command;
+class Odometrie;
class Strategie {
private:
bool is_blue;
- Asservissement* asservissement;
+ Command* command;
void doNext();
int instruction_nb;
bool collision_detected;
@@ -20,7 +21,7 @@ class Strategie {
public:
void collisionDetected();
- Strategie(bool is_blue, Asservissement* asservissement);
+ Strategie(bool is_blue, Command* command, Odometrie* odometrie);
void foundOtherRobot();
void done();
void doNthInstruction(uint16_t n);
diff --git a/stm32/src/Angle.cpp b/stm32/src/Angle.cpp
index dd5442c949a1b651d50db06fe037d623e18d674b..6a4a9dec5823cea0bbfce3b314ad3a0ec7ac4a31 100755
--- a/stm32/src/Angle.cpp
+++ b/stm32/src/Angle.cpp
@@ -31,7 +31,12 @@ Angle Angle::operator+(const Angle &ang)
Angle Angle::operator-(const Angle &ang)
{
- double tmp_val = fmod(angle-ang.angle, 2*M_PI);
+ return Angle(angle - ang.angle);
+}
+
+Angle Angle::wrap()
+{
+ double tmp_val = fmod(angle, 2*M_PI);
if(tmp_val<0) tmp_val += M_PI*2;
return Angle(tmp_val <= M_PI ? tmp_val : tmp_val-2*M_PI);
}
diff --git a/stm32/src/asservissement.cpp b/stm32/src/asservissement.cpp
index 1fbc84322513d5693c4b9f55622d00dbc8f8adb7..bd7a90c3030c5eb1419def238811a912eb688d55 100755
--- a/stm32/src/asservissement.cpp
+++ b/stm32/src/asservissement.cpp
@@ -38,30 +38,16 @@ float angle[DBG_SIZE];
Asservissement * Asservissement::asservissement = NULL;
const uint16_t Asservissement::nb_ms_between_updates = 10;
uint32_t dbgInc = 0;
-Asservissement::Asservissement(Odometrie* _odometrie) :
- vitesse_lineaire_a_atteindre(0),
- vitesse_angulaire_a_atteindre(0),
-#if 0
- vitesse_lineaire_max(4.), // en mm par nb_ms_between_updates
- vitesse_angulaire_max(M_PI/100.0), // en radian par nb_ms_between_updates
- acceleration_lineaire(4./200.0), // en mm par nb_ms_between_updates
- acceleration_angulaire((M_PI/100.0)/200.0), // en radian par nb_ms_between_updates
-#else
- vitesse_lineaire_max(4.), // en mm par nb_ms_between_updates
- vitesse_angulaire_max(M_PI/500.0), // en radian par nb_ms_between_updates
- acceleration_lineaire(4./200.0), // en mm par nb_ms_between_updates
- acceleration_angulaire((M_PI/100.0)/200.0), // en radian par nb_ms_between_updates
-#endif
+Asservissement::Asservissement(Odometrie* _odometrie, Command* command) :
seuil_collision(3.5),
buffer_collision(0xffffffff),
- nb_echantillons_buffer_collision(10),
- destination(_odometrie->getPos().position)
+ nb_echantillons_buffer_collision(10)
{
odometrie = _odometrie;
+ this->command = command;
toto = 0;
linearDutySent = 0;
angularDutySent = 0;
- en_mouvement = false;
Asservissement::asservissement = this;
#ifdef ROBOTHW
@@ -85,8 +71,14 @@ void Asservissement::update(void)
//PositionPlusAngle before(odometrie.positionPlusAngle);
asserCount++;
- if(asserCount < 87000)
+ if(asserCount > 87000)
{
+#ifdef ROUES
+ roues.gauche.tourne(0);
+ roues.droite.tourne(0);
+#endif
+ strategie->theEnd();
+ }
#ifdef CAPTEURS
capteurs.startConversion();
@@ -97,39 +89,35 @@ void Asservissement::update(void)
Angle vitesse_angulaire_atteinte = odometrie->getVitesseAngulaire();
Distance vitesse_lineaire_atteinte = odometrie->getVitesseLineaire();
+ command->update(positionPlusAngleActuelle, vitesse_angulaire_atteinte, vitesse_lineaire_atteinte);
+
buffer_collision <<= 1;
- bool tmp = (fabs((vitesse_lineaire_atteinte - vitesse_lineaire_a_atteindre)) < seuil_collision);
+ bool tmp = (fabs((vitesse_lineaire_atteinte - command->getLinearSpeed())) < seuil_collision);
buffer_collision |= tmp;
if (buffer_collision << (16 - nb_echantillons_buffer_collision)) {
// Pas collision !!!
- Position vecteur_pos_actuelle_pos_arrivee = destination-positionPlusAngleActuelle.position;
-
- Distance distance_restante = (vecteur_pos_actuelle_pos_arrivee).getNorme();
- Angle angle_restant = (direction ? destAngle : vecteur_pos_actuelle_pos_arrivee.getAngle()) - positionPlusAngleActuelle.angle;
-#ifdef CAPTEURS
- bool is_there_someone_in_front = capteurs.getValue(Capteurs::AvantGauche) || capteurs.getValue(Capteurs::AvantDroite) || capteurs.getValue(Capteurs::Avant);
-#else
- bool is_there_someone_in_front = false;
-#endif
+//#ifdef CAPTEURS
+// bool is_there_someone_in_front = capteurs.getValue(Capteurs::AvantGauche) || capteurs.getValue(Capteurs::AvantDroite) || capteurs.getValue(Capteurs::Avant);
+//#else
+// bool is_there_someone_in_front = false;
+//#endif
//is_there_someone_in_front = false;
//On s’arrête si l’on a atteint la cible ou s’il y a le robot adverse devant
- if((distance_restante > Distance(20) && !is_there_someone_in_front && !direction) || (direction && fabs(angle_restant.getValueInRadian()) > 0.1))
- {
- if( (min(fabs(angle_restant.getValueInRadian()), fabs((angle_restant-Angle(M_PI)).getValueInRadian()))< M_PI_4) && !direction)
+ //if((distance_restante > Distance(20) && !is_there_someone_in_front && !direction) || (direction && fabs(angle_restant.getValueInRadian()) > 0.1))
+ //{
+ //if( (min(fabs(angle_restant.getValueInRadian()), fabs((angle_restant-Angle(M_PI)).getValueInRadian()))< M_PI_4) && !direction)
{
- vitesse_lineaire_a_atteindre = getVitesseLineaireAfterTrapeziumFilter(vitesse_lineaire_atteinte, distance_restante, angle_restant);
#ifdef ROUES
float linear_duty_component = pid_filter_distance.getFilteredValue(vitesse_lineaire_a_atteindre-/*blah*/vitesse_lineaire_atteinte);
linearDutySent += linear_duty_component;
#endif
}
- else
- linearDutySent=0;
+ //else
+ // linearDutySent=0;
- vitesse_angulaire_a_atteindre = getVitesseAngulaireAfterTrapeziumFilter(vitesse_angulaire_atteinte, angle_restant);
#ifdef ROUES
float angular_duty_component = pid_filter_angle.getFilteredValue(vitesse_angulaire_a_atteindre-/*didou*/vitesse_angulaire_atteinte);
@@ -159,7 +147,7 @@ void Asservissement::update(void)
roues.gauche.tourne(min(max(linearDutySent-angularDutySent, -0.9),0.9));
roues.droite.tourne(min(max(linearDutySent+angularDutySent, -0.9),0.9));
#endif
- }
+ /*}
else
{
if(stop || is_there_someone_in_front)
@@ -179,7 +167,7 @@ void Asservissement::update(void)
en_mouvement = false;
strategie->done();
}
- }
+ }*/
}
else // collision
@@ -188,111 +176,102 @@ void Asservissement::update(void)
roues.gauche.tourne(0);
roues.droite.tourne(0);
#endif
- vitesse_lineaire_a_atteindre = Distance(0);
- vitesse_angulaire_a_atteindre = Angle(0);
+ //vitesse_lineaire_a_atteindre = Distance(0);
+ //vitesse_angulaire_a_atteindre = Angle(0);
linearDutySent = 0;
angularDutySent = 0;
- if(en_mouvement)
- {
- en_mouvement = false;
- strategie->collisionDetected();
- }
+ //if(en_mouvement)
+ //{
+ // en_mouvement = false;
+ // strategie->collisionDetected();
+ //}
}
- }
- else
- {
-#ifdef ROUES
- roues.gauche.tourne(0);
- roues.droite.tourne(0);
-#endif
- strategie->theEnd();
- }
}
-void Asservissement::recule(Distance distance) {
- goTo((odometrie->getPos() - distance).position);
-}
-
-Distance Asservissement::getVitesseLineaireAfterTrapeziumFilter(Distance vitesse_lineaire_atteinte, Distance distance_restante, Angle angle_restant)
-{
-
- // Le pivot est la distance que l'on parcourerait si on commencait à décélérer dès maintenant jusqu'une vitesse nulle
- Distance pivot(vitesse_lineaire_a_atteindre*vitesse_lineaire_a_atteindre/(2*acceleration_lineaire));
- if( (distance_restante-pivot)<=0 &&
- ((fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2 && vitesse_lineaire_atteinte<=0)
- || (fabs((angle_restant - Angle(0)).getValueInRadian())<=M_PI_2 && vitesse_lineaire_atteinte>=0))
- && stop)
- {
- //On est proche de la cible et l’on doit décélerer
- if(vitesse_lineaire_a_atteindre>0.)
- {
- return vitesse_lineaire_a_atteindre-Distance(min(vitesse_lineaire_a_atteindre, acceleration_lineaire));
- }
- else
- {
- return vitesse_lineaire_a_atteindre-Distance(max(vitesse_lineaire_a_atteindre, -acceleration_lineaire));
- }
-
- }
- else if(fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2)
- {
- // Sinon, on est en phase d'accélération
- // Si la destination est derrière nous
- // Alors on ajoute à la vitesse une accélération négative (jusqu'à la valeur (-vitesse_lineaire_max))
- // Autrement dit, si on avance dans la mauvaise direction on ralenti et si on recule dans la bonne direction alors on recule plus vite
- return vitesse_lineaire_a_atteindre
- + Distance(max(-acceleration_lineaire, (-vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
- }
- else
- {
- // Sinon, la destination est devant nous
- // Alors on ajoute à la vitesse une accélération positive (jusqu'à la valeur (vitesse_lineaire_max))
- // Autrement dit, si on recule dans la mauvaise direction on ralenti et si on avance dans la bonne direction alors on avance plus vite
- return vitesse_lineaire_a_atteindre
- + Distance(min(acceleration_lineaire, (vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
- }
-}
-
-Angle Asservissement::getVitesseAngulaireAfterTrapeziumFilter(Angle vitesse_angulaire_atteinte, Angle angle_restant)
-{
- if(fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2)
- {
- angle_restant = angle_restant-Angle(M_PI);
- }
- Angle didou(angle_restant);
- Angle pivot(vitesse_angulaire_a_atteindre.getValueInRadian()*vitesse_angulaire_a_atteindre.getValueInRadian()/(2*acceleration_angulaire.getValueInRadian()));
-
- if((Angle(fabs(angle_restant.getValueInRadian()))-pivot).getValueInRadian()<=0)
- {
- return vitesse_angulaire_a_atteindre-Angle(copysign(min(fabs(vitesse_angulaire_a_atteindre.getValueInRadian()), acceleration_angulaire.getValueInRadian()), vitesse_angulaire_a_atteindre.getValueInRadian()));
- }
- else
- {
- return vitesse_angulaire_a_atteindre + Angle(copysign(min(acceleration_angulaire.getValueInRadian(), vitesse_angulaire_max.getValueInRadian()-fabs(vitesse_angulaire_a_atteindre.getValueInRadian())), angle_restant.getValueInRadian()));
- }
-}
-
-void Asservissement::goTo(Position position, bool stop)
-{
- destination = position;
- direction = false;
- en_mouvement = true;
- this->stop = stop;
-}
-
-void Asservissement::goToDirection(Angle angle)
-{
- destination = odometrie->getPos().position;
- destAngle = angle;
- direction = true;
- en_mouvement = true;
- this->stop = true;
-}
-
-void Asservissement::tourne(Angle angle){
- goToDirection(odometrie->getPos().angle+angle);
-}
+//void Asservissement::recule(Distance distance) {
+ //goTo((odometrie->getPos() - distance).position);
+//}
+
+//Distance Asservissement::getVitesseLineaireAfterTrapeziumFilter(Distance vitesse_lineaire_atteinte, Distance distance_restante, Angle angle_restant)
+//{
+//
+// // Le pivot est la distance que l'on parcourerait si on commencait à décélérer dès maintenant jusqu'une vitesse nulle
+// Distance pivot(vitesse_lineaire_a_atteindre*vitesse_lineaire_a_atteindre/(2*acceleration_lineaire));
+// if( (distance_restante-pivot)<=0 &&
+// ((fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2 && vitesse_lineaire_atteinte<=0)
+// || (fabs((angle_restant - Angle(0)).getValueInRadian())<=M_PI_2 && vitesse_lineaire_atteinte>=0))
+// && stop)
+// {
+// //On est proche de la cible et l’on doit décélerer
+// if(vitesse_lineaire_a_atteindre>0.)
+// {
+// return vitesse_lineaire_a_atteindre-Distance(min(vitesse_lineaire_a_atteindre, acceleration_lineaire));
+// }
+// else
+// {
+// return vitesse_lineaire_a_atteindre-Distance(max(vitesse_lineaire_a_atteindre, -acceleration_lineaire));
+// }
+//
+// }
+// else if(fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2)
+// {
+// // Sinon, on est en phase d'accélération
+// // Si la destination est derrière nous
+// // Alors on ajoute à la vitesse une accélération négative (jusqu'à la valeur (-vitesse_lineaire_max))
+// // Autrement dit, si on avance dans la mauvaise direction on ralenti et si on recule dans la bonne direction alors on recule plus vite
+// return vitesse_lineaire_a_atteindre
+// + Distance(max(-acceleration_lineaire, (-vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
+// }
+// else
+// {
+// // Sinon, la destination est devant nous
+// // Alors on ajoute à la vitesse une accélération positive (jusqu'à la valeur (vitesse_lineaire_max))
+// // Autrement dit, si on recule dans la mauvaise direction on ralenti et si on avance dans la bonne direction alors on avance plus vite
+// return vitesse_lineaire_a_atteindre
+// + Distance(min(acceleration_lineaire, (vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
+// }
+//}
+//
+//Angle Asservissement::getVitesseAngulaireAfterTrapeziumFilter(Angle vitesse_angulaire_atteinte, Angle angle_restant)
+//{
+// if(fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2)
+// {
+// angle_restant = angle_restant-Angle(M_PI);
+// }
+// Angle didou(angle_restant);
+// Angle pivot(vitesse_angulaire_a_atteindre.getValueInRadian()*vitesse_angulaire_a_atteindre.getValueInRadian()/(2*acceleration_angulaire.getValueInRadian()));
+//
+// if((Angle(fabs(angle_restant.getValueInRadian()))-pivot).getValueInRadian()<=0)
+// {
+// return vitesse_angulaire_a_atteindre-Angle(copysign(min(fabs(vitesse_angulaire_a_atteindre.getValueInRadian()), acceleration_angulaire.getValueInRadian()), vitesse_angulaire_a_atteindre.getValueInRadian()));
+// }
+// else
+// {
+// return vitesse_angulaire_a_atteindre + Angle(copysign(min(acceleration_angulaire.getValueInRadian(), vitesse_angulaire_max.getValueInRadian()-fabs(vitesse_angulaire_a_atteindre.getValueInRadian())), angle_restant.getValueInRadian()));
+// }
+//}
+
+//void Asservissement::goTo(Position position, bool stop)
+//{
+// destination = position;
+// direction = false;
+// en_mouvement = true;
+// this->stop = stop;
+//}
+//
+//void Asservissement::goToDirection(Angle angle)
+//{
+// destination = odometrie->getPos().position;
+// destAngle = angle;
+// direction = true;
+// en_mouvement = true;
+// this->stop = true;
+//}
+//
+//void Asservissement::tourne(Angle angle){
+// goToDirection(odometrie->getPos().angle+angle);
+//}
extern "C" void SysTick_Handler()
{
diff --git a/stm32/src/command.cpp b/stm32/src/command.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1aad4e8a774f7aae594ad145c1852e6cb874be33
--- /dev/null
+++ b/stm32/src/command.cpp
@@ -0,0 +1,119 @@
+#include "command.h"
+#include <math.h>
+
+template<class T> T min(T a, T b)
+{
+ return a < b ? a : b;
+}
+
+template<class T> T max(T a, T b)
+{
+ return a > b ? a : b;
+}
+
+Command::Command() :
+ vitesse_lineaire_a_atteindre(0),
+ vitesse_angulaire_a_atteindre(0),
+#if 0
+ vitesse_lineaire_max(4.), // en mm par nb_ms_between_updates
+ vitesse_angulaire_max(M_PI/100.0), // en radian par nb_ms_between_updates
+ acceleration_lineaire(4./200.0), // en mm par nb_ms_between_updates
+ acceleration_angulaire((M_PI/100.0)/200.0) // en radian par nb_ms_between_updates
+#else
+ vitesse_lineaire_max(4.), // en mm par nb_ms_between_updates
+ vitesse_angulaire_max(M_PI/500.0), // en radian par nb_ms_between_updates
+ acceleration_lineaire(4./200.0), // en mm par nb_ms_between_updates
+ acceleration_angulaire((M_PI/100.0)/200.0) // en radian par nb_ms_between_updates
+#endif
+{
+}
+
+#include <iostream>
+
+void Command::update(PositionPlusAngle positionPlusAngleActuelle, Angle vitesse_angulaire_atteinte, float vitesse_lineaire_atteinte)
+{
+ Position vecteur_pos_actuelle_pos_arrivee = destination-positionPlusAngleActuelle.position;
+
+ Distance distance_restante = (vecteur_pos_actuelle_pos_arrivee).getNorme();
+ Angle angle_restant = vecteur_pos_actuelle_pos_arrivee.getAngle() - positionPlusAngleActuelle.angle;
+
+ if(false) //distance_restante < 5)
+ vitesse_lineaire_a_atteindre = 0;
+ else
+ vitesse_lineaire_a_atteindre = getVitesseLineaireAfterTrapeziumFilter(vitesse_lineaire_atteinte, distance_restante, angle_restant);
+
+ if(false) //fabs(angle_restant.getValueInRadian()) < 0.01)
+ vitesse_angulaire_a_atteindre = 0;
+ else
+ vitesse_angulaire_a_atteindre = getVitesseAngulaireAfterTrapeziumFilter(vitesse_angulaire_atteinte, angle_restant);
+ std::cout << "Ook " << distance_restante << " " << vitesse_lineaire_a_atteindre << std::endl;
+}
+
+
+
+Distance Command::getVitesseLineaireAfterTrapeziumFilter(Distance vitesse_lineaire_atteinte, Distance distance_restante, Angle angle_restant)
+{
+
+ // Le pivot est la distance que l'on parcourerait si on commencait à décélérer dès maintenant jusqu'une vitesse nulle
+ Distance pivot(vitesse_lineaire_a_atteindre*vitesse_lineaire_a_atteindre/(2*acceleration_lineaire));
+ if( (distance_restante-pivot)<=0 &&
+ ((fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2 && vitesse_lineaire_atteinte<=0)
+ || (fabs((angle_restant - Angle(0)).getValueInRadian())<=M_PI_2 && vitesse_lineaire_atteinte>=0))
+ && stop)
+ {
+ //On est proche de la cible et l’on doit décélerer
+ if(vitesse_lineaire_a_atteindre>0.)
+ {
+ return vitesse_lineaire_a_atteindre-Distance(min(vitesse_lineaire_a_atteindre, acceleration_lineaire));
+ }
+ else
+ {
+ return vitesse_lineaire_a_atteindre-Distance(max(vitesse_lineaire_a_atteindre, -acceleration_lineaire));
+ }
+
+ }
+ else if(fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2)
+ {
+ // Sinon, on est en phase d'accélération
+ // Si la destination est derrière nous
+ // Alors on ajoute à la vitesse une accélération négative (jusqu'à la valeur (-vitesse_lineaire_max))
+ // Autrement dit, si on avance dans la mauvaise direction on ralenti et si on recule dans la bonne direction alors on recule plus vite
+ return vitesse_lineaire_a_atteindre
+ + Distance(max(-acceleration_lineaire, (-vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
+ }
+ else
+ {
+ // Sinon, la destination est devant nous
+ // Alors on ajoute à la vitesse une accélération positive (jusqu'à la valeur (vitesse_lineaire_max))
+ // Autrement dit, si on recule dans la mauvaise direction on ralenti et si on avance dans la bonne direction alors on avance plus vite
+ return vitesse_lineaire_a_atteindre
+ + Distance(min(acceleration_lineaire, (vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
+ }
+}
+
+Angle Command::getVitesseAngulaireAfterTrapeziumFilter(Angle vitesse_angulaire_atteinte, Angle angle_restant)
+{
+ if(fabs((angle_restant.wrap()).getValueInRadian())>M_PI_2 + 0.01)
+ {
+ angle_restant = (angle_restant+Angle(M_PI)).wrap();
+ }
+
+ Angle pivot(vitesse_angulaire_a_atteindre.getValueInRadian()*vitesse_angulaire_a_atteindre.getValueInRadian()/(2*acceleration_angulaire.getValueInRadian()));
+
+ if((Angle(fabs(angle_restant.getValueInRadian()))-pivot).getValueInRadian()<=0)
+ {
+ return vitesse_angulaire_a_atteindre-Angle(copysign(min((float)fabs(vitesse_angulaire_a_atteindre.getValueInRadian()), acceleration_angulaire.getValueInRadian()), vitesse_angulaire_a_atteindre.getValueInRadian()));
+ }
+ else
+ {
+ return vitesse_angulaire_a_atteindre + Angle(copysign(min(acceleration_angulaire.getValueInRadian(), vitesse_angulaire_max.getValueInRadian()-(float)fabs(vitesse_angulaire_a_atteindre.getValueInRadian())), angle_restant.getValueInRadian()));
+ }
+}
+
+void Command::goTo(Position position, bool stop)
+{
+ destination = position;
+ direction = false;
+ en_mouvement = true;
+ this->stop = stop;
+}
diff --git a/stm32/src/simul/robot.cpp b/stm32/src/simul/robot.cpp
index 276684e2f1b467ec7516f86867342e242700ed52..d0a60a03118bd339dc51755f905fa2e51869c35e 100644
--- a/stm32/src/simul/robot.cpp
+++ b/stm32/src/simul/robot.cpp
@@ -44,9 +44,6 @@ public:
Robot::Robot(b2World & world) : world(world), olds(10000)
{
- deriv.position.x = 0;
- deriv.position.y = 0;
- deriv.angle = 0;
manual = false;
elem = NULL;
@@ -54,14 +51,16 @@ Robot::Robot(b2World & world) : world(world), olds(10000)
level = 0;
odometrie = new OdoRobot(this);
- asservissement = new Asservissement(odometrie);
- strategie = new Strategie(true, asservissement);
+ command = new Command;
+ Asservissement* asservissement = new Asservissement(odometrie, command);
+ strategie = new Strategie(true, command, odometrie);
+ asservissement->strategie = strategie;
- pos.position.x=1000.;
- pos.position.y=500.;
- pos.angle=0;
+ pos = odometrie->getPos();
+ deriv.position.x = 0;
+ deriv.position.y = 0;
+ deriv.angle = 0;
- asservissement->strategie = strategie;
b2BodyDef bodyDef;
#ifndef BOX2D_2_0_1
@@ -116,6 +115,14 @@ Robot::Robot(b2World & world) : world(world), olds(10000)
#ifdef BOX2D_2_0_1
body->SetMassFromShapes();
#endif
+
+ //Little hack so that linear and angular speed of the object
+ //are those of the local coord (0,0) of the robot.
+ //We don't really care of the mass center accuracy.
+ b2MassData md;
+ body->GetMassData(&md);
+ md.center = b2Vec2(0,0);
+ body->SetMassData(&md);
}
void Robot::updateForces(int dt)
@@ -166,9 +173,9 @@ void Robot::paint(QPainter &p, int dt)
else
{
Asservissement::asservissement->update();
- deriv.position.x = asservissement->vitesse_lineaire_a_atteindre;
+ deriv.position.x = command->getLinearSpeed();
deriv.position.y = 0;
- deriv.angle = asservissement->vitesse_angulaire_a_atteindre;
+ deriv.angle = command->getAngularSpeed();
}
}
diff --git a/stm32/src/simul/table.cpp b/stm32/src/simul/table.cpp
index 4613c96483be53648dc70cf5afa348cbb8812d54..5469a39d95fc000107ab34c1716c6f36cefa0572 100644
--- a/stm32/src/simul/table.cpp
+++ b/stm32/src/simul/table.cpp
@@ -94,18 +94,18 @@ Table::Table(QWidget* parent) :
tableBody->CreateFixture(&fixture);
//Init position of elements
- int l1 = rand() % 20;
- int l2 = rand() % 20;
- int r1 = rand() % 20;
- int r2 = rand() % 20;
-
- addCard(l1, 1);
- addCard(l2, 2);
- addCard(l2, 4);
- addCard(l1, 5);
- addCard(r1, -1);
- addCard(r2, -2);
- elements.push_back(new Element(world,getCase(3,3),Element::Pawn)); //Central element
+ //int l1 = rand() % 20;
+ //int l2 = rand() % 20;
+ //int r1 = rand() % 20;
+ //int r2 = rand() % 20;
+
+ //addCard(l1, 1);
+ //addCard(l2, 2);
+ //addCard(l2, 4);
+ //addCard(l1, 5);
+ //addCard(r1, -1);
+ //addCard(r2, -2);
+ //elements.push_back(new Element(world,getCase(3,3),Element::Pawn)); //Central element
}
Table::~Table()
diff --git a/stm32/src/strategie.cpp b/stm32/src/strategie.cpp
index 541409fa77359e46a3d0d3bc304b2e36741160cf..218eadbeb996c93830b1865a6c5dc1bc6e718fb6 100755
--- a/stm32/src/strategie.cpp
+++ b/stm32/src/strategie.cpp
@@ -1,4 +1,5 @@
#include "strategie.h"
+#include "odometrie.h"
#include "PositionPlusAngle.h"
#ifdef ROBOTHW
#include "stm32f10x.h"
@@ -6,12 +7,12 @@
#include "stm32f10x_rcc.h"
#include "stm32f10x_gpio.h"
#endif
-#include "asservissement.h"
+#include "command.h"
#include <math.h>
#define INSTRUCTION_COLLISION 128
-Strategie::Strategie(bool is_blue, Asservissement* asservissement) :
+Strategie::Strategie(bool is_blue, Command* command, Odometrie* odometrie) :
collision_detected(false)
{
this->is_blue = is_blue;
@@ -23,17 +24,17 @@ collision_detected(false)
//Angle angleDeDepart(M_PI_2);
//Position positionDeDepart(255,275);
//Angle angleDeDepart(0.779);
- Position positionDeDepart(0,0);
+ Position positionDeDepart(1000,700);
Angle angleDeDepart(0);
positionDeDepart.y = positionDeDepart.y*(is_blue ? 1:-1);
angleDeDepart = angleDeDepart*(is_blue ? 1:-1);
- //asservissement = new Asservissement(PositionPlusAngle(Position(335, 400), Angle(M_PI_2)), roueCodeuseGauche, roueCodeuseDroite);
- this->asservissement = asservissement;
- asservissement->odometrie->setPos(PositionPlusAngle(positionDeDepart,angleDeDepart));
- //asservissement = new Asservissement(PositionPlusAngle(positionDeDepart, angleDeDepart), roueCodeuseGauche, roueCodeuseDroite);
- //asservissement = new Asservissement(PositionPlusAngle(Position(0, 0), Angle(0)), roueCodeuseGauche, roueCodeuseDroite);
- asservissement->strategie = this;
+ //command = new Asservissement(PositionPlusAngle(Position(335, 400), Angle(M_PI_2)), roueCodeuseGauche, roueCodeuseDroite);
+ this->command = command;
+ odometrie->setPos(PositionPlusAngle(positionDeDepart,angleDeDepart));
+ //command = new Asservissement(PositionPlusAngle(positionDeDepart, angleDeDepart), roueCodeuseGauche, roueCodeuseDroite);
+ //command = new Asservissement(PositionPlusAngle(Position(0, 0), Angle(0)), roueCodeuseGauche, roueCodeuseDroite);
+ //command->strategie = this;
instruction_nb=1;
doNthInstruction(instruction_nb);
}
@@ -65,50 +66,50 @@ void Strategie::doNthInstruction(uint16_t n){
int cote = (is_blue ? 1:-1);
if(n==1)
- asservissement->goTo(Position(100, 100), false);
+ command->goTo(Position(1500, 700), false);
return;
//rouleau.recracheBoule();
//return;
#ifdef DONTUSE
switch(n) {
case 1:
- asservissement->goTo(Position(780, cote*780), false);
+ command->goTo(Position(780, cote*780), false);
break;
case 2:
rouleau.avaleBoule();
- asservissement->goTo(Position(1840, cote*2640));
+ command->goTo(Position(1840, cote*2640));
break;
case 3:
- asservissement->goToDirection(Angle(0));
+ command->goToDirection(Angle(0));
break;
case 4:
rouleau.recracheBoule();
break;
/*case 3:
//rouleau.recracheBoule();
- asservissement->goTo(Position(400, 600));
+ command->goTo(Position(400, 600));
break;*/
/*case 4:
rouleau.arrete();
- asservissement->goTo(Position(200, 800));
+ command->goTo(Position(200, 800));
break;
case 5:
- asservissement->goTo(Position(-200, 800));
+ command->goTo(Position(-200, 800));
break;
case 6:
rouleau.recracheBoule();
- asservissement->goTo(Position(-400, 600));
+ command->goTo(Position(-400, 600));
break;
case 7:
rouleau.arrete();
- asservissement->goTo(Position(-400, 200));
+ command->goTo(Position(-400, 200));
break;
case 8:
- asservissement->goTo(Position(-200, 0));
+ command->goTo(Position(-200, 0));
break;
case 9:
rouleau.arrete();
- asservissement->goTo(Position(0, 0));
+ command->goTo(Position(0, 0));
break;*/
default:
break;
@@ -118,32 +119,32 @@ void Strategie::doNthInstruction(uint16_t n){
switch(n) {
case 1:
//rouleau.avaleBoule();
- //asservissement->tourne(Angle(1.5));
- asservissement->goTo(Position(724, cote*779), false);
+ //command->tourne(Angle(1.5));
+ command->goTo(Position(724, cote*779), false);
break;
case 2:
//rouleau.avaleBoule();
- asservissement->goTo(Position(1242, cote*1694), false);
+ command->goTo(Position(1242, cote*1694), false);
break;
case 3:
//rouleau.avaleBoule();
- asservissement->goTo(Position(1790, cote*2610),false);
+ command->goTo(Position(1790, cote*2610),false);
break;
case 4:
- asservissement->goToDirection(Angle(0));
+ command->goToDirection(Angle(0));
break;
case 5:
//rouleau.recracheBoule();
- asservissement->goTo(Position(1750, cote*2610));
+ command->goTo(Position(1750, cote*2610));
break;
case 6:
- asservissement->goTo(Position(1820, cote*2610));
+ command->goTo(Position(1820, cote*2610));
break;
case INSTRUCTION_COLLISION:
- asservissement->recule(Distance(100));
+ command->recule(Distance(100));
break;
case (INSTRUCTION_COLLISION+1):
- asservissement->tourne(Angle(1.5));
+ command->tourne(Angle(1.5));
collision_detected = false;
instruction_nb--;
break;
@@ -152,7 +153,7 @@ void Strategie::doNthInstruction(uint16_t n){
/*
if(n==0)
{
- asservissement->recule(2500);
+ command->recule(2500);
}*/
}