src/robomon/RobomonAtlantis.cpp

   1 /*
   2  * RobomonAtlantis.cpp                  07/10/31
   3  *
   4  * Robot`s visualization and control GUI for robot of the
   5  * Eurobot 2008 (Mission to Mars).
   6  *
   7  * Copyright: (c) 2008 CTU Dragons
   8  *            CTU FEE - Department of Control Engineering
   9  * Authors: Martin Zidek, Michal Sojka, Tran Duy Khanh
  10  * License: GNU GPL v.2
  11  */
  12
  13 #include <QtGui>
  14 #include <queue>
  15 #include <cstdlib>
  16 #include <sys/shm.h>
  17 #include <sys/stat.h>
  18 #include <sys/socket.h>
  19 #include <netinet/in.h>
  20 #include <arpa/inet.h>
  21 #include <stdio.h>
  22 #include <unistd.h>
  23 #include <math.h>
  24 #include <errno.h>
  25
  26 #include <orte.h>
  27 #include <path_planner.h>
  28 #include <robodim.h>
  29 #include <sharp.h>
  30 #include <trgen.h>
  31 #include <map.h>
  32 #include <robomath.h>
  33 #include <hokuyo.h>
  34 #include <actuators.h>
  35 #include "PlaygroundScene.h"
  36 #include "MiscGui.h"
  37 #include "robomon_orte.h"
  38 #include "RobomonAtlantis.h"
  39 #include "playgroundview.h"
  40 #include "trail.h"
  41
  42 #include <QCoreApplication>
  43 #include <QEvent>
  44 #include <QKeyEvent>
  45 #include <QDebug>
  46 #include <QMessageBox>
  47
  48 RobomonAtlantis::RobomonAtlantis(QWidget *parent)
  49         : QWidget(parent)
  50 {
  51         QFont font;
  52         font.setPointSize(7);
  53         setFont(font);
  54
  55         debugWindowEnabled = false;
  56
  57         createLeftLayout();
  58         createRightLayout();
  59
  60         QHBoxLayout *mainLayout = new QHBoxLayout;
  61         mainLayout->addLayout(leftLayout);
  62         mainLayout->addLayout(rightLayout);
  63         setLayout(mainLayout);
  64
  65         createOrte();
  66         createRobots();
  67         createActions();
  68
  69         setFocusPolicy(Qt::StrongFocus);
  70         sharedMemoryOpened = false;
  71         WDBG("Youuuhouuuu!!");
  72 }
  73
  74 /**********************************************************************
  75  * GUI
  76  **********************************************************************/
  77 void RobomonAtlantis::createLeftLayout()
  78 {
  79         leftLayout = new QVBoxLayout();
  80
  81         createDebugGroupBox();
  82         debugWindowEnabled = true;
  83         createPlaygroundGroupBox();
  84         leftLayout->addWidget(playgroundGroupBox);
  85         leftLayout->addWidget(debugGroupBox);
  86 }
  87
  88 void RobomonAtlantis::createRightLayout()
  89 {
  90         rightLayout = new QVBoxLayout();
  91         QGridLayout *layout = new QGridLayout();
  92         QVBoxLayout *vlayout = new QVBoxLayout();
  93
  94         createPositionGroupBox();
  95         createMiscGroupBox();
  96         createFSMGroupBox();
  97         createActuatorsGroupBox();
  98         createDIOGroupBox();
  99         createPowerGroupBox();
 100         createSensorsGroupBox();
 101
 102         vlayout->addWidget(positionGroupBox);
 103         vlayout->addWidget(miscGroupBox);
 104         vlayout->addWidget(fsmGroupBox);
 105         layout->addLayout(vlayout, 0, 0);
 106         layout->addWidget(actuatorsGroupBox, 0, 1);
 107 //      layout->addWidget(dioGroupBox, 0, 2);
 108
 109         rightLayout->addLayout(layout);
 110         rightLayout->addWidget(powerGroupBox);
 111         rightLayout->addWidget(sensorsGroupBox);
 112 }
 113
 114 void RobomonAtlantis::createPlaygroundGroupBox()
 115 {
 116         playgroundGroupBox = new QGroupBox(tr("Playground"));
 117         QHBoxLayout *layout = new QHBoxLayout();
 118
 119         playgroundScene = new PlaygroundScene();
 120         playgroundSceneView = new PlaygroundView(playgroundScene);
 121         //playgroundSceneView->setMinimumWidth(630);
 122         //playgroundSceneView->setMinimumHeight(445);
 123         playgroundSceneView->setMatrix(QMatrix(1,0,0,-1,0,0), true);
 124         playgroundSceneView->fitInView(playgroundScene->itemsBoundingRect());
 125         playgroundSceneView->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
 126         layout->addWidget(playgroundSceneView);
 127
 128         playgroundGroupBox->setLayout(layout);
 129 }
 130
 131 void RobomonAtlantis::createPositionGroupBox()
 132 {
 133         positionGroupBox = new QGroupBox(tr("Position state"));
 134         positionGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 135         QGridLayout *layout = new QGridLayout();
 136
 137         actPosX = new QLineEdit();
 138         actPosY = new QLineEdit();
 139         actPosPhi = new QLineEdit();
 140
 141         estPosX = new QLineEdit();
 142         estPosY = new QLineEdit();
 143         estPosPhi = new QLineEdit();
 144
 145         actPosX->setReadOnly(true);
 146         actPosY->setReadOnly(true);
 147         actPosPhi->setReadOnly(true);
 148
 149         estPosX->setReadOnly(true);
 150         estPosY->setReadOnly(true);
 151         estPosPhi->setReadOnly(true);
 152
 153         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 154         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 155         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 156
 157         layout->addWidget(MiscGui::createLabel("X"), 5, 0);
 158         layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
 159         layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
 160
 161         layout->addWidget(MiscGui::createLabel("Reference", Qt::AlignLeft), 0, 1);
 162         layout->addWidget(actPosX, 1, 1);
 163         layout->addWidget(actPosY, 2, 1);
 164         layout->addWidget(actPosPhi, 3, 1);
 165
 166         layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1);
 167         layout->addWidget(estPosX, 5, 1);
 168         layout->addWidget(estPosY, 6, 1);
 169         layout->addWidget(estPosPhi, 7, 1);
 170
 171         positionGroupBox->setLayout(layout);
 172 }
 173
 174 void RobomonAtlantis::createMiscGroupBox()
 175 {
 176         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 177         miscGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 178         QGridLayout *layout = new QGridLayout();
 179
 180         obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
 181         obstacleSimulationCheckBox->setShortcut(tr("o"));
 182         layout->addWidget(obstacleSimulationCheckBox);
 183
 184         startPlug = new QCheckBox("Start plug");
 185         layout->addWidget(startPlug);
 186
 187         puckInside = new QCheckBox("Puck inside");
 188         layout->addWidget(puckInside);
 189
 190         miscGroupBox->setLayout(layout);
 191 }
 192
 193 void RobomonAtlantis::createFSMGroupBox()
 194 {
 195         fsmGroupBox = new QGroupBox(tr("FSM"));
 196         fsmGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 197         QGridLayout *layout = new QGridLayout();
 198
 199         layout->addWidget(MiscGui::createLabel("Main:"), 1, 0);
 200         fsm_main_state = new QLabel();
 201         fsm_main_state->setMinimumWidth(100);
 202         layout->addWidget(fsm_main_state, 1, 1);
 203
 204         layout->addWidget(MiscGui::createLabel("Act:"), 2, 0);
 205         fsm_act_state = new QLabel();
 206         layout->addWidget(fsm_act_state, 2, 1);
 207
 208         layout->addWidget(MiscGui::createLabel("Motion:"), 3, 0);
 209         fsm_motion_state = new QLabel();
 210         layout->addWidget(fsm_motion_state, 3, 1);
 211
 212         fsmGroupBox->setLayout(layout);
 213 }
 214
 215 void RobomonAtlantis::createDebugGroupBox()
 216 {
 217         debugGroupBox = new QGroupBox(tr("Debug window"));
 218         debugGroupBox->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Maximum);
 219         QHBoxLayout *layout = new QHBoxLayout();
 220
 221         debugWindow = new QTextEdit();
 222         debugWindow->setReadOnly(true);
 223
 224         layout->addWidget(debugWindow);
 225         debugGroupBox->setLayout(layout);
 226 }
 227
 228 void RobomonAtlantis::createActuatorsGroupBox()
 229 {
 230         actuatorsGroupBox = new QGroupBox(tr("Actuators"));
 231         actuatorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Preferred);
 232         QHBoxLayout *layout = new QHBoxLayout();
 233
 234         createMotorsGroupBox();
 235         createPickerGroupBox();
 236
 237         layout->setAlignment(Qt::AlignLeft);
 238         layout->addWidget(enginesGroupBox);
 239         layout->addWidget(pickerGroupBox);
 240         actuatorsGroupBox->setLayout(layout);
 241 }
 242
 243 void RobomonAtlantis::createDIOGroupBox()
 244 {
 245         dioGroupBox = new QGroupBox(tr("DIO"));
 246         QGridLayout *layout = new QGridLayout();
 247         QFont font;
 248
 249         font.setPointSize(5);
 250         dioGroupBox->setFont(font);
 251
 252         for (int i=0; i<8; i++) {
 253                 diCheckBox[i] = new QCheckBox(QString("DI%1").arg(i));
 254                 doCheckBox[i] = new QCheckBox(QString("D0%1").arg(i));
 255                 layout->addWidget(diCheckBox[i], i, 0);
 256                 layout->addWidget(doCheckBox[i], i+8, 0);
 257         }
 258
 259         dioGroupBox->setMaximumWidth(70);
 260         dioGroupBox->setLayout(layout);
 261 }
 262
 263 void RobomonAtlantis::createPowerGroupBox()
 264 {
 265         powerGroupBox = new QGroupBox(tr("Power management"));
 266         powerGroupBox->setSizePolicy(QSizePolicy::Minimum, QSizePolicy::Minimum);
 267         powerGroupBox->setMaximumWidth(450);
 268         QGridLayout *layout = new QGridLayout();
 269
 270         voltage33CheckBox = new QCheckBox(tr("&3.3V"));
 271         voltage50CheckBox = new QCheckBox(tr("&5.0V"));
 272         voltage80CheckBox = new QCheckBox(tr("&8.0V"));
 273
 274         voltage33CheckBox->setShortcut(tr("3"));
 275         voltage50CheckBox->setShortcut(tr("5"));
 276         voltage80CheckBox->setShortcut(tr("8"));
 277
 278         layout->addWidget(voltage33CheckBox, 0, 0);
 279         layout->addWidget(voltage50CheckBox, 0, 2);
 280         layout->addWidget(voltage80CheckBox, 0, 4); //1, 0);
 281         layout->addWidget(MiscGui::createLabel("BAT"), 0, 6); //1, 2);
 282
 283         voltage33LineEdit = new QLineEdit();
 284         voltage50LineEdit = new QLineEdit();
 285         voltage80LineEdit = new QLineEdit();
 286         voltageBATLineEdit = new QLineEdit();
 287
 288         voltage33LineEdit->setReadOnly(true);
 289         voltage50LineEdit->setReadOnly(true);
 290         voltage80LineEdit->setReadOnly(true);
 291         voltageBATLineEdit->setReadOnly(true);
 292
 293         layout->addWidget(voltage33LineEdit, 0, 1);
 294         layout->addWidget(voltage50LineEdit, 0, 3);
 295         layout->addWidget(voltage80LineEdit, 0, 5); //1, 1);
 296         layout->addWidget(voltageBATLineEdit, 0, 7); //1, 3);
 297
 298         powerGroupBox->setLayout(layout);
 299 }
 300
 301 void RobomonAtlantis::createSensorsGroupBox()
 302 {
 303         sensorsGroupBox = new QGroupBox(tr("Sensors"));
 304         sensorsGroupBox->setSizePolicy(QSizePolicy::Fixed, QSizePolicy::Fixed);
 305         sensorsGroupBox->setMaximumWidth(450);
 306         QHBoxLayout *layout = new QHBoxLayout();
 307
 308         layout->addWidget(MiscGui::createLabel("Puck Sharp"));
 309         sharpPuck = new QProgressBar();
 310         sharpPuck->setFormat("%v mm");
 311         sharpPuck->setTextVisible(true);
 312         sharpPuck->setMaximum(250);
 313         layout->addWidget(sharpPuck);
 314
 315         layout->addWidget(MiscGui::createLabel("Lift"));
 316         liftPos = new QProgressBar();
 317         liftPos->setFormat("%v IRC");
 318         liftPos->setTextVisible(true);
 319         liftPos->setMaximum(1000);
 320         layout->addWidget(liftPos);
 321
 322         layout->addWidget(MiscGui::createLabel("Pusher"));
 323         pusherPos = new QProgressBar();
 324         pusherPos->setFormat("%v IRC");
 325         pusherPos->setTextVisible(true);
 326         pusherPos->setMaximum(250);
 327         layout->addWidget(pusherPos);
 328
 329         sensorsGroupBox->setLayout(layout);
 330 }
 331
 332 void RobomonAtlantis::createMotorsGroupBox()
 333 {
 334         enginesGroupBox = new QGroupBox(tr("Motors"));
 335         QVBoxLayout *layout = new QVBoxLayout();
 336         QHBoxLayout *layout1 = new QHBoxLayout();
 337         QHBoxLayout *layout2 = new QHBoxLayout();
 338
 339         leftMotorSlider = new QSlider(Qt::Vertical);
 340         rightMotorSlider = new QSlider(Qt::Vertical);
 341         bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
 342         stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
 343
 344         leftMotorSlider->setMinimum(-100);
 345         leftMotorSlider->setMaximum(100);
 346         leftMotorSlider->setTracking(false);
 347         leftMotorSlider->setTickPosition(QSlider::TicksLeft);
 348
 349         rightMotorSlider->setMinimum(-100);
 350         rightMotorSlider->setMaximum(100);
 351         rightMotorSlider->setTracking(false);
 352         rightMotorSlider->setTickPosition(QSlider::TicksRight);
 353
 354         stopMotorsPushButton->setMaximumWidth(90);
 355
 356         layout1->addWidget(leftMotorSlider);
 357         layout1->addWidget(MiscGui::createLabel("0"));
 358         layout1->addWidget(rightMotorSlider);
 359
 360         layout2->addWidget(bothMotorsCheckBox);
 361
 362         layout->addWidget(MiscGui::createLabel("100"));
 363         layout->addLayout(layout1);
 364         layout->addWidget(MiscGui::createLabel("-100"));
 365         layout->addLayout(layout2);
 366         layout->addWidget(stopMotorsPushButton);
 367         enginesGroupBox->setLayout(layout);
 368 }
 369
 370 void RobomonAtlantis::createPickerGroupBox()
 371 {
 372         pickerGroupBox = new QGroupBox(tr("Picker"));
 373         QVBoxLayout *layout = new QVBoxLayout();
 374
 375         leftBeltCheckBox = new QCheckBox(tr("L Belt"));
 376         rightBeltCheckBox = new QCheckBox(tr("R Belt"));
 377         leftChelaCheckBox = new QCheckBox(tr("L Chela"));
 378         rightChelaCheckBox = new QCheckBox(tr("R Chela"));
 379         leftBeltDial = new QDial();
 380         rightBeltDial = new QDial();
 381         leftChelaDial = new QDial();
 382         rightChelaDial = new QDial();
 383         pickPushButton = new QPushButton(tr("Pick!"));
 384
 385         leftBeltDial->setMinimum(0);
 386         leftBeltDial->setMaximum(200);
 387         leftBeltDial->setValue(100);
 388
 389         rightBeltDial->setMinimum(0);
 390         rightBeltDial->setMaximum(200);
 391         rightBeltDial->setValue(100);
 392
 393         leftChelaDial->setMinimum(0);
 394         leftChelaDial->setMaximum(255);
 395         leftChelaDial->setValue(127);
 396
 397         rightChelaDial->setMinimum(0);
 398         rightChelaDial->setMaximum(255);
 399         rightChelaDial->setValue(127);
 400
 401         layout->addWidget(leftChelaDial);
 402         layout->addWidget(leftChelaCheckBox);
 403         layout->addWidget(rightChelaDial);
 404         layout->addWidget(rightChelaCheckBox);
 405         layout->addWidget(leftBeltDial);
 406         layout->addWidget(leftBeltCheckBox);
 407         layout->addWidget(rightBeltDial);
 408         layout->addWidget(rightBeltCheckBox);
 409         layout->addWidget(pickPushButton);
 410
 411         pickerGroupBox->setLayout(layout);
 412 }
 413
 414 void RobomonAtlantis::createRobots()
 415 {
 416         robotActPos = new Robot("Ref", QPen(Qt::darkBlue), QBrush(Qt::NoBrush));
 417         robotActPos->setZValue(11);
 418         trailRefPos = new Trail(QPen(Qt::darkBlue));
 419         trailRefPos->setZValue(11);
 420
 421         robotEstPosOdo = new Robot("Odo", QPen(Qt::white), QBrush(Qt::darkRed));
 422         robotEstPosOdo->setZValue(10);
 423         trailOdoPos = new Trail(QPen(Qt::white));
 424         trailOdoPos->setZValue(10);
 425         robotEstPosUzv = new Robot("Uzv", QPen(), QBrush(Qt::darkGray));
 426         robotEstPosUzv->setZValue(10);
 427         trailUzvPos = new Trail(QPen());
 428         trailUzvPos->setZValue(10);
 429
 430         playgroundScene->addItem(robotActPos);
 431         playgroundScene->addItem(robotEstPosUzv);
 432         playgroundScene->addItem(robotEstPosOdo);
 433
 434         showTrails(false);
 435
 436         playgroundScene->addItem(trailRefPos);
 437         playgroundScene->addItem(trailUzvPos);
 438         playgroundScene->addItem(trailOdoPos);
 439 }
 440
 441 /**********************************************************************
 442  * GUI actions
 443  **********************************************************************/
 444 void RobomonAtlantis::createActions()
 445 {
 446         /* power management */
 447         connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
 448                         this, SLOT(setVoltage33(int)));
 449         connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
 450                         this, SLOT(setVoltage50(int)));
 451         connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
 452                         this, SLOT(setVoltage80(int)));
 453
 454         /* motors */
 455         connect(leftMotorSlider, SIGNAL(valueChanged(int)),
 456                         this, SLOT(setLeftMotor(int)));
 457         connect(rightMotorSlider, SIGNAL(valueChanged(int)),
 458                         this, SLOT(setRightMotor(int)));
 459         connect(stopMotorsPushButton, SIGNAL(clicked()),
 460                         this, SLOT(stopMotors()));
 461
 462         /* DIO */
 463         for (int i=0; i<8; i++)
 464                 connect(doCheckBox[0], SIGNAL(stateChanged(int)),
 465                                         this, SLOT(setDO(int)));
 466
 467         connect(startPlug, SIGNAL(stateChanged(int)), this, SLOT(sendStart(int)));
 468         connect(puckInside, SIGNAL(stateChanged(int)), this, SLOT(sendPuckInside(int)));
 469
 470         /* obstacle simulation */
 471         simulationEnabled = 0;
 472         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 473                         this, SLOT(setSimulation(int)));
 474         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 475                         this, SLOT(setObstacleSimulation(int)));
 476         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 477                         playgroundScene, SLOT(showObstacle(int)));
 478         connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
 479                         this, SLOT(changeObstacle(QPointF)));
 480
 481         /* Actuators */
 482         connect(pickPushButton, SIGNAL(clicked()), this, SLOT(pick()));
 483         connect(leftBeltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setBelts(int)));
 484         connect(rightBeltCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setBelts(int)));
 485         connect(leftChelaCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setChelae(int)));
 486         connect(rightChelaCheckBox, SIGNAL(stateChanged(int)), this, SLOT(setChelae(int)));
 487         connect(leftBeltDial, SIGNAL(valueChanged(int)), this, SLOT(setBelts(int)));
 488         connect(rightBeltDial, SIGNAL(valueChanged(int)), this, SLOT(setBelts(int)));
 489         connect(leftChelaDial, SIGNAL(valueChanged(int)), this, SLOT(setChelae(int)));
 490         connect(rightChelaDial, SIGNAL(valueChanged(int)), this, SLOT(setChelae(int)));
 491 }
 492
 493 void RobomonAtlantis::setVoltage33(int state)
 494 {
 495         if (state)
 496                 orte.pwr_ctrl.voltage33 = true;
 497         else
 498                 orte.pwr_ctrl.voltage33 = false;
 499 }
 500
 501 void RobomonAtlantis::setVoltage50(int state)
 502 {
 503         if (state)
 504                 orte.pwr_ctrl.voltage50 = true;
 505         else
 506                 orte.pwr_ctrl.voltage50 = false;
 507 }
 508
 509 void RobomonAtlantis::setVoltage80(int state)
 510 {
 511         if (state)
 512                 orte.pwr_ctrl.voltage80 = true;
 513         else
 514                 orte.pwr_ctrl.voltage80 = false;
 515 }
 516
 517 void RobomonAtlantis::setLeftMotor(int value)
 518 {
 519         short int leftMotor;
 520         short int rightMotor;
 521
 522         if(bothMotorsCheckBox->isChecked())
 523                 rightMotorSlider->setValue(value);
 524
 525         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 526         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 527
 528         orte.motion_speed.left = leftMotor;
 529         orte.motion_speed.right = rightMotor;
 530
 531 }
 532
 533 void RobomonAtlantis::setRightMotor(int value)
 534 {
 535         short int leftMotor;
 536         short int rightMotor;
 537
 538         if(bothMotorsCheckBox->isChecked())
 539                 leftMotorSlider->setValue(value);
 540
 541         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 542         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 543
 544         orte.motion_speed.left = leftMotor;
 545         orte.motion_speed.right = rightMotor;
 546
 547 }
 548
 549 void RobomonAtlantis::stopMotors()
 550 {
 551         leftMotorSlider->setValue(0);
 552         rightMotorSlider->setValue(0);
 553 }
 554
 555
 556 void RobomonAtlantis::setDO(int state)
 557 {
 558         Q_UNUSED(state);
 559         /* FIXME: digital output control comes here */
 560 }
 561
 562 void RobomonAtlantis::pick()
 563 {
 564         // TODO: send signal to the fsmact
 565 }
 566
 567 void RobomonAtlantis::setBelts(int value)
 568 {
 569         Q_UNUSED(value);
 570         unsigned char leftBelt;
 571         unsigned char rightBelt;
 572
 573         leftBelt = (unsigned char)leftBeltDial->value();
 574         rightBelt = (unsigned char)rightBeltDial->value();
 575
 576         act_belts(leftBelt, rightBelt);
 577 }
 578
 579 void RobomonAtlantis::setChelae(int value)
 580 {
 581         Q_UNUSED(value);
 582         unsigned char leftChela;
 583         unsigned char rightChela;
 584
 585         leftChela = (unsigned char)leftChelaDial->value();
 586         rightChela = (unsigned char)rightChelaDial->value();
 587
 588         act_chelae(leftChela, rightChela);
 589 }
 590
 591 void RobomonAtlantis::showMap(bool show)
 592 {
 593         openSharedMemory();
 594
 595         if (sharedMemoryOpened == false)
 596                 return;
 597
 598         if (show) {
 599                 mapTimer = new QTimer(this);
 600                 connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 601                 mapTimer->start(200);
 602         } else {
 603                 mapTimer->stop();
 604                 disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 605         }
 606         playgroundScene->showMap(show);
 607 }
 608
 609 void RobomonAtlantis::paintMap()
 610 {
 611         using namespace Qt;
 612         struct map *map = ShmapIsMapInit();
 613
 614         if (!map) return;
 615
 616         for(int i=0; i < MAP_WIDTH; i++) {
 617                 for(int j=0; j<MAP_HEIGHT; j++) {
 618                         QColor color;
 619
 620                         struct map_cell *cell = &map->cells[j][i];
 621                         color = lightGray;
 622
 623                         if ((cell->flags & MAP_FLAG_WALL) &&
 624                             (cell->flags & MAP_FLAG_INVALIDATE_WALL) == 0)
 625                                 color = darkYellow;
 626                         if (cell->flags & MAP_FLAG_IGNORE_OBST)
 627                                 color = darkGreen;
 628                         if (cell->flags & MAP_FLAG_SIMULATED_WALL)
 629                                 color = yellow;
 630                         if (cell->flags & MAP_FLAG_PATH)
 631                                 color = darkRed;
 632                         if (cell->flags & MAP_FLAG_START)
 633                                 color = red;
 634                         if (cell->flags & MAP_FLAG_GOAL)
 635                                 color = green;
 636                         if (cell->detected_obstacle) {
 637                                 QColor c1(color), c2(blue);
 638                                 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
 639                                 QColor c(c1.red()   + (int)(f*(c2.red()   - c1.red())),
 640                                          c1.green() + (int)(f*(c2.green() - c1.green())),
 641                                          c1.blue()  + (int)(f*(c2.blue()  - c1.blue())));
 642                                 color = c;
 643                         }
 644                         if (cell->flags & MAP_FLAG_DET_OBST)
 645                                 color = cyan;
 646
 647                         playgroundScene->setMapColor(i, j, color);
 648                 }
 649         }
 650 }
 651
 652 void RobomonAtlantis::setSimulation(int state)
 653 {
 654         if(state) {
 655                 robottype_publisher_hokuyo_scan_create(&orte,
 656                                                        dummy_publisher_callback, this);
 657         } else {
 658                 if (!simulationEnabled)
 659                         return;
 660                 robottype_publisher_hokuyo_scan_destroy(&orte);
 661         }
 662         simulationEnabled = state;
 663 }
 664
 665 /*!
 666         \fn RobomonAtlantis::setObstacleSimulation(int state)
 667  */
 668 void RobomonAtlantis::setObstacleSimulation(int state)
 669 {
 670         if (state) {
 671                 /* TODO Maybe it is possible to attach only once to Shmap */
 672                 ShmapInit(0);
 673                 obstacleSimulationTimer = new QTimer(this);
 674                 connect(obstacleSimulationTimer, SIGNAL(timeout()),
 675                         this, SLOT(simulateObstaclesHokuyo()));
 676                 obstacleSimulationTimer->start(100);
 677                 setMouseTracking(true);
 678         } else {
 679                 if (obstacleSimulationTimer)
 680                         delete obstacleSimulationTimer;
 681                 //double distance = 0.8;
 682         }
 683 }
 684
 685
 686 void RobomonAtlantis::simulateObstaclesHokuyo()
 687 {
 688         double distance, wall_distance;
 689         int i;
 690         uint16_t *hokuyo = orte.hokuyo_scan.data;
 691
 692         for (i=0; i<HOKUYO_CLUSTER_CNT; i++) {
 693                 wall_distance = distanceToWallHokuyo(i);
 694                 distance = distanceToObstacleHokuyo(i, simulatedObstacle, SIM_OBST_SIZE_M/*meters*/);
 695                 if (wall_distance < distance)
 696                         distance = wall_distance;
 697                 hokuyo[i] = distance*1000;
 698         }
 699         ORTEPublicationSend(orte.publication_hokuyo_scan);
 700
 701 }
 702
 703 void RobomonAtlantis::changeObstacle(QPointF position)
 704 {
 705         if (!simulationEnabled) {
 706                 simulationEnabled = 1;
 707                 obstacleSimulationCheckBox->setChecked(true);
 708         }
 709
 710         simulatedObstacle.x = position.x();
 711         simulatedObstacle.y = position.y();
 712         simulateObstaclesHokuyo();
 713 }
 714
 715 /**********************************************************************
 716  * EVENTS
 717  **********************************************************************/
 718 bool RobomonAtlantis::event(QEvent *event)
 719 {
 720         switch (event->type()) {
 721                 case QEVENT(QEV_MOTION_STATUS):
 722                         emit motionStatusReceivedSignal();
 723                         break;
 724                 case QEVENT(QEV_REFERENCE_POSITION):
 725                         emit actualPositionReceivedSignal();
 726                         break;
 727                 case QEVENT(QEV_ESTIMATED_POSITION_UZV):
 728                         emit estimatedPositionReceivedSignal();
 729                         break;
 730                 case QEVENT(QEV_ESTIMATED_POSITION_ODO):
 731                         robotEstPosOdo->moveRobot(orte.est_pos_odo.x,
 732                                         orte.est_pos_odo.y, orte.est_pos_odo.phi);
 733                         trailOdoPos->addPoint(QPointF(orte.est_pos_odo.x,
 734                                               orte.est_pos_odo.y));
 735                         break;
 736                 case QEVENT(QEV_DI):
 737                         emit diReceivedSignal();
 738                         break;
 739                 case QEVENT(QEV_ACCELEROMETER):
 740                         emit accelerometerReceivedSignal();
 741                         break;
 742                 case QEVENT(QEV_ACCUMULATOR):
 743                         emit accumulatorReceivedSignal();
 744                         break;
 745                 case QEVENT(QEV_POWER_VOLTAGE):
 746                         emit powerVoltageReceivedSignal();
 747                         break;
 748                 case QEVENT(QEV_FSM_MAIN):
 749                         fsm_main_state->setText(orte.fsm_main.state_name);
 750                         break;
 751                 case QEVENT(QEV_FSM_ACT):
 752                         fsm_act_state->setText(orte.fsm_act.state_name);
 753                         break;
 754                 case QEVENT(QEV_FSM_MOTION):
 755                         fsm_motion_state->setText(orte.fsm_motion.state_name);
 756                         break;
 757                 case QEVENT(QEV_PUCK_DISTANCE):
 758                         sharpPuck->setValue(orte.puck_distance.distance*1000);
 759                         break;
 760                 case QEVENT(QEV_LIFT_PUSHER):
 761                         liftPos->setValue(orte.actuator_status.lift_pos);
 762                         pusherPos->setValue(orte.actuator_status.pusher_pos);
 763                         break;
 764                 default:
 765                         if (event->type() == QEvent::Close)
 766                                 closeEvent((QCloseEvent *)event);
 767                         else if (event->type() == QEvent::KeyPress)
 768                                 keyPressEvent((QKeyEvent *)event);
 769                         else if (event->type() == QEvent::KeyRelease)
 770                                 keyReleaseEvent((QKeyEvent *)event);
 771                         else if (event->type() == QEvent::FocusIn)
 772                                 grabKeyboard();
 773                         else if (event->type() == QEvent::FocusOut)
 774                                 releaseKeyboard();
 775                         else {
 776                                 event->ignore();
 777                                 return false;
 778                         }
 779                         break;
 780         }
 781         event->accept();
 782         return true;
 783 }
 784
 785 void RobomonAtlantis::keyPressEvent(QKeyEvent *event)
 786 {
 787         double peak, gain;
 788
 789         if (event->isAutoRepeat()) {
 790                 switch (event->key()) {
 791                         case Qt::Key_Down:
 792                                 peak = leftMotorSlider->minimum()/2;
 793                                 if (leftMotorValue < peak ||
 794                                         rightMotorValue < peak)
 795                                         gain = 1.01;
 796                                 else
 797                                         gain = 1.3;
 798                                 leftMotorValue *= gain;
 799                                 rightMotorValue *= gain;
 800                                 leftMotorSlider->setValue((int)leftMotorValue);
 801                                 rightMotorSlider->setValue((int)rightMotorValue);
 802                                 break;
 803
 804                         case Qt::Key_Up:
 805                         case Qt::Key_Left:
 806                         case Qt::Key_Right:
 807                                 peak = leftMotorSlider->maximum()/2;
 808                                 if (leftMotorValue > peak ||
 809                                         rightMotorValue > peak)
 810                                         gain = 1.01;
 811                                 else
 812                                         gain = 1.3;
 813                                 leftMotorValue *= gain;
 814                                 rightMotorValue *= gain;
 815                                 leftMotorSlider->setValue((int)leftMotorValue);
 816                                 rightMotorSlider->setValue((int)rightMotorValue);
 817                                 break;
 818
 819                         default:
 820                                 event->ignore();
 821                                 break;
 822                 }
 823                 return;
 824         }
 825
 826         switch (event->key()) {
 827                 case Qt::Key_Up:
 828                         leftMotorValue = 1;
 829                         rightMotorValue = 1;
 830                         bothMotorsCheckBox->setChecked(true);
 831                         leftMotorSlider->setValue((int)leftMotorValue);
 832                         setLeftMotor((int)leftMotorValue);
 833                         break;
 834                 case Qt::Key_Down:
 835                         leftMotorValue = -1;
 836                         rightMotorValue = -1;
 837                         bothMotorsCheckBox->setChecked(true);
 838                         leftMotorSlider->setValue((int)leftMotorValue);
 839                         setLeftMotor((int)leftMotorValue);
 840                         break;
 841                 case Qt::Key_Left:
 842                         leftMotorValue = -1;
 843                         rightMotorValue = 1;
 844                         leftMotorSlider->setValue((int)leftMotorValue);
 845                         rightMotorSlider->setValue((int)rightMotorValue);
 846                         setLeftMotor((int)leftMotorValue);
 847                         setRightMotor((int)leftMotorValue);
 848                         break;
 849                 case Qt::Key_Right:
 850                         leftMotorValue = 1;
 851                         rightMotorValue = -1;
 852                         leftMotorSlider->setValue((int)leftMotorValue);
 853                         rightMotorSlider->setValue((int)rightMotorValue);
 854                         setLeftMotor((int)leftMotorValue);
 855                         setRightMotor((int)rightMotorValue);
 856                         break;
 857                 default:
 858                         event->ignore();
 859                         break;
 860         }
 861         event->accept();
 862 }
 863
 864 void RobomonAtlantis::keyReleaseEvent(QKeyEvent *event)
 865 {
 866         if (event->isAutoRepeat()) {
 867                 event->ignore();
 868                 return;
 869         }
 870
 871         switch (event->key()) {
 872                 case Qt::Key_Up:
 873                 case Qt::Key_Down:
 874                 case Qt::Key_Left:
 875                 case Qt::Key_Right:
 876                         leftMotorValue = 0;
 877                         rightMotorValue = 0;
 878                         bothMotorsCheckBox->setChecked(false);
 879                         leftMotorSlider->setValue((int)leftMotorValue);
 880                         rightMotorSlider->setValue((int)rightMotorValue);
 881                         break;
 882                 default:
 883                         event->ignore();
 884                         break;
 885         }
 886         event->accept();
 887 }
 888
 889 void RobomonAtlantis::closeEvent(QCloseEvent *)
 890 {
 891         robottype_roboorte_destroy(&orte);
 892 }
 893
 894 /**********************************************************************
 895  * ORTE
 896  **********************************************************************/
 897 void RobomonAtlantis::createOrte()
 898 {
 899         int rv;
 900
 901         orte.strength = 11;
 902
 903         rv = robottype_roboorte_init(&orte);
 904         if (rv) {
 905                 printf("RobomonAtlantis: Unable to initialize ORTE\n");
 906         }
 907
 908         /* publishers */
 909         robottype_publisher_motion_speed_create(&orte, dummy_publisher_callback, NULL);
 910
 911         robottype_publisher_pwr_ctrl_create(&orte, dummy_publisher_callback, NULL);
 912         robottype_publisher_lift_create(&orte, NULL, &orte);
 913         robottype_publisher_pusher_create(&orte, NULL, &orte);
 914         robottype_publisher_chelae_create(&orte, NULL, &orte);
 915         robottype_publisher_belts_create(&orte, NULL, &orte);
 916         robottype_publisher_holder_create(&orte, NULL, &orte);
 917         robottype_publisher_robot_cmd_create(&orte, NULL, &orte);
 918         robottype_publisher_puck_inside_create(&orte, NULL, &orte);
 919
 920         /* subscribers */
 921         robottype_subscriber_pwr_voltage_create(&orte,
 922                                 receivePowerVoltageCallBack, this);
 923         robottype_subscriber_motion_status_create(&orte,
 924                                 receiveMotionStatusCallBack, this);
 925         robottype_subscriber_ref_pos_create(&orte,
 926                                 receiveActualPositionCallBack, this);
 927         robottype_subscriber_est_pos_uzv_create(&orte,
 928                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_UZV));
 929         robottype_subscriber_est_pos_odo_create(&orte,
 930                         generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_ODO));
 931         robottype_subscriber_fsm_main_create(&orte,
 932                                              rcv_fsm_main_cb, this);
 933         robottype_subscriber_fsm_motion_create(&orte,
 934                                              rcv_fsm_motion_cb, this);
 935         robottype_subscriber_fsm_act_create(&orte,
 936                                              rcv_fsm_act_cb, this);
 937         robottype_subscriber_puck_distance_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_PUCK_DISTANCE));
 938         robottype_subscriber_actuator_status_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_LIFT_PUSHER));
 939
 940
 941         /*createDISubscriber(this, &orteData);*/
 942         /*createAccelerometerSubscriber(this, &orteData);*/
 943         /*createAccumulatorSubscriber(this, &orteData);*/
 944         /* motors */
 945         orte.motion_speed.left = 0;
 946         orte.motion_speed.right = 0;
 947         /* power management */
 948         orte.pwr_ctrl.voltage33 = true;
 949         orte.pwr_ctrl.voltage50 = true;
 950         orte.pwr_ctrl.voltage80 = true;
 951         voltage33CheckBox->setChecked(true);
 952         voltage50CheckBox->setChecked(true);
 953         voltage80CheckBox->setChecked(true);
 954
 955         act_init(&orte);
 956
 957         /* set actions to do when we receive data from orte */
 958         connect(this, SIGNAL(motionStatusReceivedSignal()),
 959                         this, SLOT(motionStatusReceived()));
 960         connect(this, SIGNAL(actualPositionReceivedSignal()),
 961                         this, SLOT(actualPositionReceived()));
 962         connect(this, SIGNAL(estimatedPositionReceivedSignal()),
 963                         this, SLOT(estimatedPositionReceived()));
 964         connect(this, SIGNAL(diReceivedSignal()),
 965                         this, SLOT(diReceived()));
 966         connect(this, SIGNAL(accelerometerReceivedSignal()),
 967                         this, SLOT(accelerometerReceived()));
 968         connect(this, SIGNAL(accumulatorReceivedSignal()),
 969                         this, SLOT(accumulatorReceived()));
 970         connect(this, SIGNAL(powerVoltageReceivedSignal()),
 971                         this, SLOT(powerVoltageReceived()));
 972 }
 973
 974 void RobomonAtlantis::motionStatusReceived()
 975 {
 976         WDBG("ORTE received: motion status");
 977 }
 978
 979 void RobomonAtlantis::actualPositionReceived()
 980 {
 981         actPosX->setText(QString("%1").arg(orte.ref_pos.x, 0, 'f', 3));
 982         actPosY->setText(QString("%1").arg(orte.ref_pos.y, 0, 'f', 3));
 983         actPosPhi->setText(QString("%1(%2)")
 984                         .arg(DEGREES(orte.ref_pos.phi), 0, 'f', 0)
 985                         .arg(orte.ref_pos.phi, 0, 'f', 1));
 986         robotActPos->moveRobot(orte.ref_pos.x,
 987                 orte.ref_pos.y, orte.ref_pos.phi);
 988         trailRefPos->addPoint(QPointF(orte.ref_pos.x, orte.ref_pos.y));
 989 }
 990
 991 void RobomonAtlantis::estimatedPositionReceived()
 992 {
 993         estPosX->setText(QString("%1").arg(orte.est_pos_uzv.x, 0, 'f', 3));
 994         estPosY->setText(QString("%1").arg(orte.est_pos_uzv.y, 0, 'f', 3));
 995         estPosPhi->setText(QString("%1(%2)")
 996                         .arg(DEGREES(orte.est_pos_uzv.phi), 0, 'f', 0)
 997                         .arg(orte.est_pos_uzv.phi, 0, 'f', 1));
 998         robotEstPosUzv->moveRobot(orte.est_pos_uzv.x,
 999                 orte.est_pos_uzv.y, orte.est_pos_uzv.phi);
1000         trailUzvPos->addPoint(QPointF(orte.est_pos_odo.x,
1001                               orte.est_pos_odo.y));
1002 }
1003
1004 void RobomonAtlantis::diReceived()
1005 {
1006         WDBG("ORTE received: DI");
1007 }
1008
1009 void RobomonAtlantis::accelerometerReceived()
1010 {
1011         WDBG("ORTE received: accelerometer");
1012 }
1013
1014 void RobomonAtlantis::accumulatorReceived()
1015 {
1016         WDBG("ORTE received: accumulator");
1017 }
1018
1019 void RobomonAtlantis::powerVoltageReceived()
1020 {
1021         voltage33LineEdit->setText(QString("%1").arg(
1022                         orte.pwr_voltage.voltage33, 0, 'f', 3));
1023         voltage50LineEdit->setText(QString("%1").arg(
1024                         orte.pwr_voltage.voltage50, 0, 'f', 3));
1025         voltage80LineEdit->setText(QString("%1").arg(
1026                         orte.pwr_voltage.voltage80, 0, 'f', 3));
1027         voltageBATLineEdit->setText(QString("%1").arg(
1028                         orte.pwr_voltage.voltageBAT, 0, 'f', 3));
1029
1030 }
1031
1032 /**********************************************************************
1033  * MISCELLANEOUS
1034  **********************************************************************/
1035 void RobomonAtlantis::openSharedMemory()
1036 {
1037         int segmentId;
1038         int sharedSegmentSize;
1039
1040         if (sharedMemoryOpened)
1041                 return;
1042
1043         sharedSegmentSize = sizeof(unsigned int) * MAP_WIDTH * MAP_HEIGHT;
1044
1045         /* Get segment identificator in a read only mode  */
1046         segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
1047         if(segmentId == -1) {
1048                 QMessageBox::critical(this, "robomon",
1049                                 "Unable to open shared memory segment!");
1050                 return;
1051         }
1052
1053         /* Init Shmap */
1054         ShmapInit(0);
1055
1056         /* Attach the shared memory segment */
1057         //map =  (_Map*)shmat (segmentId, (void*) 0, 0);
1058
1059         sharedMemoryOpened = true;
1060 }
1061
1062 double RobomonAtlantis::distanceToWallHokuyo(int beamnum)
1063 {
1064         double distance=4.0, min_distance=4.0;
1065         int i,j;
1066         Point wall;
1067         struct map *map = ShmapIsMapInit();
1068
1069         if (!map) return min_distance;
1070
1071         // Simulate obstacles
1072         for(j=0;j<MAP_HEIGHT;j++) {
1073                 for (i=0;i<MAP_WIDTH;i++) {
1074                         struct map_cell *cell = &map->cells[j][i];
1075                         if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
1076                                 // WALL
1077                                 ShmapCell2Point(i, j, &wall.x, &wall.y);
1078
1079                                 distance = distanceToObstacleHokuyo(beamnum, wall, MAP_CELL_SIZE_M);
1080                                 if (distance<min_distance) min_distance = distance;
1081                         }
1082                 }
1083         }
1084
1085         return min_distance;
1086 }
1087
1088 /**
1089  * Calculation for Hokuyo simulation. Calculates distance that would
1090  * be returned by Hokuyo sensors, if there is only one obstacle (as
1091  * specified by parameters).
1092  *
1093  * @param beamnum Hokuyo's bean number [0..HOKUYO_CLUSTER_CNT]
1094  * @param obstacle Position of the obstacle (x, y in meters).
1095  * @param obstacleSize Size (diameter) of the obstacle in meters.
1096  *
1097  * @return Distance measured by sensors in meters.
1098  */
1099 double RobomonAtlantis::distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize)
1100 {
1101         struct robot_pos_type e = orte.est_pos_uzv;
1102         double sensor_a;
1103         struct sharp_pos s;
1104
1105         s.x = HOKUYO_CENTER_OFFSET_M;
1106         s.y = 0.0;
1107         s.ang = HOKUYO_CLUSTER_TO_RAD(beamnum);
1108
1109         Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
1110                      e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
1111         sensor_a = e.phi + s.ang;
1112
1113         const double sensorRange = 4.0; /*[meters]*/
1114
1115         double distance, angle;
1116
1117         angle = sensor.angleTo(obstacle) - sensor_a;
1118         angle = fmod(angle, 2.0*M_PI);
1119         if (angle > +M_PI) angle -= 2.0*M_PI;
1120         if (angle < -M_PI) angle += 2.0*M_PI;
1121         angle = fabs(angle);
1122         distance = sensor.distanceTo(obstacle)-0.11;
1123         if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
1124                 // We can see the obstackle from here.
1125                 if (angle < M_PI/2.0) {
1126                     distance = distance/cos(angle);
1127                 }
1128                 if (distance > sensorRange)
1129                         distance = sensorRange;
1130         } else {
1131                 distance = sensorRange;
1132         }
1133
1134         return distance;
1135 }
1136
1137 void RobomonAtlantis::sendStart(int plug)
1138 {
1139         orte.robot_cmd.start = plug ? 0 : 1;
1140         ORTEPublicationSend(orte.publication_robot_cmd);
1141 }
1142
1143 void RobomonAtlantis::sendPuckInside(int value)
1144 {
1145         orte.puck_inside.status = value ? 1 : 0;
1146         ORTEPublicationSend(orte.publication_puck_inside);
1147
1148 }
1149
1150 void RobomonAtlantis::resetTrails()
1151 {
1152         trailRefPos->reset();
1153         trailUzvPos->reset();
1154         trailOdoPos->reset();
1155 }
1156
1157 void RobomonAtlantis::showTrails(bool show)
1158 {
1159         trailRefPos->setVisible(show);
1160         trailUzvPos->setVisible(show);
1161         trailOdoPos->setVisible(show);
1162 }