diff --git a/include/distance.h b/include/distance.h
index 84fe6abedf53dcf8cfd01703a4c5c147e0062560..a424fda47f7268ffed93c77eb6e6323a2b9f9899 100755
--- a/include/distance.h
+++ b/include/distance.h
@@ -1,26 +1,8 @@
-#ifndef DISTANCE_H_INCLUDED
+#ifndef DISTANCE_H_INCLUDED
#define DISTANCE_H_INCLUDED
-class Distance{
- private:
- float value;
+typedef float Distance;
- public:
- float getValueInMillimeters() const;
- Distance();
- Distance(float distance_en_millimetres);
- Distance operator+(const Distance &dist);
- Distance operator-(const Distance &dist);
- Distance operator-() const;
- Distance operator*(const Distance &dist);
- Distance operator*(float dist);
- Distance operator=(const Distance &dist);
- Distance operator+=(const Distance &dist);
- Distance operator-=(const Distance &dist);
- bool operator==(const Distance &d);
- bool presqueEgales(const Distance &d);
- bool operator>(const Distance &dist);
- bool operator<(const Distance &dist);
-};
-
-#endif // DISTANCE_H_INCLUDED
+bool presqueEgales(Distance d1, Distance d2);
+
+#endif // DISTANCE_H_INCLUDED
diff --git a/include/simul/element.h b/include/simul/element.h
index 25c78aa6befefa2a01d51dafbb872f8645b38d36..6f6357dc07b7d2a83936000117bf3829b7739aac 100644
--- a/include/simul/element.h
+++ b/include/simul/element.h
@@ -28,7 +28,7 @@ public:
pa.setBrush(QBrush(QColor("yellow")));
pa.setPen(QBrush(QColor("yellow")));
- pa.drawEllipse(QPoint(p.x.getValueInMillimeters(),p.y.getValueInMillimeters()),100,100);
+ pa.drawEllipse(QPoint(p.x,p.y),100,100);
}
void updatePos();
diff --git a/src/Position.cpp b/src/Position.cpp
index 8c3f283a28b20dc3f83190e16f53be0decb32bf2..7cda49379f710ea8f237d9cfb72947dbe9e2e3e2 100755
--- a/src/Position.cpp
+++ b/src/Position.cpp
@@ -1,8 +1,7 @@
#include "Position.h"
+#include "distance.h"
#include <math.h>
-/*Toutes les paramètres doivent être donnés en centimètre */
-
Position::Position(Distance blah_x, Distance blah_y) : x(blah_x), y(blah_y){
}
@@ -39,33 +38,18 @@ Position Position::operator-=(const Position &position){
}
bool Position::presqueEgales(const Position &p){
- if(this->x.presqueEgales(p.x) && this->y.presqueEgales(p.y))
- return true;
- return false;
- /*if((this->x-p.x)*(this->x-p.x) + (this->y-p.y)*(this->y-p.y) < 1.41)
- return true;
- return false;*/
+ return ::presqueEgales(x, p.x) && ::presqueEgales(y,p.y);
}
bool Position::operator==(const Position &p){
return (this->x == p.x && this->y == p.y);
- /*return (this->x-p.x) == 0
- && (this->y-p.y) == 0;*/
}
-Distance Position::getNorme(void){
- return Distance(sqrt(x.getValueInMillimeters()*x.getValueInMillimeters()+y.getValueInMillimeters()*y.getValueInMillimeters()));
+Distance Position::getNorme(){
+ return Distance(sqrt(x*x+y*y));
}
-Angle Position::getAngle(void){
- if(x==Distance(0)){
- return y>Distance(0) ? Angle(M_PI_2) : Angle(-M_PI_2);
- }
- else if(x>Distance(0)){
- return Angle(atan(y.getValueInMillimeters()/x.getValueInMillimeters()));
- }
- else {
- double tmp_val = atan(y.getValueInMillimeters()/x.getValueInMillimeters());
- return tmp_val>0 ? Angle(-M_PI+tmp_val) : Angle(M_PI+tmp_val);
- }
+Angle Position::getAngle()
+{
+ return atan2(y,x);
}
diff --git a/src/PositionPlusAngle.cpp b/src/PositionPlusAngle.cpp
index 2411abc0f5fd74f33c19199a693165ef0968784b..e9db82d013e547bca0c4b8e1708a16b4b2fea4ee 100755
--- a/src/PositionPlusAngle.cpp
+++ b/src/PositionPlusAngle.cpp
@@ -9,7 +9,7 @@ PositionPlusAngle::PositionPlusAngle(Position pos, Angle ang) : position(pos), a
PositionPlusAngle PositionPlusAngle::operator+(const Distance &distance)
{
- return PositionPlusAngle(Position(position.x.getValueInMillimeters() + distance.getValueInMillimeters()*cos(angle.getValueInRadian()), position.y.getValueInMillimeters() + distance.getValueInMillimeters()*sin(angle.getValueInRadian())), angle);
+ return PositionPlusAngle(Position(position.x + distance*cos(angle.getValueInRadian()), position.y + distance*sin(angle.getValueInRadian())), angle);
}
PositionPlusAngle PositionPlusAngle::operator-(const Distance &distance)
diff --git a/src/asservissement.cpp b/src/asservissement.cpp
index e3e7c9703fdc519afedf3edf567d80c71bf7bdab..d908469f55bf5273f261993a65fbb5448500554a 100755
--- a/src/asservissement.cpp
+++ b/src/asservissement.cpp
@@ -101,7 +101,7 @@ void Asservissement::update(void)
Distance vitesse_lineaire_atteinte = odometrie->getVitesseLineaire();
buffer_collision <<= 1;
- bool tmp = (fabs((vitesse_lineaire_atteinte - vitesse_lineaire_a_atteindre).getValueInMillimeters()) < seuil_collision);
+ bool tmp = (fabs((vitesse_lineaire_atteinte - vitesse_lineaire_a_atteindre)) < seuil_collision);
buffer_collision |= tmp;
@@ -145,15 +145,15 @@ void Asservissement::update(void)
//roueGauche[caca] = odometrie.roueCodeuseGauche->getTickValue();
//roueDroite[caca] = odometrie.roueCodeuseDroite->getTickValue();
- /*vitesseLin[caca] = vitesse_lineaire_atteinte.getValueInMillimeters();
- vitesseLinE[caca] = vitesse_lineaire_a_atteindre.getValueInMillimeters();
+ /*vitesseLin[caca] = vitesse_lineaire_atteinte;
+ vitesseLinE[caca] = vitesse_lineaire_a_atteindre;
linearDuty[caca] = linearDutySent;*/
/*vitesseAng[caca] = vitesse_angulaire_atteinte.getValueInRadian();
vitesseAngE[caca] = vitesse_angulaire_a_atteindre.getValueInRadian();
angularDuty[caca] = angularDutySent;*/
- posx[caca] = positionPlusAngleActuelle.position.x.getValueInMillimeters();
- posy[caca] = positionPlusAngleActuelle.position.y.getValueInMillimeters();
- angle[caca] = positionPlusAngleActuelle.angle.getValueInRadian(); //*angle_restant.getValueInRadian();*/distance_restante.getValueInMillimeters(); //positionPlusAngleActuelle.angle.getValueInRadian()*180/M_PI;
+ posx[caca] = positionPlusAngleActuelle.position.x;
+ posy[caca] = positionPlusAngleActuelle.position.y;
+ angle[caca] = positionPlusAngleActuelle.angle.getValueInRadian(); //*angle_restant.getValueInRadian();*/distance_restante; //positionPlusAngleActuelle.angle.getValueInRadian()*180/M_PI;
caca++;
}
toto = 0;
@@ -225,20 +225,20 @@ Distance Asservissement::getVitesseLineaireAfterTrapeziumFilter(Distance vitesse
{
// 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.getValueInMillimeters()*vitesse_lineaire_a_atteindre.getValueInMillimeters()/(2*acceleration_lineaire.getValueInMillimeters()));
- if( (distance_restante-pivot).getValueInMillimeters()<=0 &&
- ((fabs((angle_restant - Angle(0)).getValueInRadian())>M_PI_2 && vitesse_lineaire_atteinte.getValueInMillimeters()<=0)
- || (fabs((angle_restant - Angle(0)).getValueInRadian())<=M_PI_2 && vitesse_lineaire_atteinte.getValueInMillimeters()>=0))
+ 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.getValueInMillimeters()>0)
+ if(vitesse_lineaire_a_atteindre>0.)
{
- return vitesse_lineaire_a_atteindre-Distance(min(vitesse_lineaire_a_atteindre.getValueInMillimeters(), acceleration_lineaire.getValueInMillimeters()));
+ 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.getValueInMillimeters(), -acceleration_lineaire.getValueInMillimeters()));
+ return vitesse_lineaire_a_atteindre-Distance(max(vitesse_lineaire_a_atteindre, -acceleration_lineaire));
}
}
@@ -249,7 +249,7 @@ Distance Asservissement::getVitesseLineaireAfterTrapeziumFilter(Distance vitesse
// 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.getValueInMillimeters(), (-vitesse_lineaire_max.getValueInMillimeters()-vitesse_lineaire_a_atteindre.getValueInMillimeters())));
+ + Distance(max(-acceleration_lineaire, (-vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
}
else
{
@@ -257,7 +257,7 @@ Distance Asservissement::getVitesseLineaireAfterTrapeziumFilter(Distance vitesse
// 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.getValueInMillimeters(), (vitesse_lineaire_max.getValueInMillimeters()-vitesse_lineaire_a_atteindre.getValueInMillimeters())));
+ + Distance(min(acceleration_lineaire, (vitesse_lineaire_max-vitesse_lineaire_a_atteindre)));
}
}
diff --git a/src/distance.cpp b/src/distance.cpp
index c5ca609a45822efcb3e4df276c748ea67c2bb90a..aa9ca49c54263e5fcb4247da5393c0f626d0a977 100755
--- a/src/distance.cpp
+++ b/src/distance.cpp
@@ -2,75 +2,8 @@
#include <math.h>
-/*Toutes les paramètres doivent être donnés en centimètre */
-
-Distance::Distance(){
- this->value=0;
-}
-
-Distance::Distance(float distance_en_millimetres){
- this->value=distance_en_millimetres;
-}
-
-Distance Distance::operator+(const Distance &dist){
- Distance resultat(this->value + dist.value);
- return resultat;
-}
-
-Distance Distance::operator*(const Distance &dist){
- Distance resultat(this->value * dist.value);
- return resultat;
-}
-
-Distance Distance::operator*(float dist){
- Distance resultat(this->value * dist);
- return resultat;
-}
-
-Distance Distance::operator-(const Distance &dist){
- Distance resultat(this->value - dist.value);
- return resultat;
-}
-
-Distance Distance::operator-() const
+bool presqueEgales(Distance d1, Distance d2)
{
- return Distance(-value);
+ return (fabs(d1 - d2) < /*0.44*/10);
}
-Distance Distance::operator=(const Distance &dist){
- this->value=dist.value;
-
- return *this;
-}
-
-Distance Distance::operator+=(const Distance &dist){
- this->value += dist.value;
- return *this;
-}
-
-Distance Distance::operator-=(const Distance &dist){
- this->value -= dist.value;
- return *this;
-}
-
-bool Distance::presqueEgales(const Distance &d){
- if(fabs(this->value - d.value) < /*0.44*/10)
- return true;
- return false;
-}
-
-bool Distance::operator>(const Distance &dist){
- return this->value > dist.value;
-}
-
-bool Distance::operator<(const Distance &dist){
- return this->value < dist.value;
-}
-
-bool Distance::operator==(const Distance &d){
- return (this->value == d.value);
-}
-
-float Distance::getValueInMillimeters() const {
- return value;
-}
diff --git a/src/pid_filter_distance.cpp b/src/pid_filter_distance.cpp
index 5ef9377df312485c580e6cc21ebe485c31b0577f..754b18727053fba17fe345b84ef2073a13d0af1e 100755
--- a/src/pid_filter_distance.cpp
+++ b/src/pid_filter_distance.cpp
@@ -16,7 +16,7 @@ float PIDFilterDistance::getFilteredValue(Distance erreur){
Distance integrale = sommeErreurs;
Distance derivee = erreur - erreurPrecedente;
erreurPrecedente=erreur;
- return proportionnel.getValueInMillimeters()*Kp
- + integrale.getValueInMillimeters()*Ki
- + derivee.getValueInMillimeters()*Kd;
+ return proportionnel*Kp
+ + integrale*Ki
+ + derivee*Kd;
}
diff --git a/src/simul/element.cpp b/src/simul/element.cpp
index bc693b2be27ad17519a19faeb37ed2f15e78314e..e5f71f016337a0564bb38f9731d4e9341cca2e3b 100644
--- a/src/simul/element.cpp
+++ b/src/simul/element.cpp
@@ -7,7 +7,7 @@ Element::Element(b2World & world, Position p, Type t)
b2BodyDef bodyDef;
bodyDef.type = b2_dynamicBody;
- bodyDef.position.Set(p.x.getValueInMillimeters()/100., p.y.getValueInMillimeters()/100.);
+ bodyDef.position.Set(p.x/100., p.y/100.);
body = world.CreateBody(&bodyDef);
diff --git a/src/simul/robot.cpp b/src/simul/robot.cpp
index 51382abb806ce6534c22bf652b76b2f87cf060e4..5b0948337db58a06fe6f53f52d6bb66560e0d2d3 100644
--- a/src/simul/robot.cpp
+++ b/src/simul/robot.cpp
@@ -61,7 +61,7 @@ Robot::Robot(b2World & world) : olds(10000)
b2BodyDef bodyDef;
bodyDef.type = b2_dynamicBody;
- bodyDef.position.Set(pos.position.x.getValueInMillimeters()/100., pos.position.y.getValueInMillimeters()/100.);
+ bodyDef.position.Set(pos.position.x/100., pos.position.y/100.);
bodyDef.angle = pos.angle.getValueInRadian();
body = world.CreateBody(&bodyDef);
@@ -82,12 +82,12 @@ void Robot::updateForces(int dt)
return;
Position impulse;
- impulse.x = (deriv.position.x*cos(pos.angle.getValueInRadian()) - deriv.position.y*sin(pos.angle.getValueInRadian()));
- impulse.y = (deriv.position.x*sin(pos.angle.getValueInRadian()) + deriv.position.y*cos(pos.angle.getValueInRadian()));
+ impulse.x = (deriv.position.x*(float)cos(pos.angle.getValueInRadian()) - deriv.position.y*(float)sin(pos.angle.getValueInRadian()));
+ impulse.y = (deriv.position.x*(float)sin(pos.angle.getValueInRadian()) + deriv.position.y*(float)cos(pos.angle.getValueInRadian()));
float32 rdt = 1000./(float)dt;
- b2Vec2 bvelocity = 0.01*rdt*b2Vec2(impulse.x.getValueInMillimeters(),impulse.y.getValueInMillimeters());
+ b2Vec2 bvelocity = 0.01*rdt*b2Vec2(impulse.x,impulse.y);
float bangular = deriv.angle.getValueInRadian()*rdt;
//body->ApplyForce(10*body->GetMass()*(bimpulse - body->GetLinearVelocity()), body->GetWorldCenter());
//body->ApplyTorque((bangular - body->GetAngularVelocity())*body->GetInertia());
@@ -118,7 +118,7 @@ void Robot::paint(QPainter &p, int dt)
if(manual)
{
keyPressEvent(NULL,false);
- deriv.position.x = deriv.position.x* 0.97;
+ deriv.position.x = deriv.position.x* 0.97f;
deriv.angle = deriv.angle * 0.9;
}
else
@@ -130,7 +130,7 @@ void Robot::paint(QPainter &p, int dt)
}
}
- p.setWorldTransform(QTransform().translate(pos.position.x.getValueInMillimeters(),pos.position.y.getValueInMillimeters()).rotateRadians(pos.angle.getValueInRadian()));
+ p.setWorldTransform(QTransform().translate(pos.position.x,pos.position.y).rotateRadians(pos.angle.getValueInRadian()));
p.setPen(QColor(Qt::black));
@@ -140,14 +140,14 @@ void Robot::paint(QPainter &p, int dt)
p.setOpacity(1);
p.setPen(QColor(Qt::red));
- p.drawLine(0,0,pos.position.x.getValueInMillimeters(),0);
+ p.drawLine(0,0,pos.position.x,0);
p.drawLine(0,100*pos.angle.getValueInRadian(),0,0);
p.setWorldTransform(QTransform());
p.setPen(QColor(Qt::green));
for(unsigned int i=0; i+1 < olds.size(); i++)
- p.drawLine(olds[i].position.x.getValueInMillimeters(), olds[i].position.y.getValueInMillimeters(), olds[i+1].position.x.getValueInMillimeters(), olds[i+1].position.y.getValueInMillimeters());
+ p.drawLine(olds[i].position.x, olds[i].position.y, olds[i+1].position.x, olds[i+1].position.y);
}
#define IF_KEYSWITCH(n,a) \