From 484d61975b73f2501d0ae9a5369cf08d4111f5df 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 01:01:42 +0100
Subject: [PATCH] =?UTF-8?q?Asservissement=20du=20robot=20pendant=20la=20si?=
=?UTF-8?q?mulation=20avec=20la=20vitesse=20et=20non=20l=E2=80=99acc=C3=A9?=
=?UTF-8?q?leration?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
src/simul/robot.cpp | 19 ++++++++++++++-----
1 file changed, 14 insertions(+), 5 deletions(-)
diff --git a/src/simul/robot.cpp b/src/simul/robot.cpp
index a880b438..562e518d 100644
--- a/src/simul/robot.cpp
+++ b/src/simul/robot.cpp
@@ -87,11 +87,12 @@ void Robot::updateForces(int dt)
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());
+ b2Vec2 bimpulse = 0.01*rdt*b2Vec2(impulse.x.getValueInMillimeters(),impulse.y.getValueInMillimeters());
+ float bangular = deriv.angle.getValueInRadian()*rdt;
+ //body->ApplyForce(10*body->GetMass()*(bimpulse - body->GetLinearVelocity()), body->GetWorldCenter());
+ //body->ApplyTorque((bangular - body->GetAngularVelocity())*body->GetInertia());
+ body->SetLinearVelocity(bimpulse);
+ body->SetAngularVelocity(bangular);
}
@@ -104,6 +105,14 @@ void Robot::paint(QPainter &p, int dt)
pos.position.y = 100*body->GetWorldCenter().y;
pos.angle = body->GetAngle();
+ float rdt = (float)dt/1000.;
+ deriv.angle = body->GetAngularVelocity()*rdt;
+ float derx = 100*body->GetLinearVelocity().x*rdt;
+ float dery = 100*body->GetLinearVelocity().y*rdt;
+
+ deriv.position.x = derx*cos(pos.angle.getValueInRadian()) + dery*sin(pos.angle.getValueInRadian());
+ deriv.position.y = 0;
+
olds.push_back(pos);
if(manual)
{
--
GitLab