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
//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 :
SharpSensor** sharpsList = strategie->getSensors();
// 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;
capteurSharpAvantDroit.type = 1;
capteurSharpAvantDroit.object = sharpsList[1];
fixture.userData=(void*)&capteurSharpAvantDroit;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
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;
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
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
255
256
257
258
259
260
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
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
capteurSharpAvantGauche.type = 1;
capteurSharpAvantGauche.object = sharpsList[0];
fixture.userData=(void*)&capteurSharpAvantGauche;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
body->CreateFixture(&fixture);
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp cote droit
inc = 0;
v[inc++].Set(53.f*ratio_qt_box2d,-160.f*ratio_qt_box2d);
v[inc++].Set(103.f*ratio_qt_box2d,-205.f*ratio_qt_box2d);
v[inc++].Set(143.f*ratio_qt_box2d,-168.5f*ratio_qt_box2d);
v[inc++].Set(93.f*ratio_qt_box2d,-123.5f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
capteurSharpcoteDroit.type = 1;
capteurSharpcoteDroit.object = sharpsList[3];
fixture.userData=(void*)&capteurSharpcoteDroit;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
body->CreateFixture(&fixture);
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp cote gauche
inc = 0;
v[inc++].Set(93.f*ratio_qt_box2d,123.5f*ratio_qt_box2d);
v[inc++].Set(143.f*ratio_qt_box2d,168.5f*ratio_qt_box2d);
v[inc++].Set(103.f*ratio_qt_box2d,205.f*ratio_qt_box2d);
v[inc++].Set(53.f*ratio_qt_box2d,160.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
capteurSharpcoteGauche.type = 1;
capteurSharpcoteGauche.object = sharpsList[2];
fixture.userData=(void*)&capteurSharpcoteGauche;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
body->CreateFixture(&fixture);
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp arriere milieu
inc = 0;
v[inc++].Set(-95.f*ratio_qt_box2d,25.f*ratio_qt_box2d);
v[inc++].Set(-145.f*ratio_qt_box2d,25.f*ratio_qt_box2d);
v[inc++].Set(-145.f*ratio_qt_box2d,-25.f*ratio_qt_box2d);
v[inc++].Set(-95.f*ratio_qt_box2d,-25.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
capteurSharpArriereMilieu.type = 1;
capteurSharpArriereMilieu.object = sharpsList[5];
fixture.userData=(void*)&capteurSharpArriereMilieu;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
body->CreateFixture(&fixture);
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp arriere droit
inc = 0;
v[inc++].Set(-95.f*ratio_qt_box2d,-90.f*ratio_qt_box2d);
v[inc++].Set(-145.f*ratio_qt_box2d,-90.f*ratio_qt_box2d);
v[inc++].Set(-145.f*ratio_qt_box2d,-140.f*ratio_qt_box2d);
v[inc++].Set(-95.f*ratio_qt_box2d,-140.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
capteurSharpArriereDroit.type = 1;
capteurSharpArriereDroit.object = sharpsList[6];
fixture.userData=(void*)&capteurSharpArriereDroit;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
body->CreateFixture(&fixture);
// on déclare les points d'une partie : Attention, doit être convexe et orientée dans le sens indirect
//Capteur sharp arriere gauche
inc = 0;
v[inc++].Set(-95.f*ratio_qt_box2d,140.f*ratio_qt_box2d);
v[inc++].Set(-145.f*ratio_qt_box2d,140.f*ratio_qt_box2d);
v[inc++].Set(-145.f*ratio_qt_box2d,90.f*ratio_qt_box2d);
v[inc++].Set(-95.f*ratio_qt_box2d,90.f*ratio_qt_box2d);
#ifndef BOX2D_2_0_1
box.Set(v, 4);
#endif
fixture.isSensor=true;
capteurSharpArriereGauche.type = 1;
capteurSharpArriereGauche.object = sharpsList[4];
fixture.userData=(void*)&capteurSharpArriereGauche;
fixture.filter.maskBits=0x8;
fixture.filter.categoryBits=0x1;
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();
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
// 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);
/*
polygonCapteurSharpAvantDroit;
polygonCapteurSharpAvantGauche;
polygonCapteurSharpcoteDroit;
polygonCapteurSharpcoteGauche;
polygonCapteurSharpArriereMilieu;
polygonCapteurSharpArriereDroit;
polygonCapteurSharpArriereGauche;*/
QPolygon polygonCapteurSharpAvantGauche;
polygonCapteurSharpAvantGauche << QPoint(133.f,-87.f) << QPoint(183.f,-87.f)
QPolygon polygonCapteurSharpAvantDroit;
polygonCapteurSharpAvantDroit << QPoint(133.f,87.f) << QPoint(183.f,87.f)
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
p.drawPolygon(polygonCapteurSharpAvantDroit);
QPolygon polygonCapteurSharpcoteGauche;
polygonCapteurSharpcoteGauche << QPoint(53.f,-160.f) << QPoint(103.f,-205.f)
<< QPoint(143.f,-168.5f) << QPoint(93.f,-123.5f);
p.drawPolygon(polygonCapteurSharpcoteGauche);
QPolygon polygonCapteurSharpcoteDroit;
polygonCapteurSharpcoteDroit << QPoint(53.f,160.f) << QPoint(103.f,205.f)
<< QPoint(143.f,168.5f) << QPoint(93.f,123.5f);
p.drawPolygon(polygonCapteurSharpcoteDroit);
QPolygon polygonCapteurSharpArriereDroit;
polygonCapteurSharpArriereDroit << QPoint(-95.f,140.f) << QPoint(-145.f,140.f)
<< QPoint(-145.f,90.f) << QPoint(-95.f,90.f);
p.drawPolygon(polygonCapteurSharpArriereDroit);
QPolygon polygonCapteurSharpArriereMilieu;
polygonCapteurSharpArriereMilieu << QPoint(-95.f,-25) << QPoint(-145.f,-25.f)
<< QPoint(-145.f,25.f) << QPoint(-95.f,25.f);
p.drawPolygon(polygonCapteurSharpArriereMilieu);
QPolygon polygonCapteurSharpArriereGauche;
polygonCapteurSharpArriereGauche << QPoint(-95.f,-140.f) << QPoint(-145.f,-140.f)
<< QPoint(-145.f,-90.f) << QPoint(-95.f,-90.f);
p.drawPolygon(polygonCapteurSharpArriereGauche);
p.setPen(QColor(Qt::black));
p.setBrush(QBrush(QColor(90,90,90)));
p.setOpacity(.3);
}
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
//On affiche les LEDs de débug
//int nbLed = sizeof(ledIsOn)/sizeof(int);
for(int numeroLed = 0 ; numeroLed<NB_LED ; numeroLed++)
{
if (isLedOn(numeroLed))
{
p.setOpacity(1);
if(numeroLed==0)
{
p.setPen(QColor("orange"));
p.setBrush(QBrush("orange"));
}
else if(numeroLed==1)
{
p.setPen(QColor("green"));
p.setBrush(QBrush("green"));
}
else
{
p.setPen(QColor(255,0,0));
p.setBrush(QBrush(QColor(255,0,0)));
}
p.drawEllipse(QPoint(0,(int)(numeroLed*100-50*(NB_LED-1))),10,-10);
}
}
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;
}