From 35b82ced04c8a158ca625912e94c590fcb674a16 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 00:22:07 +0100
Subject: [PATCH] =?UTF-8?q?Ajout=20d=E2=80=99une=20petite=20simulation=20p?=
=?UTF-8?q?hysique=20avec=20Box2D?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
include/simul/element.h | 13 ++---
include/simul/robot.h | 5 +-
include/simul/table.h | 4 ++
simulation/Makefile.am | 1 +
simulation/aclocal.m4 | 1 +
simulation/configure.ac | 6 ++-
simulation/m4/ax_dep_check.m4 | 42 ++++++++++++++++
src/asservissement.cpp | 6 ++-
src/roue.cpp | 1 +
src/simul/element.cpp | 29 +++++++++++
src/simul/robot.cpp | 93 +++++++++++++++++++++++++----------
src/simul/table.cpp | 60 ++++++++++++++++++++--
12 files changed, 221 insertions(+), 40 deletions(-)
create mode 100644 simulation/m4/ax_dep_check.m4
create mode 100644 src/simul/element.cpp
diff --git a/include/simul/element.h b/include/simul/element.h
index b125b1d1..25c78aa6 100644
--- a/include/simul/element.h
+++ b/include/simul/element.h
@@ -3,6 +3,7 @@
#include "Position.h"
#include <QPainter>
+#include "Box2D/Box2D.h"
class Element
{
@@ -17,11 +18,9 @@ public:
Type type;
- Element(Position p, Type t)
- {
- this->p = p;
- type = t;
- }
+ b2Body* body;
+
+ Element(b2World & world, Position p, Type t);
void paint(QPainter & pa)
{
@@ -29,8 +28,10 @@ public:
pa.setBrush(QBrush(QColor("yellow")));
pa.setPen(QBrush(QColor("yellow")));
- pa.drawEllipse(QPoint(p.x.getValueInMillimeters(),p.x.getValueInMillimeters()),150,150);
+ pa.drawEllipse(QPoint(p.x.getValueInMillimeters(),p.y.getValueInMillimeters()),100,100);
}
+
+ void updatePos();
};
#endif //PION_H_INCLUDED
diff --git a/include/simul/robot.h b/include/simul/robot.h
index 43e3a51f..ebeaaf53 100644
--- a/include/simul/robot.h
+++ b/include/simul/robot.h
@@ -5,6 +5,7 @@
#include <QKeyEvent>
#include <boost/circular_buffer.hpp>
#include "PositionPlusAngle.h"
+#include <Box2D/Box2D.h>
class Robot
{
@@ -19,11 +20,13 @@ public:
class OdoRobot* odometrie;
class Strategie* strategie;
+ b2Body* body;
- Robot();
+ Robot(b2World &world);
void paint(QPainter &p, int dt);
void keyPressEvent(QKeyEvent* evt,bool press);
+ void updateForces(int dt);
};
#endif //ROBOT_H_INCLUDED
diff --git a/include/simul/table.h b/include/simul/table.h
index b54da675..b36e7929 100644
--- a/include/simul/table.h
+++ b/include/simul/table.h
@@ -3,6 +3,7 @@
#include <QWidget>
#include "element.h"
+#include <Box2D/Box2D.h>
class Table : public QWidget
{
@@ -10,6 +11,9 @@ private:
int dt;
std::vector<class Element*> elements;
std::vector<class Robot*> robots;
+
+ b2World world;
+ b2Body* tableBody;
public:
static const int tableWidth = 3000;
diff --git a/simulation/Makefile.am b/simulation/Makefile.am
index beba2e3d..05565af3 100644
--- a/simulation/Makefile.am
+++ b/simulation/Makefile.am
@@ -4,6 +4,7 @@ INCLUDES = -Iinclude -Iimpl -Imoc -I.
paprika_SOURCES=\
+src/simul/element.cpp \
src/simul/main.cpp \
src/simul/main_window.cpp \
src/simul/table.cpp \
diff --git a/simulation/aclocal.m4 b/simulation/aclocal.m4
index c89e5f68..939b3946 100644
--- a/simulation/aclocal.m4
+++ b/simulation/aclocal.m4
@@ -1108,3 +1108,4 @@ AC_SUBST([am__untar])
]) # _AM_PROG_TAR
m4_include([m4/ax_boost_base.m4])
+m4_include([m4/ax_dep_check.m4])
diff --git a/simulation/configure.ac b/simulation/configure.ac
index 34eb3a37..f352465e 100644
--- a/simulation/configure.ac
+++ b/simulation/configure.ac
@@ -23,6 +23,8 @@ AX_BOOST_BASE([1.40], [AC_MSG_RESULT(yes) ] , [AC_MSG_RESULT(no) ] )
CXXFLAGS=
+AX_DEP_CHECK(Box2D, [/usr/lib /usr/local/lib], libBox2D.so,[/usr/include /usr/local/include],Box2D/Box2D.h, -lBox2D)
+
AC_ARG_ENABLE([debug], AS_HELP_STRING([--enable-debug],[Turn on debugging]), [enable_debug=$enableval],[enable_debug="no"])
AC_MSG_CHECKING(debug)
AC_MSG_RESULT($enable_debug)
@@ -30,8 +32,8 @@ if test "x$enable_debug" = "xyes"; then
CXXFLAGS="$CXXFLAGS -g3 -ggdb -Wall -DDEBUG"
fi
-CXXFLAGS="$CXXFLAGS $AM_CXXFLAGS $QT_CFLAGS $BOOST_CFLAGS"
-LDFLAGS="$LDFLAGS $AM_LDFLAGS $QT_LIBS $BOOST_LDFLAGS"
+CXXFLAGS="$CXXFLAGS $AM_CXXFLAGS $QT_CFLAGS $BOOST_CFLAGS $Box2D_CFLAGS"
+LDFLAGS="$LDFLAGS $AM_LDFLAGS $QT_LIBS $BOOST_LDFLAGS $Box2D_LIBS"
AC_SUBST([CFLAGS])
AC_HEADER_STDC
diff --git a/simulation/m4/ax_dep_check.m4 b/simulation/m4/ax_dep_check.m4
new file mode 100644
index 00000000..f17b1af7
--- /dev/null
+++ b/simulation/m4/ax_dep_check.m4
@@ -0,0 +1,42 @@
+
+AC_DEFUN([AX_DEP_CHECK],
+[
+
+libluabindpath="/usr/lib /usr/local/lib"
+libluabindname=libluabind.so
+libluabindheader="/usr/include /usr/local/include"
+
+AC_MSG_CHECKING(for $1 libs)
+
+for l in $2; do
+ if test -f $l/$3; then
+ AC_MSG_RESULT($l/$3)
+ $1_LIBS="-L$l $6"
+ break
+ fi
+done
+
+if test -z "${$1_LIBS}"; then
+ AC_MSG_RESULT(not found)
+ exit 1
+fi
+
+AC_MSG_CHECKING(for $1 headers)
+
+for l in $4; do
+ if test -f $l/$5; then
+ AC_MSG_RESULT($l)
+ $1_CFLAGS="-I$l"
+ break
+ fi
+done
+
+if test -z "${$1_CFLAGS}"; then
+ AC_MSG_RESULT(not found)
+ exit 1
+fi
+
+AC_SUBST($1_CFLAGS)
+AC_SUBST($1_LIBS)
+
+])
diff --git a/src/asservissement.cpp b/src/asservissement.cpp
index ab7be00b..e3e7c970 100755
--- a/src/asservissement.cpp
+++ b/src/asservissement.cpp
@@ -62,7 +62,8 @@ Asservissement::Asservissement(Odometrie* _odometrie) :
en_mouvement = false;
Asservissement::asservissement = this;
- *((uint32_t *)(STK_CTRL_ADDR)) = 0x03; // CLKSOURCE:0 ; TICKINT: 1 ; ENABLE:1
+#ifdef ROBOTHW
+ *((uint32_t *)(STK_CTRL_ADDR)) = 0x03; // CLKSOURCE:0 ; TICKINT: 1 ; ENABLE:1
*((uint32_t *)(STK_LOAD_ADDR)) = 9000*nb_ms_between_updates; // valeur en ms*9000 (doit etre inférieur à 0x00FFFFFF=16 777 215)
NVIC_InitTypeDef SysTick_IRQ;
@@ -72,15 +73,18 @@ Asservissement::Asservissement(Odometrie* _odometrie) :
SysTick_IRQ.NVIC_IRQChannelPreemptionPriority = 0;
SysTick_IRQ.NVIC_IRQChannelSubPriority = 1;
NVIC_Init(&SysTick_IRQ);
+#endif
}
int asserCount = 0;
void Asservissement::update(void)
{
+/*#ifdef ROBOTHW
roues.gauche.tourne(0.1);
roues.droite.tourne(0.1);
return;
+#endif*/
//PositionPlusAngle before(odometrie.positionPlusAngle);
asserCount++;
diff --git a/src/roue.cpp b/src/roue.cpp
index 6afae2f7..16d5663c 100755
--- a/src/roue.cpp
+++ b/src/roue.cpp
@@ -29,6 +29,7 @@ Roue::Roue(unsigned char OCx, GPIO_TypeDef * GPIOx_Sens, uint16_t GPIO_Pin_Sens)
void Roue::tourne(float rapport)
{
+ rapport = 0;
if(rapport >= 0){
GPIO_WriteBit(GPIOx_Sens, GPIO_Pin_Sens, Bit_RESET);
}
diff --git a/src/simul/element.cpp b/src/simul/element.cpp
new file mode 100644
index 00000000..61c441bc
--- /dev/null
+++ b/src/simul/element.cpp
@@ -0,0 +1,29 @@
+#include "simul/element.h"
+
+Element::Element(b2World & world, Position p, Type t)
+{
+ this->p = p;
+ type = t;
+
+ b2BodyDef bodyDef;
+ bodyDef.type = b2_dynamicBody;
+ bodyDef.position.Set(p.x.getValueInMillimeters()/100., p.y.getValueInMillimeters()/100.);
+
+ body = world.CreateBody(&bodyDef);
+
+ b2CircleShape circle;
+ circle.m_p.Set(0.,0.);
+ circle.m_radius = 1.0f;
+
+ b2FixtureDef fixtureDef;
+ fixtureDef.shape = &circle;
+ fixtureDef.density = 1.0f;
+ fixtureDef.friction = 0; //0.3f;
+ body->CreateFixture(&fixtureDef);
+}
+
+void Element::updatePos()
+{
+ p.x = 100.*body->GetPosition().x;
+ p.y = 100.*body->GetPosition().y;
+}
diff --git a/src/simul/robot.cpp b/src/simul/robot.cpp
index e3dd2e10..a880b438 100644
--- a/src/simul/robot.cpp
+++ b/src/simul/robot.cpp
@@ -23,12 +23,12 @@ public:
return robot->pos;
}
- Distance vitesseLineaire()
+ Distance getVitesseLineaire()
{
return robot->deriv.position.getNorme();
}
- Angle vitesseAngulaire()
+ Angle getVitesseAngulaire()
{
return robot->deriv.angle;
}
@@ -40,11 +40,8 @@ public:
};
-Robot::Robot() : olds(10000)
+Robot::Robot(b2World & world) : olds(10000)
{
- pos.position.x=1000;
- pos.position.y=500;
- pos.angle=0;
deriv.position.x = 0;
deriv.position.y = 0;
@@ -55,22 +52,72 @@ Robot::Robot() : olds(10000)
odometrie = new OdoRobot(this);
asservissement = new Asservissement(odometrie);
strategie = new Strategie(true, asservissement);
+
+ pos.position.x=1000.;
+ pos.position.y=500.;
+ pos.angle=0;
+
asservissement->strategie = strategie;
+
+ b2BodyDef bodyDef;
+ bodyDef.type = b2_dynamicBody;
+ bodyDef.position.Set(pos.position.x.getValueInMillimeters()/100., pos.position.y.getValueInMillimeters()/100.);
+ bodyDef.angle = pos.angle.getValueInRadian();
+
+ body = world.CreateBody(&bodyDef);
+
+ b2PolygonShape box;
+ box.SetAsBox(1.,1., b2Vec2(1,1),0);
+
+ b2FixtureDef fixtureDef;
+ fixtureDef.shape = &box;
+ fixtureDef.density = 10.0f;
+ fixtureDef.friction = 0.f;
+ body->CreateFixture(&fixtureDef);
+}
+
+void Robot::updateForces(int dt)
+{
+ if(dt == 0)
+ 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()));
+
+ float32 rdt = 1000./(float)dt;
+
+ b2Vec2 bimpulse = 10*(0.01*rdt*b2Vec2(impulse.x.getValueInMillimeters(),impulse.y.getValueInMillimeters()) - body->GetLinearVelocity());
+ //body->SetLinearVelocity(bimpulse);
+ body->ApplyForce(body->GetMass()*bimpulse, body->GetWorldCenter());
+ //body->SetAngularVelocity(deriv.angle.getValueInRadian()*(1000./(float)dt));
+ body->ApplyTorque((deriv.angle.getValueInRadian()*rdt - body->GetAngularVelocity())*body->GetInertia());
+
}
+
void Robot::paint(QPainter &p, int dt)
{
- if(manual)
- {
- keyPressEvent(NULL,false);
- deriv.position.x = deriv.position.x* 0.99;
- deriv.angle = deriv.angle * 0.9;
- }
- else
+ if(dt)
{
- Asservissement::asservissement->update();
- deriv.position.x = asservissement->vitesse_lineaire_a_atteindre;
- deriv.angle = asservissement->vitesse_angulaire_a_atteindre;
+ pos.position.x = 100*body->GetWorldCenter().x;
+ pos.position.y = 100*body->GetWorldCenter().y;
+ pos.angle = body->GetAngle();
+
+ olds.push_back(pos);
+ if(manual)
+ {
+ keyPressEvent(NULL,false);
+ deriv.position.x = deriv.position.x* 0.97;
+ deriv.angle = deriv.angle * 0.9;
+ }
+ else
+ {
+ Asservissement::asservissement->update();
+ deriv.position.x = asservissement->vitesse_lineaire_a_atteindre;
+ deriv.position.y = 0;
+ deriv.angle = asservissement->vitesse_angulaire_a_atteindre;
+ }
}
p.setWorldTransform(QTransform().translate(pos.position.x.getValueInMillimeters(),pos.position.y.getValueInMillimeters()).rotateRadians(pos.angle.getValueInRadian()));
@@ -87,16 +134,9 @@ void Robot::paint(QPainter &p, int dt)
p.drawLine(0,100*pos.angle.getValueInRadian(),0,0);
p.setWorldTransform(QTransform());
- if(dt)
- {
- olds.push_back(pos);
- pos.position.x += (deriv.position.x*cos(pos.angle.getValueInRadian()) - deriv.position.y*sin(pos.angle.getValueInRadian()));
- pos.position.y += (deriv.position.x*sin(pos.angle.getValueInRadian()) + deriv.position.y*cos(pos.angle.getValueInRadian()));
- pos.angle += deriv.angle;
- }
p.setPen(QColor(Qt::green));
- for(unsigned int i=0; i < olds.size()-1; i++)
+ 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());
}
@@ -111,8 +151,8 @@ void Robot::keyPressEvent(QKeyEvent* evt, bool press)
if(manual)
{
- float dinc = 0.1;
- float ainc = 0.003;
+ float dinc = .5;
+ float ainc = 0.005;
IF_KEYSWITCH(avant,evt->key() == Qt::Key_Up)
deriv.position.x += dinc;
@@ -122,7 +162,6 @@ void Robot::keyPressEvent(QKeyEvent* evt, bool press)
deriv.angle += ainc;
IF_KEYSWITCH(droite,evt->key() == Qt::Key_Left)
deriv.angle -= ainc;
- std::cout << deriv.position.x.getValueInMillimeters() << " & " << deriv.angle.getValueInRadian() << std::endl;
}
}
diff --git a/src/simul/table.cpp b/src/simul/table.cpp
index f78b368e..d570d655 100644
--- a/src/simul/table.cpp
+++ b/src/simul/table.cpp
@@ -3,7 +3,7 @@
#include <iostream>
#include <QPainter>
-Table::Table(QWidget* parent) : QWidget(parent)
+Table::Table(QWidget* parent) : QWidget(parent), world(b2Vec2(0.f,0.f), false)
{
dt=0;
setAutoFillBackground(true);
@@ -12,9 +12,56 @@ Table::Table(QWidget* parent) : QWidget(parent)
p.setColor(QPalette::Window,QColor(Qt::darkGray));
setPalette(p);
- robots.push_back(new Robot());
+ robots.push_back(new Robot(world));
- elements.push_back(new Element(Position(300,300),Element::Pion));
+ elements.push_back(new Element(world,Position(900,900),Element::Pion));
+
+ //Geometry
+ b2BodyDef bodyDef;
+ bodyDef.position.Set(0., 0.);
+
+ tableBody = world.CreateBody(&bodyDef);
+
+
+ b2PolygonShape box;
+ b2FixtureDef fixtureDef;
+ fixtureDef.shape = &box;
+ fixtureDef.friction = 0;
+ fixtureDef.density = 0;
+
+ box.SetAsBox(30,1, b2Vec2(0,-1),0);
+ tableBody->CreateFixture(&fixtureDef);
+
+ box.SetAsBox(1,21, b2Vec2(-1,0),0);
+ tableBody->CreateFixture(&fixtureDef);
+
+ box.SetAsBox(1,21, b2Vec2(31,0),0);
+ tableBody->CreateFixture(&fixtureDef);
+
+ box.SetAsBox(30.,1., b2Vec2(0,22),0);
+ tableBody->CreateFixture(&fixtureDef);
+
+ //Starting zones borders
+ box.SetAsBox(4.00,.11, b2Vec2(0.,4.), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+ box.SetAsBox(4.00,.11, b2Vec2(30.,4.), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+
+ //Blocked zones
+ box.SetAsBox(3.50,1.20, b2Vec2(8,21), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+ box.SetAsBox(3.50,1.20, b2Vec2(22,21), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+
+ box.SetAsBox(.11,0.65, b2Vec2(4.61,19.15), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+ box.SetAsBox(.11,0.65, b2Vec2(18.61,19.15), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+
+ box.SetAsBox(.11,0.65, b2Vec2(11.39,19.15), 0.);
+ tableBody->CreateFixture(&fixtureDef);
+ box.SetAsBox(.11,0.64, b2Vec2(25.39,19.15), 0.);
+ tableBody->CreateFixture(&fixtureDef);
}
Table::~Table()
@@ -24,6 +71,13 @@ Table::~Table()
void Table::update(int dt)
{
this->dt = dt;
+ for(unsigned int i=0; i < robots.size(); i++)
+ robots[i]->updateForces(dt);
+
+ world.Step((float)dt/1000., 10, 10);
+ world.ClearForces();
+ for(unsigned int i=0; i < elements.size(); i++)
+ elements[i]->updatePos();
repaint();
}
--
GitLab