Newer
Older
Grégoire Payen de La Garanderie
committed
#include "simul/robot.h"
#include <cmath>
#include "odometrie.h"
#include "asservissement.h"
#include "strategie.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) : 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
hilnius
committed
manual = true;
Grégoire Payen de La Garanderie
committed
odometrie = new Odometrie(this);
Xavier Corbillon
committed
new Sensors();
Xavier Corbillon
committed
asservissement = new Asservissement(odometrie);
Xavier CORBILLON
committed
strategie = new Strategie(true, odometrie);
Xavier Corbillon
committed
Xavier CORBILLON
committed
//asservissement->strategie = strategie;
Grégoire Payen de La Garanderie
committed
pos = odometrie->getPos();
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
Grégoire Payen de La Garanderie
committed
inc = 0;
hilnius
committed
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);
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
body->CreateFixture(&fixture);
Grégoire Payen de La Garanderie
committed
#ifdef BOX2D_2_0_1
body->SetMassFromShapes();
#endif
Grégoire Payen de La Garanderie
committed
//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);
}
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
{
Asservissement::asservissement->update();
deriv.position.x = asservissement->getLinearSpeed();
deriv.position.y = 0;
deriv.angle = asservissement->getAngularSpeed();
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
// 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
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
// 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;
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)));
}