Newer
Older
Grégoire Payen de La Garanderie
committed
#include "simul/robot.h"
#include <cmath>
#include "odometrie.h"
#include "asservissement.h"
#include "sensors.h"
Grégoire Payen de La Garanderie
committed
#include <iostream>
hilnius
committed
#define ratio_qt_box2d 0.01
Xavier CORBILLON
committed
Odometrie* Odometrie::odometrie = NULL;
Grégoire Payen de La Garanderie
committed
//Odometrie class implementation for the simulation
//Yes, it's ugly ! it should not be in this file.
//But in a separate file
Odometrie::Odometrie(Robot* robot) : robot(robot)
Grégoire Payen de La Garanderie
committed
{
Xavier CORBILLON
committed
Odometrie::odometrie = this;
Grégoire Payen de La Garanderie
committed
}
Grégoire Payen de La Garanderie
committed
PositionPlusAngle Odometrie::getPos() const
Grégoire Payen de La Garanderie
committed
{
return robot->getPos();
Grégoire Payen de La Garanderie
committed
}
Grégoire Payen de La Garanderie
committed
Distance Odometrie::getVitesseLineaire() const
Grégoire Payen de La Garanderie
committed
{
return robot->getVitesseLineaire();
Grégoire Payen de La Garanderie
committed
}
Grégoire Payen de La Garanderie
committed
Angle Odometrie::getVitesseAngulaire() const
Grégoire Payen de La Garanderie
committed
{
return robot->getVitesseAngulaire();
Grégoire Payen de La Garanderie
committed
}
Grégoire Payen de La Garanderie
committed
void Odometrie::setPos(const PositionPlusAngle& p)
Grégoire Payen de La Garanderie
committed
{
robot->setPos(p);
Grégoire Payen de La Garanderie
committed
}
Grégoire Payen de La Garanderie
committed
Grégoire Payen de La Garanderie
committed
Robot::Robot(b2World & world, PositionPlusAngle depart, bool manual) : world(world), olds(10000)
Grégoire Payen de La Garanderie
committed
{
leftUpperHammerStatus = 0;
leftLowerHammerStatus = 0;
rightUpperHammerStatus = 0;
rightLowerHammerStatus = 0;
Grégoire Payen de La Garanderie
committed
Grégoire Payen de La Garanderie
committed
odometrie = new Odometrie(this);
Victor Dubois
committed
//new Sensors();
Xavier Corbillon
committed
Xavier Corbillon
committed
asservissement = new Asservissement(odometrie);
Xavier Corbillon
committed
Xavier CORBILLON
committed
//asservissement->strategie = strategie;
Grégoire Payen de La Garanderie
committed
deriv.position.x = 0;
deriv.position.y = 0;
deriv.angle = 0;
hilnius
committed
/// Déclaration graphique (QT)
int inc = 0;
robotPolygonPoints[inc++] = QPoint(24,0);
robotPolygonPoints[inc++] = QPoint(36,30);
robotPolygonPoints[inc++] = QPoint(45,36);
robotPolygonPoints[inc++] = QPoint(66,42);
robotPolygonPoints[inc++] = QPoint(108,42);
robotPolygonPoints[inc++] = QPoint(133,87);
robotPolygonPoints[inc++] = QPoint(53,160);
robotPolygonPoints[inc++] = QPoint(-25,160);
robotPolygonPoints[inc++] = QPoint(-95,140);
robotPolygonPoints[inc++] = QPoint(-95,-140);
robotPolygonPoints[inc++] = QPoint(-25,-160);
robotPolygonPoints[inc++] = QPoint(53,-160);
robotPolygonPoints[inc++] = QPoint(133,-87);
robotPolygonPoints[inc++] = QPoint(108,-42);
robotPolygonPoints[inc++] = QPoint(66,-42);
robotPolygonPoints[inc++] = QPoint(45,-36);
robotPolygonPoints[inc++] = QPoint(36,-30);
/// Déclaration Physique (Box2D)
b2BodyDef bodyDef;
Grégoire Payen de La Garanderie
committed
#ifndef BOX2D_2_0_1
bodyDef.type = b2_dynamicBody;
#endif
bodyDef.position.Set(pos.position.x/100., pos.position.y/100.);
Grégoire Payen de La Garanderie
committed
bodyDef.angle = pos.angle;
body = world.CreateBody(&bodyDef);
hilnius
committed
Grégoire Payen de La Garanderie
committed
#ifdef BOX2D_2_0_1
Grégoire Payen de La Garanderie
committed
b2PolygonDef &fixture = box;
#define CreateFixture CreateShape
#else
b2PolygonShape box;
b2FixtureDef fixture;
fixture.shape = &box;
#endif
fixture.density = 10.0f;
fixture.friction = 1.0f;
Grégoire Payen de La Garanderie
committed
hilnius
committed
// On crée la "box" de base du robot : void b2PolygonShape::SetAsBox(float32 hx,float32 hy,const b2Vec2 & center,float32 angle)
box.SetAsBox(60*ratio_qt_box2d,42*ratio_qt_box2d,b2Vec2((24.f-60.f)*ratio_qt_box2d,0),0);
Grégoire Payen de La Garanderie
committed
body->CreateFixture(&fixture);
Grégoire Payen de La Garanderie
committed
Grégoire Payen de La Garanderie
committed
#ifdef BOX2D_2_0_1
b2Vec2* v = box.vertices;
box.vertexCount = 6;
Grégoire Payen de La Garanderie
committed
#else
b2Vec2 v[6];
Grégoire Payen de La Garanderie
committed
#endif
hilnius
committed
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
inc = 0;
v[inc++].Set(-95.f*ratio_qt_box2d,42.f*ratio_qt_box2d);
v[inc++].Set(108.f*ratio_qt_box2d,42.f*ratio_qt_box2d);
v[inc++].Set(133.f*ratio_qt_box2d,87.f*ratio_qt_box2d);
v[inc++].Set(53.f*ratio_qt_box2d,160.f*ratio_qt_box2d);
v[inc++].Set(-25.f*ratio_qt_box2d,160.f*ratio_qt_box2d);
v[inc++].Set(-95.f*ratio_qt_box2d,140.f*ratio_qt_box2d);
Grégoire Payen de La Garanderie
committed
#ifndef BOX2D_2_0_1
hilnius
committed
box.Set(v, 6);
Grégoire Payen de La Garanderie
committed
#endif
Grégoire Payen de La Garanderie
committed
body->CreateFixture(&fixture);
hilnius
committed
// on déclare les points de la deuxieme partie : Attention, doit être convexe et orientée dans le sens indirect
inc = 0;
v[inc++].Set(-95.f*ratio_qt_box2d,-140.f*ratio_qt_box2d);
v[inc++].Set(-25.f*ratio_qt_box2d,-160.f*ratio_qt_box2d);
v[inc++].Set(53.f*ratio_qt_box2d,-160.f*ratio_qt_box2d);
hilnius
committed
v[inc++].Set(133.f*ratio_qt_box2d,-87.f*ratio_qt_box2d);
v[inc++].Set(108.f*ratio_qt_box2d,-42.f*ratio_qt_box2d);
v[inc++].Set(-95.f*ratio_qt_box2d,-42.f*ratio_qt_box2d);
Grégoire Payen de La Garanderie
committed
#ifndef BOX2D_2_0_1
hilnius
committed
box.Set(v, 6);
Grégoire Payen de La Garanderie
committed
#endif
Grégoire Payen de La Garanderie
committed
Grégoire Payen de La Garanderie
committed
#ifdef BOX2D_2_0_1
Grégoire Payen de La Garanderie
committed
#endif
Grégoire Payen de La Garanderie
committed
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
//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);
// SENSORS :
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp avant droit
inc = 0;
v[inc++].Set(133.f*ratio_qt_box2d,-87.f*ratio_qt_box2d);
v[inc++].Set(183.f*ratio_qt_box2d,-87.f*ratio_qt_box2d);
v[inc++].Set(188.f*ratio_qt_box2d,-42.f*ratio_qt_box2d);
v[inc++].Set(108.f*ratio_qt_box2d,-42.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
body->CreateFixture(&fixture);// */
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp avant gauche
inc = 0;
v[inc++].Set(108.f*ratio_qt_box2d,42.f*ratio_qt_box2d);
v[inc++].Set(188.f*ratio_qt_box2d,42.f*ratio_qt_box2d);
v[inc++].Set(183.f*ratio_qt_box2d,87.f*ratio_qt_box2d);
v[inc++].Set(133.f*ratio_qt_box2d,87.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
body->CreateFixture(&fixture);// */
}
Robot::~Robot()
{
delete asservissement;
delete odometrie;
delete strategie;
world.DestroyBody(body);
}
void Robot::updateForces(int dt)
{
if(dt == 0)
return;
Position impulse;
Grégoire Payen de La Garanderie
committed
impulse.x = (deriv.position.x*(float)cos(pos.angle) - deriv.position.y*(float)sin(pos.angle));
impulse.y = (deriv.position.x*(float)sin(pos.angle) + deriv.position.y*(float)cos(pos.angle));
float32 rdt = 1000./(float)dt;
b2Vec2 bvelocity = 0.01*rdt*b2Vec2(impulse.x,impulse.y);
Grégoire Payen de La Garanderie
committed
float bangular = deriv.angle*rdt;
Grégoire Payen de La Garanderie
committed
//body->ApplyForce(10*body->GetMass()*(bimpulse - body->GetLinearVelocity()), body->GetWorldCenter());
//body->ApplyTorque((bangular - body->GetAngularVelocity())*body->GetInertia());
Grégoire Payen de La Garanderie
committed
body->SetLinearVelocity(bvelocity);
Grégoire Payen de La Garanderie
committed
body->SetAngularVelocity(bangular);
Grégoire Payen de La Garanderie
committed
}
Grégoire Payen de La Garanderie
committed
void Robot::paint(QPainter &p, int dt)
{
if(dt)
Grégoire Payen de La Garanderie
committed
pos.position.x = 100*body->GetPosition().x;
pos.position.y = 100*body->GetPosition().y;
pos.angle = body->GetAngle();
Grégoire Payen de La Garanderie
committed
float rdt = (float)dt/1000.;
deriv.angle = body->GetAngularVelocity()*rdt;
float derx = 100*body->GetLinearVelocity().x*rdt;
float dery = 100*body->GetLinearVelocity().y*rdt;
Grégoire Payen de La Garanderie
committed
deriv.position.x = derx*cos(pos.angle) + dery*sin(pos.angle);
Grégoire Payen de La Garanderie
committed
deriv.position.y = 0;
hilnius
committed
//olds.push_back(pos);
if(manual)
{
keyPressEvent(NULL,false);
deriv.position.x = deriv.position.x* 0.97f;
deriv.angle = deriv.angle * 0.9;
}
else
{
Odometrie::odometrie->update();
StrategieV2::update();
Asservissement::asservissement->update();
deriv.position.x = asservissement->getLinearSpeed();
deriv.position.y = 0;
deriv.angle = asservissement->getAngularSpeed();
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
// hammers update
if (leftLowerHammerStatus == 100)
leftLowerHammerStatus = 0;
if (rightLowerHammerStatus == 100)
rightLowerHammerStatus = 0;
if (leftUpperHammerStatus == 200)
leftUpperHammerStatus = 0;
if (rightUpperHammerStatus == 200)
rightUpperHammerStatus = 0;
if (leftLowerHammerStatus > 0)
leftLowerHammerStatus += (int)(dt/2);
if (rightLowerHammerStatus > 0)
rightLowerHammerStatus += (int)(dt/2);
if (leftUpperHammerStatus > 0)
leftUpperHammerStatus += (int)(dt);
if (rightUpperHammerStatus > 0)
rightUpperHammerStatus += (int)(dt);
if (leftLowerHammerStatus > 100)
leftLowerHammerStatus = 100;
if (rightLowerHammerStatus > 100)
rightLowerHammerStatus = 100;
if (leftUpperHammerStatus > 200)
leftUpperHammerStatus = 200;
if (rightUpperHammerStatus > 200)
rightUpperHammerStatus = 200;
Grégoire Payen de La Garanderie
committed
p.setWorldTransform(QTransform().translate(pos.position.x,-pos.position.y).rotateRadians(-pos.angle));
Grégoire Payen de La Garanderie
committed
p.setPen(QColor(Qt::black));
p.setBrush(QBrush(QColor(90,90,90)));
p.setOpacity(.3);
Grégoire Payen de La Garanderie
committed
hilnius
committed
// on peint le robot.
p.drawConvexPolygon(robotPolygonPoints, 17);
p.drawRect(-20, 160, 20, rightUpperHammerStatus);
p.drawRect(0, 160, 20, rightLowerHammerStatus);
p.drawRect(-20, -160, 20, -leftUpperHammerStatus);
p.drawRect(0, -160, 20, -leftLowerHammerStatus);
Xavier CORBILLON
committed
if(true)//On dessine les champs des capteurs
{
p.setPen(QColor(Qt::red));
p.setBrush(QBrush(QColor(255,0,0)));
p.setOpacity(.2);
QPolygon polygon;
polygon << QPoint(133.f,-87.f) << QPoint(183.f,-87.f)
<< QPoint(188.f,-42.f) << QPoint(108.f,-42.f);
p.drawPolygon(polygon);
QPolygon polygon2;
polygon2 << QPoint(133.f,87.f) << QPoint(183.f,87.f)
<< QPoint(188.f,42.f) << QPoint(108.f,42.f);
p.drawPolygon(polygon2);
p.setPen(QColor(Qt::black));
p.setBrush(QBrush(QColor(90,90,90)));
p.setOpacity(.3);
}
Xavier CORBILLON
committed
// p.drawChord(-103/2 + 104, -107, 2*103, 215, 16*90, 16*180);
Grégoire Payen de La Garanderie
committed
//p.drawRect(-268, -179.5, 268, 359);
//drawTriangle(p, 0, 0, 65, 0, 60, 0);
p.setOpacity(1);
Grégoire Payen de La Garanderie
committed
p.setPen(QColor(Qt::red));
p.drawLine(0,0,pos.position.x,0);
Grégoire Payen de La Garanderie
committed
p.drawLine(0,100*pos.angle,0,0);
Grégoire Payen de La Garanderie
committed
p.setWorldTransform(QTransform());
p.setPen(QColor(Qt::green));
for(unsigned int i=0; i+1 < olds.size(); i++)
p.drawLine(olds[i].position.x, -olds[i].position.y, olds[i+1].position.x, -olds[i+1].position.y);
Grégoire Payen de La Garanderie
committed
}
#define IF_KEYSWITCH(n,a) \
static bool n=false; \
if(evt) n= a ? press : n; if(n)
void Robot::keyPressEvent(QKeyEvent* evt, bool press)
{
if(evt && press && evt->text() == "e" && !evt->isAutoRepeat())
manual = !manual;
if(evt && press && evt->text() == "u" && !evt->isAutoRepeat())
{
level = (level != 100) ? 100 : 0;
#ifdef BOX2D_2_0_1
#define b2Filter b2FilterData
#define GetFixtureList GetShapeList
#endif
b2Filter filter;
if(level == 100)
{
filter.categoryBits = 0x4;
filter.maskBits = 0x3;
}
else
{
filter.maskBits = 0x3;
filter.categoryBits = 0x1;
}
}
if(manual)
{
IF_KEYSWITCH(avant,evt->key() == Qt::Key_Up)
deriv.position.x += dinc;
IF_KEYSWITCH(arriere,evt->key() == Qt::Key_Down)
deriv.position.x -= dinc;
IF_KEYSWITCH(gauche,evt->key() == Qt::Key_Right)
deriv.angle -= ainc;
Grégoire Payen de La Garanderie
committed
IF_KEYSWITCH(droite,evt->key() == Qt::Key_Left)
deriv.angle += ainc;
IF_KEYSWITCH(leftUpperHammerUp,evt->key() == Qt::Key_W)
leftUpperHammerStatus = 1;
IF_KEYSWITCH(leftUpperHammerDown,evt->key() == Qt::Key_X)
leftLowerHammerStatus = 1;
IF_KEYSWITCH(rightUpperHammerUp,evt->key() == Qt::Key_C)
rightUpperHammerStatus = 1;
IF_KEYSWITCH(rightUpperHammerDown,evt->key() == Qt::Key_V)
rightLowerHammerStatus = 1;
}
else // automated robot
{
IF_KEYSWITCH(leftUpperHammerUp,evt->key() == Qt::Key_O)
StrategieV2::willCollide();
}
void Robot::setLevel()
{
level = (level != 100) ? 100 : 0;
#ifdef BOX2D_2_0_1
#define b2Filter b2FilterData
#define GetFixtureList GetShapeList
#endif
b2Filter filter;
if(level == 100)
{
filter.categoryBits = 0x4;
filter.maskBits = 0x3;
}
else
{
filter.maskBits = 0x3;
filter.categoryBits = 0x1;
PositionPlusAngle Robot::getPos()
Xavier Corbillon
committed
float alea1 = 0., alea2 = 0., alea3 = 0.;
Xavier Corbillon
committed
/* alea1 = 2.1*(rand() % 801 -400)/1000;
alea2 = 2.1*(rand() % 801 -400)/1000;
alea3 = 2.1*(rand() % 801 -400)/6000; */
Xavier Corbillon
committed
PositionPlusAngle erreur(Position(alea1,alea2),Angle(alea3));
return (pos + erreur);
void Robot::setPos(PositionPlusAngle p)
pos = p;
return;
}
Angle Robot::getVitesseAngulaire()
{
return deriv.angle;
}
Distance Robot::getVitesseLineaire()
{
Xavier Corbillon
committed
return deriv.position.x;
QPoint Robot::getLeftUpperHammerPos() const
{
return QPoint((this->pos.position.x - (160 + leftUpperHammerStatus - 10)*sin(pos.angle)), -(this->pos.position.y + (160 + leftUpperHammerStatus-10)*cos(pos.angle)));
}
QPoint Robot::getRightUpperHammerPos() const
{
return QPoint(this->pos.position.x + (160 + rightUpperHammerStatus -10)*sin(pos.angle), -(this->pos.position.y - (160 + rightUpperHammerStatus-10)*cos(pos.angle)));
}
QPoint Robot::getLeftLowerHammerPos() const
{
return QPoint(this->pos.position.x - (160 + leftLowerHammerStatus-10)*sin(pos.angle) , -(this->pos.position.y + (160 + leftLowerHammerStatus-10)*cos(pos.angle)));
}
QPoint Robot::getRightLowerHammerPos() const
{
return QPoint(this->pos.position.x + (160 + rightLowerHammerStatus-10)*sin(pos.angle), -(this->pos.position.y - (160 + rightLowerHammerStatus-10)*cos(pos.angle)));
}
void Robot::startLeftUpperHammer()
{
leftUpperHammerStatus = 1;
}
void Robot::startRightUpperHammer()
{
rightUpperHammerStatus = 1;
}
void Robot::startLeftLowerHammer()
{
leftLowerHammerStatus = 1;
}
void Robot::startRightLowerHammer()
{
leftUpperHammerStatus = 1;
}