From aaf2b7c4307ff9f24e66e966ae1732fcd15c6e58 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Gr=C3=A9goire=20Payen=20de=20La=20Garanderie?=
<gregoire.payendelagaranderie@telecom-bretagne.eu>
Date: Sun, 16 Jan 2011 14:38:44 +0100
Subject: [PATCH] typdef float Distance; :-)
---
include/distance.h | 28 +++------------
include/simul/element.h | 2 +-
src/Position.cpp | 30 ++++------------
src/PositionPlusAngle.cpp | 2 +-
src/asservissement.cpp | 30 ++++++++--------
src/distance.cpp | 71 ++-----------------------------------
src/pid_filter_distance.cpp | 6 ++--
src/simul/element.cpp | 2 +-
src/simul/robot.cpp | 16 ++++-----
9 files changed, 43 insertions(+), 144 deletions(-)
diff --git a/include/distance.h b/include/distance.h
index 84fe6abe..a424fda4 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 25c78aa6..6f6357dc 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 8c3f283a..7cda4937 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 2411abc0..e9db82d0 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 e3e7c970..d908469f 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 c5ca609a..aa9ca49c 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 5ef9377d..754b1872 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 bc693b2b..e5f71f01 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 51382abb..5b094833 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) \
--
GitLab