src/robomon/src2/RobomonExplorer.cpp

   1 /*
   2  * RobomonExplorer.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
  25 #include <orte.h>
  26 #include <path_planner.h>
  27 #include <robodim_eb2008.h>
  28 #include <sharp.h>
  29 #include <trgen.h>
  30 #include <map.h>
  31 #include <robomath.h>
  32 #include "PlaygroundScene.h"
  33 #include "MiscGui.h"
  34 #include "robomon_orte.h"
  35 #include "RobomonExplorer.h"
  36
  37 #include <QCoreApplication>
  38 #include <QEvent>
  39 #include <QKeyEvent>
  40 #include <QDebug>
  41 #include <QMessageBox>
  42 #include <servos_eb2008.h>
  43
  44 RobomonExplorer::RobomonExplorer(QWidget *parent)
  45         : QWidget(parent)
  46 {
  47         QFont font;
  48         font.setPointSize(7);
  49         setFont(font);
  50
  51         debugWindowEnabled = false;
  52
  53         createLeftLayout();
  54         createRightLayout();
  55
  56         QHBoxLayout *mainLayout = new QHBoxLayout;
  57         mainLayout->addLayout(leftLayout);
  58         mainLayout->addLayout(rightLayout);
  59         setLayout(mainLayout);
  60
  61         createOrte();
  62         createRobots();
  63         createActions();
  64
  65         setFocusPolicy(Qt::StrongFocus);
  66         sharedMemoryOpened = false;
  67         WDBG("Youuuhouuuu!!");
  68 }
  69
  70 /**********************************************************************
  71  * GUI
  72  **********************************************************************/
  73 void RobomonExplorer::createLeftLayout()
  74 {
  75         leftLayout = new QVBoxLayout();
  76
  77         createDebugGroupBox();
  78         debugWindowEnabled = true;
  79         createPlaygroundGroupBox();
  80
  81         leftLayout->addWidget(playgroundGroupBox);
  82         leftLayout->addWidget(debugGroupBox);
  83 }
  84
  85 void RobomonExplorer::createRightLayout()
  86 {
  87         rightLayout = new QVBoxLayout();
  88         QGridLayout *layout = new QGridLayout();
  89         QVBoxLayout *vlayout = new QVBoxLayout();
  90
  91         createPositionGroupBox();
  92         createMiscGroupBox();
  93         createActuatorsGroupBox();
  94         createDIOGroupBox();
  95         createPowerGroupBox();
  96         createSensorsGroupBox();
  97
  98         vlayout->addWidget(positionGroupBox);
  99         vlayout->addWidget(miscGroupBox);
 100         layout->addLayout(vlayout, 0, 0);
 101         layout->addWidget(actuatorsGroupBox, 0, 1);
 102 //      layout->addWidget(dioGroupBox, 0, 2);
 103
 104         rightLayout->addLayout(layout);
 105         rightLayout->addWidget(powerGroupBox);
 106         rightLayout->addWidget(sensorsGroupBox);
 107 }
 108
 109 void RobomonExplorer::createPlaygroundGroupBox()
 110 {
 111         playgroundGroupBox = new QGroupBox(tr("Playground"));
 112         QHBoxLayout *layout = new QHBoxLayout();
 113
 114         playgroundScene =
 115                 new PlaygroundScene(PlaygroundScene::PLAYGROUND_EUROBOT2008);
 116         playgroundSceneView = new QGraphicsView(playgroundScene);
 117         playgroundSceneView->setMinimumWidth((int)(playgroundScene->width())+15);
 118         playgroundSceneView->setMinimumHeight((int)(playgroundScene->height())+15);
 119         layout->addWidget(playgroundSceneView);
 120
 121         playgroundGroupBox->setLayout(layout);
 122 }
 123
 124 void RobomonExplorer::createPositionGroupBox()
 125 {
 126         positionGroupBox = new QGroupBox(tr("Position state"));
 127         QGridLayout *layout = new QGridLayout();
 128
 129         actPosX = new QLineEdit();
 130         actPosY = new QLineEdit();
 131         actPosPhi = new QLineEdit();
 132
 133         estPosX = new QLineEdit();
 134         estPosY = new QLineEdit();
 135         estPosPhi = new QLineEdit();
 136
 137         actPosX->setReadOnly(true);
 138         actPosY->setReadOnly(true);
 139         actPosPhi->setReadOnly(true);
 140
 141         estPosX->setReadOnly(true);
 142         estPosY->setReadOnly(true);
 143         estPosPhi->setReadOnly(true);
 144
 145         layout->addWidget(MiscGui::createLabel("X"), 1, 0);
 146         layout->addWidget(MiscGui::createLabel("Y"), 2, 0);
 147         layout->addWidget(MiscGui::createLabel("Phi"), 3, 0);
 148
 149         layout->addWidget(MiscGui::createLabel("X"), 5, 0);
 150         layout->addWidget(MiscGui::createLabel("Y"), 6, 0);
 151         layout->addWidget(MiscGui::createLabel("Phi"), 7, 0);
 152
 153         layout->addWidget(MiscGui::createLabel("Actual", Qt::AlignLeft), 0, 1);
 154         layout->addWidget(actPosX, 1, 1);
 155         layout->addWidget(actPosY, 2, 1);
 156         layout->addWidget(actPosPhi, 3, 1);
 157
 158         layout->addWidget(MiscGui::createLabel("Estimated", Qt::AlignLeft), 4, 1);
 159         layout->addWidget(estPosX, 5, 1);
 160         layout->addWidget(estPosY, 6, 1);
 161         layout->addWidget(estPosPhi, 7, 1);
 162
 163         positionGroupBox->setLayout(layout);
 164 }
 165
 166 void RobomonExplorer::createMiscGroupBox()
 167 {
 168         miscGroupBox = new QGroupBox(tr("Miscellaneous"));
 169         QGridLayout *layout = new QGridLayout();
 170
 171         showMapPushButton = new QPushButton(tr("Show &map"));
 172         showMapPushButton->setShortcut(tr("m"));
 173
 174         laserEngineCheckBox = new QCheckBox(tr("L&aser"));
 175         laserEngineCheckBox->setShortcut(tr("a"));
 176
 177         layout->addWidget(showMapPushButton, 0, 0);
 178         layout->addWidget(laserEngineCheckBox, 1, 0);
 179
 180         miscGroupBox->setLayout(layout);
 181 }
 182
 183 void RobomonExplorer::createDebugGroupBox()
 184 {
 185         debugGroupBox = new QGroupBox(tr("Debug window"));
 186         QHBoxLayout *layout = new QHBoxLayout();
 187
 188         debugWindow = new QTextEdit();
 189         debugWindow->setReadOnly(true);
 190
 191         layout->addWidget(debugWindow);
 192         debugGroupBox->setLayout(layout);
 193 }
 194
 195 void RobomonExplorer::createActuatorsGroupBox()
 196 {
 197         actuatorsGroupBox = new QGroupBox(tr("Actuators"));
 198         QHBoxLayout *layout = new QHBoxLayout();
 199
 200         createMotorsGroupBox();
 201         createServosGroupBox();
 202         createDrivesGroupBox();
 203
 204         layout->setAlignment(Qt::AlignLeft);
 205         layout->addWidget(enginesGroupBox);
 206         layout->addWidget(servosGroupBox);
 207         layout->addWidget(drivesGroupBox);
 208         actuatorsGroupBox->setLayout(layout);
 209 }
 210
 211 void RobomonExplorer::createDIOGroupBox()
 212 {
 213         dioGroupBox = new QGroupBox(tr("DIO"));
 214         QGridLayout *layout = new QGridLayout();
 215         QFont font;
 216
 217         font.setPointSize(5);
 218         dioGroupBox->setFont(font);
 219
 220         for (int i=0; i<8; i++) {
 221                 diCheckBox[i] = new QCheckBox(QString("DI%1").arg(i));
 222                 doCheckBox[i] = new QCheckBox(QString("D0%1").arg(i));
 223                 layout->addWidget(diCheckBox[i], i, 0);
 224                 layout->addWidget(doCheckBox[i], i+8, 0);
 225         }
 226
 227         dioGroupBox->setMaximumWidth(70);
 228         dioGroupBox->setLayout(layout);
 229 }
 230
 231 void RobomonExplorer::createPowerGroupBox()
 232 {
 233         powerGroupBox = new QGroupBox(tr("Power management"));
 234         QGridLayout *layout = new QGridLayout();
 235
 236         voltage33CheckBox = new QCheckBox(tr("&3.3V"));
 237         voltage50CheckBox = new QCheckBox(tr("&5.0V"));
 238         voltage80CheckBox = new QCheckBox(tr("&8.0V"));
 239
 240         voltage33CheckBox->setShortcut(tr("3"));
 241         voltage50CheckBox->setShortcut(tr("5"));
 242         voltage80CheckBox->setShortcut(tr("8"));
 243
 244         layout->addWidget(voltage33CheckBox, 0, 0);
 245         layout->addWidget(voltage50CheckBox, 0, 2);
 246         layout->addWidget(voltage80CheckBox, 0, 4); //1, 0);
 247         layout->addWidget(MiscGui::createLabel("BAT"), 0, 6); //1, 2);
 248
 249         voltage33LineEdit = new QLineEdit();
 250         voltage50LineEdit = new QLineEdit();
 251         voltage80LineEdit = new QLineEdit();
 252         voltageBATLineEdit = new QLineEdit();
 253
 254         voltage33LineEdit->setReadOnly(true);
 255         voltage50LineEdit->setReadOnly(true);
 256         voltage80LineEdit->setReadOnly(true);
 257         voltageBATLineEdit->setReadOnly(true);
 258
 259         layout->addWidget(voltage33LineEdit, 0, 1);
 260         layout->addWidget(voltage50LineEdit, 0, 3);
 261         layout->addWidget(voltage80LineEdit, 0, 5); //1, 1);
 262         layout->addWidget(voltageBATLineEdit, 0, 7); //1, 3);
 263
 264         powerGroupBox->setLayout(layout);
 265 }
 266
 267 void RobomonExplorer::createSensorsGroupBox()
 268 {
 269         sensorsGroupBox = new QGroupBox(tr("Sensors"));
 270         QVBoxLayout *layout = new QVBoxLayout();
 271
 272         createSharpSensorsLayout();
 273
 274         layout->addLayout(sharpSensorsLayout);
 275         sensorsGroupBox->setLayout(layout);
 276 }
 277
 278 void RobomonExplorer::createMotorsGroupBox()
 279 {
 280         enginesGroupBox = new QGroupBox(tr("Motors"));
 281         QVBoxLayout *layout = new QVBoxLayout();
 282         QHBoxLayout *layout1 = new QHBoxLayout();
 283         QHBoxLayout *layout2 = new QHBoxLayout();
 284
 285         leftMotorSlider = new QSlider(Qt::Vertical);
 286         rightMotorSlider = new QSlider(Qt::Vertical);
 287         bothMotorsCheckBox = new QCheckBox(tr("Lock both"));
 288         stopMotorsPushButton = new QPushButton(tr("Stop Motors"));
 289
 290         leftMotorSlider->setMinimum(-100);
 291         leftMotorSlider->setMaximum(100);
 292         leftMotorSlider->setTracking(false);
 293         leftMotorSlider->setTickPosition(QSlider::TicksLeft);
 294
 295         rightMotorSlider->setMinimum(-100);
 296         rightMotorSlider->setMaximum(100);
 297         rightMotorSlider->setTracking(false);
 298         rightMotorSlider->setTickPosition(QSlider::TicksRight);
 299
 300         stopMotorsPushButton->setMaximumWidth(90);
 301
 302         layout1->addWidget(leftMotorSlider);
 303         layout1->addWidget(MiscGui::createLabel("0"));
 304         layout1->addWidget(rightMotorSlider);
 305
 306         layout2->addWidget(bothMotorsCheckBox);
 307
 308         layout->addWidget(MiscGui::createLabel("100"));
 309         layout->addLayout(layout1);
 310         layout->addWidget(MiscGui::createLabel("-100"));
 311         layout->addLayout(layout2);
 312         layout->addWidget(stopMotorsPushButton);
 313         enginesGroupBox->setLayout(layout);
 314 }
 315
 316 void RobomonExplorer::createServosGroupBox()
 317 {
 318         servosGroupBox = new QGroupBox(tr("Servos"));
 319         QGridLayout *layout = new QGridLayout();
 320
 321         bottomDoorCheckBox = new QCheckBox(tr("Bottom &Door"));
 322         backDoorCheckBox = new QCheckBox(tr("&Back Door"));
 323         topDoorCheckBox = new QCheckBox(tr("&Top Door"));
 324         leftBrushCheckBox = new QCheckBox(tr("&Left Brush"));
 325         rightBrushCheckBox = new QCheckBox(tr("&Right Brush"));
 326
 327         bottomDoorCheckBox->setShortcut(tr("d"));
 328         backDoorCheckBox->setShortcut(tr("b"));
 329         topDoorCheckBox->setShortcut(tr("t"));
 330         leftBrushCheckBox->setShortcut(tr("l"));
 331         rightBrushCheckBox->setShortcut(tr("r"));
 332
 333         layout->addWidget(bottomDoorCheckBox, 0, 0);
 334         layout->addWidget(backDoorCheckBox, 1, 0);
 335         layout->addWidget(topDoorCheckBox, 2, 0);
 336         layout->addWidget(leftBrushCheckBox, 3, 0);
 337         layout->addWidget(rightBrushCheckBox, 4, 0);
 338         servosGroupBox->setLayout(layout);
 339 }
 340
 341
 342 void RobomonExplorer::createDrivesGroupBox()
 343 {
 344         drivesGroupBox = new QGroupBox(tr("Drives"));
 345         QGridLayout *layout = new QGridLayout();
 346
 347         leftBrushDial = new QDial();
 348         rightBrushDial = new QDial();
 349         bagrDial = new QDial();
 350         carouselDial = new QDial();
 351
 352         leftBrushDriveCheckBox = new QCheckBox(tr("L&eft brush"));
 353         rightBrushDriveCheckBox = new QCheckBox(tr("Ri&ght brush"));
 354         bagrDriveCheckBox = new QCheckBox(tr("B&agr"));
 355
 356         leftBrushDial->setRange(0,200);
 357         leftBrushDial->setValue(100);
 358         rightBrushDial->setRange(0,200);
 359         rightBrushDial->setValue(100);
 360         bagrDial->setRange(0,200);
 361         bagrDial->setValue(100);
 362         carouselDial->setRange(0,4);
 363         carouselDial->setNotchesVisible(1);
 364
 365         layout->addWidget(leftBrushDial, 0, 0);
 366         layout->addWidget(leftBrushDriveCheckBox, 0, 1);
 367         layout->addWidget(rightBrushDial, 1, 0);
 368         layout->addWidget(rightBrushDriveCheckBox, 1, 1);
 369         layout->addWidget(bagrDial, 2, 0);
 370         layout->addWidget(bagrDriveCheckBox, 2, 1);
 371         layout->addWidget(carouselDial, 3, 0);
 372         drivesGroupBox->setLayout(layout);
 373 }
 374
 375 void RobomonExplorer::createSharpSensorsLayout()
 376 {
 377         sharpSensorsLayout = new QVBoxLayout();
 378         QGridLayout *layout1 = new QGridLayout();
 379         QGridLayout *layout2 = new QGridLayout();
 380         QGridLayout *layout3 = new QGridLayout();
 381
 382         sensorsSimulationCheckBox = new QCheckBox(tr("&Sensor simulation"));
 383         obstacleSimulationCheckBox = new QCheckBox(tr("&Obstacle simulation"));
 384
 385         sensorsSimulationCheckBox->setShortcut(tr("s"));
 386         obstacleSimulationCheckBox->setShortcut(tr("o"));
 387
 388         layout3->addWidget(sensorsSimulationCheckBox, 0, 0);
 389         layout3->addWidget(obstacleSimulationCheckBox, 0, 1);
 390
 391         for (int i=0; i<SHARP_LONG_CNT; i++) {
 392                 sharpLongsSlider[i] = new QSlider(Qt::Horizontal);
 393                 sharpLongsProgressBar[i] = new QProgressBar();
 394                 sharpLongsProgressBar[i]->setMinimum(0);
 395                 sharpLongsProgressBar[i]->setMaximum(800);
 396                 sharpLongsProgressBar[i]->setValue(0);
 397                 sharpLongsLabel[i] =
 398                                 MiscGui::createLabel(QString("LONG%1").arg(i+1));
 399                 layout1->addWidget(sharpLongsLabel[i], 0, i);
 400                 layout1->addWidget(sharpLongsSlider[i], 1, i);
 401                 layout1->addWidget(sharpLongsProgressBar[i], 2, i);
 402         }
 403
 404         for (int i=0; i<SHARP_SHORT_CNT; i++) {
 405                 sharpShortsSlider[i] = new QSlider(Qt::Horizontal);
 406                 sharpShortsProgressBar[i] = new QProgressBar();
 407                 sharpShortsProgressBar[i]->setMinimum(0);
 408                 sharpShortsProgressBar[i]->setMaximum(300);
 409                 sharpShortsProgressBar[i]->setValue(0);
 410                 sharpShortsLabel[i] =
 411                                 MiscGui::createLabel(QString("SHORT%1").arg(i+1));
 412                 layout2->addWidget(sharpShortsLabel[i], 0, i);
 413                 layout2->addWidget(sharpShortsSlider[i], 1, i);
 414                 layout2->addWidget(sharpShortsProgressBar[i], 2, i);
 415         }
 416
 417         sharpSensorsLayout->addLayout(layout3);
 418         sharpSensorsLayout->addLayout(layout1);
 419         sharpSensorsLayout->addLayout(layout2);
 420 }
 421
 422 void RobomonExplorer::createRobots()
 423 {
 424         robotActPos = new Robot(NULL, playgroundScene, Qt::black);
 425         robotActPos->setZValue(10);
 426         robotEstPos = new Robot(NULL, playgroundScene, Qt::green);
 427         robotEstPos->setZValue(10);
 428
 429         playgroundScene->addItem(robotActPos);
 430         playgroundScene->addItem(robotEstPos);
 431 }
 432
 433 /**********************************************************************
 434  * GUI actions
 435  **********************************************************************/
 436 void RobomonExplorer::createActions()
 437 {
 438         /* power management */
 439         connect(voltage33CheckBox, SIGNAL(stateChanged(int)),
 440                         this, SLOT(setVoltage33(int)));
 441         connect(voltage50CheckBox, SIGNAL(stateChanged(int)),
 442                         this, SLOT(setVoltage50(int)));
 443         connect(voltage80CheckBox, SIGNAL(stateChanged(int)),
 444                         this, SLOT(setVoltage80(int)));
 445
 446         /* motors */
 447         connect(leftMotorSlider, SIGNAL(valueChanged(int)),
 448                         this, SLOT(setLeftMotor(int)));
 449         connect(rightMotorSlider, SIGNAL(valueChanged(int)),
 450                         this, SLOT(setRightMotor(int)));
 451         connect(stopMotorsPushButton, SIGNAL(clicked()),
 452                         this, SLOT(stopMotors()));
 453
 454         /* servos */
 455         connect(bottomDoorCheckBox, SIGNAL(stateChanged(int)),
 456                         this, SLOT(moveFrontDoor(int)));
 457         connect(backDoorCheckBox, SIGNAL(stateChanged(int)),
 458                         this, SLOT(moveBackDoor(int)));
 459         connect(topDoorCheckBox, SIGNAL(stateChanged(int)),
 460                         this, SLOT(moveTopDoor(int)));
 461         connect(leftBrushCheckBox, SIGNAL(stateChanged(int)),
 462                         this, SLOT(moveLeftBrush(int)));
 463         connect(rightBrushCheckBox, SIGNAL(stateChanged(int)),
 464                         this, SLOT(moveRightBrush(int)));
 465
 466         /* drives */
 467         connect(leftBrushDial, SIGNAL(valueChanged(int)),
 468                         this, SLOT(setDrives(int)));
 469         connect(rightBrushDial, SIGNAL(valueChanged(int)),
 470                         this, SLOT(setDrives(int)));
 471         connect(bagrDial, SIGNAL(valueChanged(int)),
 472                         this, SLOT(setDrives(int)));
 473         connect(carouselDial, SIGNAL(valueChanged(int)),
 474                         this, SLOT(setDrives(int)));
 475         connect(leftBrushDriveCheckBox, SIGNAL(stateChanged(int)),
 476                         this, SLOT(setLeftBrushDriveCB(int)));
 477         connect(rightBrushDriveCheckBox, SIGNAL(stateChanged(int)),
 478                         this, SLOT(setRightBrushDriveCB(int)));
 479         connect(bagrDriveCheckBox, SIGNAL(stateChanged(int)),
 480                         this, SLOT(setBagrDriveCB(int)));
 481         /* DIO */
 482         for (int i=0; i<8; i++)
 483                 connect(doCheckBox[0], SIGNAL(stateChanged(int)),
 484                                         this, SLOT(setDO(int)));
 485
 486         /* laser control */
 487         connect(laserEngineCheckBox, SIGNAL(stateChanged(int)),
 488                         this, SLOT(setLaser(int)));
 489
 490         /* path planning map */
 491         connect(showMapPushButton, SIGNAL(clicked()),
 492                         this, SLOT(showMap()));
 493
 494         /* sensors simulation */
 495         simulationEnabled = 0;
 496         connect(sensorsSimulationCheckBox, SIGNAL(stateChanged(int)),
 497                         this, SLOT(setSimulation(int)));
 498         for (int i=0; i<3; i++)
 499                 connect(sharpLongsSlider[i], SIGNAL(valueChanged(int)),
 500                                         this, SLOT(setSharpValues(int)));
 501         for (int i=0; i<4; i++)
 502                 connect(sharpShortsSlider[i], SIGNAL(valueChanged(int)),
 503                                         this, SLOT(setSharpValues(int)));
 504
 505         /* obstacle simulation */
 506         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 507                         this, SLOT(setSimulation(int)));
 508         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 509                         this, SLOT(setObstacleSimulation(int)));
 510         connect(obstacleSimulationCheckBox, SIGNAL(stateChanged(int)),
 511                         playgroundScene, SLOT(showObstacle(int)));
 512         connect(playgroundScene, SIGNAL(obstacleChanged(QPointF)),
 513                         this, SLOT(changeObstacle(QPointF)));
 514 }
 515
 516 void RobomonExplorer::setVoltage33(int state)
 517 {
 518         if (state)
 519                 orte_generic.pwr_ctrl.voltage33 = true;
 520         else
 521                 orte_generic.pwr_ctrl.voltage33 = false;
 522 }
 523
 524 void RobomonExplorer::setVoltage50(int state)
 525 {
 526         if (state)
 527                 orte_generic.pwr_ctrl.voltage50 = true;
 528         else
 529                 orte_generic.pwr_ctrl.voltage50 = false;
 530 }
 531
 532 void RobomonExplorer::setVoltage80(int state)
 533 {
 534         if (state)
 535                 orte_generic.pwr_ctrl.voltage80 = true;
 536         else
 537                 orte_generic.pwr_ctrl.voltage80 = false;
 538 }
 539
 540 void RobomonExplorer::setLeftMotor(int value)
 541 {
 542         short int leftMotor;
 543         short int rightMotor;
 544
 545         if(bothMotorsCheckBox->isChecked())
 546                 rightMotorSlider->setValue(value);
 547
 548         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 549         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 550
 551         orte_generic.motion_speed.left = leftMotor;
 552         orte_generic.motion_speed.right = rightMotor;
 553
 554 }
 555
 556 void RobomonExplorer::setRightMotor(int value)
 557 {
 558         short int leftMotor;
 559         short int rightMotor;
 560
 561         if(bothMotorsCheckBox->isChecked())
 562                 leftMotorSlider->setValue(value);
 563
 564         leftMotor = (short int)(MOTOR_LIMIT * (leftMotorSlider->value()/100.0));
 565         rightMotor = (short int)(MOTOR_LIMIT * (rightMotorSlider->value()/100.0));
 566
 567         orte_generic.motion_speed.left = leftMotor;
 568         orte_generic.motion_speed.right = rightMotor;
 569
 570 }
 571
 572 void RobomonExplorer::stopMotors()
 573 {
 574         leftMotorSlider->setValue(0);
 575         rightMotorSlider->setValue(0);
 576 }
 577
 578 void RobomonExplorer::moveServos(int value)
 579 {
 580         /* FIXME: servos action comes here */
 581 }
 582
 583 void RobomonExplorer::moveFrontDoor(int state)
 584 {
 585         if (state)
 586                 orte_eb2008.servos.door_bottom = BOTTOM_DOOR_OPEN;
 587         else
 588                 orte_eb2008.servos.door_bottom = BOTTOM_DOOR_CLOSE;
 589 }
 590
 591 void RobomonExplorer::moveTopDoor(int state)
 592 {
 593         if (state)
 594                 orte_eb2008.servos.door_top = TOP_DOOR_OPEN;
 595         else
 596                 orte_eb2008.servos.door_top = TOP_DOOR_CLOSE;
 597 }
 598
 599 void RobomonExplorer::moveBackDoor(int state)
 600 {
 601         if (state)
 602                 orte_eb2008.servos.door_back = BACK_DOOR_OPEN;
 603         else
 604                 orte_eb2008.servos.door_back = BACK_DOOR_CLOSE;
 605 }
 606
 607 void RobomonExplorer::moveLeftBrush(int state)
 608 {
 609         if (state)
 610                 orte_eb2008.servos.brush_left = BRUSH_LEFT_OPEN;
 611         else
 612                 orte_eb2008.servos.brush_left = BRUSH_LEFT_CLOSE;
 613 }
 614
 615 void RobomonExplorer::moveRightBrush(int state)
 616 {
 617         if (state)
 618                 orte_eb2008.servos.brush_right = BRUSH_RIGHT_OPEN;
 619         else
 620                 orte_eb2008.servos.brush_right = BRUSH_RIGHT_CLOSE;
 621 }
 622
 623 void RobomonExplorer::setDrives(int state)
 624 {
 625         orte_eb2008.drives.brush_left = leftBrushDial->value();
 626         orte_eb2008.drives.brush_right = rightBrushDial->value();
 627         orte_eb2008.drives.bagr = bagrDial->value();
 628         orte_eb2008.drives.carousel_pos = carouselDial->value();
 629 }
 630
 631 void RobomonExplorer::setLeftBrushDriveCB(int state) {
 632
 633         if(state)
 634                 orte_eb2008.drives.brush_left = LEFT_BRUSH_DRIVE_ON;
 635         else
 636                 orte_eb2008.drives.brush_left = LEFT_BRUSH_DRIVE_OFF;
 637 }
 638
 639
 640 void RobomonExplorer::setRightBrushDriveCB(int state) {
 641
 642         if(state)
 643                 orte_eb2008.drives.brush_right = RIGHT_BRUSH_DRIVE_ON;
 644         else
 645                 orte_eb2008.drives.brush_right = RIGHT_BRUSH_DRIVE_OFF;
 646 }
 647
 648
 649 void RobomonExplorer::setBagrDriveCB(int state) {
 650
 651         if(state)
 652                 orte_eb2008.drives.bagr = BAGR_DRIVE_ON;
 653         else
 654                 orte_eb2008.drives.bagr = BAGR_DRIVE_OFF;
 655 }
 656
 657 void RobomonExplorer::setDO(int state)
 658 {
 659         /* FIXME: digital output control comes here */
 660 }
 661
 662 void RobomonExplorer::setLaser(int state) {
 663
 664         if(state)
 665                 orte_eb2008.laser_cmd.speed = LASER_DRIVE_ON;
 666         else
 667                 orte_eb2008.laser_cmd.speed = LASER_DRIVE_OFF;
 668         ORTEPublicationSend(orte_eb2008.publication_laser_cmd);
 669 }
 670
 671 void RobomonExplorer::showMap()
 672 {
 673         openSharedMemory();
 674
 675         if (sharedMemoryOpened == false)
 676                 return;
 677
 678         mapTimer = new QTimer(this);
 679         connect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 680         mapTimer->start(200);
 681
 682         showMapPushButton->setText(tr("Show Playground"));
 683         showMapPushButton->setShortcut(tr("p"));
 684         disconnect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
 685         connect(showMapPushButton, SIGNAL(clicked()),
 686                                 this, SLOT(showPlayground()));
 687 }
 688
 689 void RobomonExplorer::showPlayground()
 690 {
 691         mapTimer->stop();
 692         disconnect(mapTimer, SIGNAL(timeout()), this, SLOT(paintMap()));
 693
 694         /* FIXME: */
 695
 696         showMapPushButton->setText(tr("Show &map"));
 697         showMapPushButton->setShortcut(tr("m"));
 698         disconnect(showMapPushButton, SIGNAL(clicked()),
 699                                 this, SLOT(showPlayground()));
 700         connect(showMapPushButton, SIGNAL(clicked()), this, SLOT(showMap()));
 701 }
 702
 703 void RobomonExplorer::paintMap()
 704 {
 705         using namespace Qt;
 706         static QGraphicsRectItem *rects[MAP_WIDTH][MAP_HEIGHT];
 707         static bool firstMap = true;
 708         int x, y;
 709         struct map *map = ShmapIsMapInit();
 710
 711         if (!map) return;
 712
 713         for(int i=0; i < MAP_WIDTH; i++) {
 714                 for(int j=0; j<MAP_HEIGHT; j++) {
 715                         x = TOP_LEFT_X+i*cellSize;
 716                         y = TOP_LEFT_Y+j*cellSize;
 717
 718                         if(firstMap) {
 719                                 rects[i][j] = playgroundScene->addRect(
 720                                                 QRectF(x,y,cellSize,cellSize),
 721                                                 QPen(QBrush(Qt::black),
 722                                                 1,
 723                                                 Qt::SolidLine,
 724                                                 Qt::FlatCap,
 725                                                 Qt::BevelJoin),
 726                                                 QBrush(Qt::lightGray));
 727                                 rects[i][j]->setZValue(4);
 728                         }
 729
 730                         QColor color;
 731
 732                         struct map_cell *cell = &map->cells[j][i];
 733                         color = lightGray;
 734                         if (cell->flags & MAP_FLAG_WALL)
 735                                 color = darkYellow;
 736                         if (cell->flags & MAP_FLAG_SIMULATED_WALL)
 737                                 color = yellow;
 738                         if (cell->flags & MAP_FLAG_PATH)
 739                                 color = darkRed;
 740                         if (cell->flags & MAP_FLAG_START)
 741                                 color = red;
 742                         if (cell->flags & MAP_FLAG_GOAL)
 743                                 color = green;
 744                         if (cell->detected_obstacle) {
 745                                 QColor c1(color), c2(blue);
 746                                 double f = (double)cell->detected_obstacle/MAP_NEW_OBSTACLE*0.7;
 747                                 QColor c(c1.red()   + (int)(f*(c2.red()   - c1.red())),
 748                                          c1.green() + (int)(f*(c2.green() - c1.green())),
 749                                          c1.blue()  + (int)(f*(c2.blue()  - c1.blue())));
 750                                 color = c;
 751                         }
 752                         if (cell->flags & MAP_FLAG_DET_OBST)
 753                                 color = cyan;
 754
 755                         rects[i][j]->setBrush(QBrush(color));
 756                 }
 757         }
 758         firstMap = false;
 759 }
 760
 761 void RobomonExplorer::setSharpValues(int value)
 762 {
 763         // FIXME: sharp control
 764         /*
 765         if (!simulationEnabled) {
 766                 simulationEnabled = 1;
 767                 createSharpLongsPublisher(this, &orteData);
 768                 createSharpShortsPublisher(this, &orteData);
 769                 sensorsSimulationCheckBox->setChecked(true);
 770         }
 771
 772         orteData.orteSharpWaste.short1 = sharpShortsSlider[0]->value();
 773         orteData.orteSharpWaste.short2 = sharpShortsSlider[1]->value();
 774         orteData.orteSharpWaste.short3 = sharpShortsSlider[2]->value();
 775         orteData.orteSharpWaste.short4 = sharpShortsSlider[3]->value();
 776         ORTEPublicationSend(orteData.publisherSharpShort);
 777         orteData.orteSharpOpponent.longSharpDist1 =
 778                                 sharpLongsSlider[0]->value()/1000.0;
 779         orteData.orteSharpOpponent.longSharpDist2 =
 780                                 sharpLongsSlider[1]->value()/1000.0;
 781         orteData.orteSharpOpponent.longSharpDist3 =
 782                                 sharpLongsSlider[2]->value()/1000.0;
 783         ORTEPublicationSend(orteData.publisherSharpLong);
 784         */
 785 }
 786
 787 void RobomonExplorer::setSimulation(int state)
 788 {
 789         if(state) {
 790                 eb2008_publisher_sharps_create(&orte_eb2008,
 791                                                dummy_publisher_callback, this);
 792         } else {
 793                 if (!simulationEnabled)
 794                         return;
 795                 eb2008_publisher_sharps_destroy(&orte_eb2008);
 796         }
 797         simulationEnabled = state;
 798 }
 799
 800 /*!
 801         \fn RobomonExplorer::setObstacleSimulation(int state)
 802  */
 803 void RobomonExplorer::setObstacleSimulation(int state)
 804 {
 805         if (state) {
 806                 /* TODO Maybe it is possible to attach only once to Shmap */
 807                 ShmapInit(0);
 808                 obstacleSimulationTimer = new QTimer(this);
 809                 connect(obstacleSimulationTimer, SIGNAL(timeout()),
 810                                         this, SLOT(simulateObstacles()));
 811                 obstacleSimulationTimer->start(100);
 812                 setMouseTracking(true);
 813         } else {
 814                 if (obstacleSimulationTimer)
 815                         delete obstacleSimulationTimer;
 816                 double distance = 0.8;
 817                 orte_eb2008.sharps.left = distance;
 818                 orte_eb2008.sharps.front_left = distance;
 819                 orte_eb2008.sharps.front_right = distance;
 820                 orte_eb2008.sharps.right = distance;
 821                 ORTEPublicationSend(orte_eb2008.publication_sharps);
 822         }
 823 }
 824
 825 /*!
 826         \fn RobomonExplorer::simulateObstacles()
 827  */
 828 void RobomonExplorer::simulateObstacles()
 829 {
 830         double distance, wall_distance;
 831         int i;
 832         double *sharp[4] = {
 833                 &orte_eb2008.sharps.left,
 834                 &orte_eb2008.sharps.right,
 835                 &orte_eb2008.sharps.front_left,
 836                 &orte_eb2008.sharps.front_right
 837         };
 838
 839
 840         for (i=0; i<4; i++) {
 841                 wall_distance = distanceToWall(i);
 842                 distance = distanceToObstacle(i, simulatedObstacle, 0.5/*meters*/);
 843                 if (wall_distance < distance)
 844                         distance = wall_distance;
 845                 *sharp[i] = distance;
 846         }
 847         ORTEPublicationSend(orte_eb2008.publication_sharps);
 848
 849         // FIXME
 850         /*
 851         orteData.orteSharpOpponent.longSharpDist1 = distance;
 852         orteData.orteSharpOpponent.longSharpDist2 = distance;
 853         orteData.orteSharpOpponent.longSharpDist3 = distance;
 854         ORTEPublicationSend(orteData.publisherSharpLong);*/
 855 }
 856
 857 void RobomonExplorer::changeObstacle(QPointF position)
 858 {
 859         if (!simulationEnabled) {
 860                 simulationEnabled = 1;
 861                 // FIXME
 862 //              createSharpLongsPublisher(this, &orteData);
 863 //              createSharpShortsPublisher(this, &orteData);
 864                 obstacleSimulationCheckBox->setChecked(true);
 865         }
 866
 867         simulatedObstacle.x = position.x();
 868         simulatedObstacle.y = position.y();
 869         simulateObstacles();
 870 }
 871
 872 /**********************************************************************
 873  * EVENTS
 874  **********************************************************************/
 875 bool RobomonExplorer::event(QEvent *event)
 876 {
 877         switch (event->type()) {
 878                 case QEVENT(QEV_MOTION_STATUS):
 879                         emit motionStatusReceivedSignal();
 880                         break;
 881                 case QEVENT(QEV_ACTUAL_POSITION):
 882                         emit actualPositionReceivedSignal();
 883                         break;
 884                 case QEVENT(QEV_ESTIMATED_POSITION):
 885                         emit estimatedPositionReceivedSignal();
 886                         break;
 887                 case QEVENT(QEV_SHARP_LONGS):
 888                         emit sharpLongsReceivedSignal();
 889                         break;
 890                 case QEVENT(QEV_SHARP_SHORTS):
 891                         emit sharpShortsReceivedSignal();
 892                         break;
 893                 case QEVENT(QEV_SHARPS):
 894                         emit sharpsReceivedSignal();
 895                         break;
 896                 case QEVENT(QEV_DI):
 897                         emit diReceivedSignal();
 898                         break;
 899                 case QEVENT(QEV_ACCELEROMETER):
 900                         emit accelerometerReceivedSignal();
 901                         break;
 902                 case QEVENT(QEV_ACCUMULATOR):
 903                         emit accumulatorReceivedSignal();
 904                         break;
 905                 case QEVENT(QEV_POWER_VOLTAGE):
 906                         emit powerVoltageReceivedSignal();
 907                         break;
 908                 default:
 909                         if (event->type() == QEvent::Close)
 910                                 closeEvent((QCloseEvent *)event);
 911                         else if (event->type() == QEvent::KeyPress)
 912                                 keyPressEvent((QKeyEvent *)event);
 913                         else if (event->type() == QEvent::KeyRelease)
 914                                 keyReleaseEvent((QKeyEvent *)event);
 915                         else if (event->type() == QEvent::FocusIn)
 916                                 grabKeyboard();
 917                         else if (event->type() == QEvent::FocusOut)
 918                                 releaseKeyboard();
 919                         else {
 920                                 event->ignore();
 921                                 return false;
 922                         }
 923                         break;
 924         }
 925         event->accept();
 926         return true;
 927 }
 928
 929 void RobomonExplorer::keyPressEvent(QKeyEvent *event)
 930 {
 931         double peak, gain;
 932
 933         if (event->isAutoRepeat()) {
 934                 switch (event->key()) {
 935                         case Qt::Key_Down:
 936                                 peak = leftMotorSlider->minimum()/2;
 937                                 if (leftMotorValue < peak ||
 938                                         rightMotorValue < peak)
 939                                         gain = 1.01;
 940                                 else
 941                                         gain = 1.3;
 942                                 leftMotorValue *= gain;
 943                                 rightMotorValue *= gain;
 944                                 leftMotorSlider->setValue((int)leftMotorValue);
 945                                 rightMotorSlider->setValue((int)rightMotorValue);
 946                                 break;
 947
 948                         case Qt::Key_Up:
 949                         case Qt::Key_Left:
 950                         case Qt::Key_Right:
 951                                 peak = leftMotorSlider->maximum()/2;
 952                                 if (leftMotorValue > peak ||
 953                                         rightMotorValue > peak)
 954                                         gain = 1.01;
 955                                 else
 956                                         gain = 1.3;
 957                                 leftMotorValue *= gain;
 958                                 rightMotorValue *= gain;
 959                                 leftMotorSlider->setValue((int)leftMotorValue);
 960                                 rightMotorSlider->setValue((int)rightMotorValue);
 961                                 break;
 962
 963                         default:
 964                                 event->ignore();
 965                                 break;
 966                 }
 967                 return;
 968         }
 969
 970         switch (event->key()) {
 971                 case Qt::Key_Up:
 972                         leftMotorValue = 1;
 973                         rightMotorValue = 1;
 974                         bothMotorsCheckBox->setChecked(true);
 975                         leftMotorSlider->setValue((int)leftMotorValue);
 976                         setLeftMotor((int)leftMotorValue);
 977                         break;
 978                 case Qt::Key_Down:
 979                         leftMotorValue = -1;
 980                         rightMotorValue = -1;
 981                         bothMotorsCheckBox->setChecked(true);
 982                         leftMotorSlider->setValue((int)leftMotorValue);
 983                         setLeftMotor((int)leftMotorValue);
 984                         break;
 985                 case Qt::Key_Left:
 986                         leftMotorValue = -1;
 987                         rightMotorValue = 1;
 988                         leftMotorSlider->setValue((int)leftMotorValue);
 989                         rightMotorSlider->setValue((int)rightMotorValue);
 990                         setLeftMotor((int)leftMotorValue);
 991                         setRightMotor((int)leftMotorValue);
 992                         break;
 993                 case Qt::Key_Right:
 994                         leftMotorValue = 1;
 995                         rightMotorValue = -1;
 996                         leftMotorSlider->setValue((int)leftMotorValue);
 997                         rightMotorSlider->setValue((int)rightMotorValue);
 998                         setLeftMotor((int)leftMotorValue);
 999                         setRightMotor((int)rightMotorValue);
1000                         break;
1001                 default:
1002                         event->ignore();
1003                         break;
1004         }
1005         event->accept();
1006 }
1007
1008 void RobomonExplorer::keyReleaseEvent(QKeyEvent *event)
1009 {
1010         if (event->isAutoRepeat()) {
1011                 event->ignore();
1012                 return;
1013         }
1014
1015         switch (event->key()) {
1016                 case Qt::Key_Up:
1017                 case Qt::Key_Down:
1018                 case Qt::Key_Left:
1019                 case Qt::Key_Right:
1020                         leftMotorValue = 0;
1021                         rightMotorValue = 0;
1022                         bothMotorsCheckBox->setChecked(false);
1023                         leftMotorSlider->setValue((int)leftMotorValue);
1024                         rightMotorSlider->setValue((int)rightMotorValue);
1025                         break;
1026                 default:
1027                         event->ignore();
1028                         break;
1029         }
1030         event->accept();
1031 }
1032
1033 void RobomonExplorer::closeEvent(QCloseEvent *event)
1034 {
1035         generic_roboorte_destroy(&orte_generic);
1036         eb2008_roboorte_destroy(&orte_eb2008);
1037 }
1038
1039 /**********************************************************************
1040  * ORTE
1041  **********************************************************************/
1042 void RobomonExplorer::createOrte()
1043 {
1044         orte_generic.strength = 11;
1045         orte_eb2008.strength = 11;
1046
1047         generic_roboorte_init(&orte_generic);
1048         eb2008_roboorte_init(&orte_eb2008);
1049
1050         /* publishers */
1051         generic_publisher_motion_speed_create(&orte_generic, dummy_publisher_callback, NULL);
1052
1053         generic_publisher_pwr_ctrl_create(&orte_generic, dummy_publisher_callback, NULL);
1054
1055         eb2008_publisher_servos_create(&orte_eb2008, dummy_publisher_callback, NULL);
1056         eb2008_publisher_drives_create(&orte_eb2008, dummy_publisher_callback, NULL);
1057         eb2008_publisher_laser_cmd_create(&orte_eb2008, NULL, NULL);
1058
1059         /* subscribers */
1060         generic_subscriber_pwr_voltage_create(&orte_generic,
1061                                 receivePowerVoltageCallBack, this);
1062         generic_subscriber_motion_status_create(&orte_generic,
1063                                 receiveMotionStatusCallBack, this);
1064         generic_subscriber_ref_pos_create(&orte_generic,
1065                                 receiveActualPositionCallBack, this);
1066         generic_subscriber_est_pos_create(&orte_generic,
1067                                 receiveEstimatedPositionCallBack, this);
1068
1069         eb2008_subscriber_sharps_create(&orte_eb2008,
1070                                 receiveSharpsCallBack, this);
1071
1072         /*createDISubscriber(this, &orteData);*/
1073         /*createAccelerometerSubscriber(this, &orteData);*/
1074         /*createAccumulatorSubscriber(this, &orteData);*/
1075         /* motors */
1076         orte_generic.motion_speed.left = 0;
1077         orte_generic.motion_speed.right = 0;
1078         /* power management */
1079         orte_generic.pwr_ctrl.voltage33 = true;
1080         orte_generic.pwr_ctrl.voltage50 = true;
1081         orte_generic.pwr_ctrl.voltage80 = true;
1082         voltage33CheckBox->setChecked(true);
1083         voltage50CheckBox->setChecked(true);
1084         voltage80CheckBox->setChecked(true);
1085
1086         /* servos */
1087         orte_eb2008.servos.door_bottom = FRONT_DOOR_UP; //FIXME
1088         orte_eb2008.servos.door_back = BACK_DOOR_UP; //FIXME
1089         orte_eb2008.servos.door_top = TOP_DOOR_UP; //FIXME
1090         bottomDoorCheckBox->setChecked(true);
1091         backDoorCheckBox->setChecked(true);
1092         topDoorCheckBox->setChecked(true);
1093         leftBrushCheckBox->setChecked(false);
1094         rightBrushCheckBox->setChecked(false);
1095
1096         setDrives(0);
1097         setLaser(0);
1098
1099         /* set actions to do when we receive data from orte */
1100         connect(this, SIGNAL(motionStatusReceivedSignal()),
1101                         this, SLOT(motionStatusReceived()));
1102         connect(this, SIGNAL(actualPositionReceivedSignal()),
1103                         this, SLOT(actualPositionReceived()));
1104         connect(this, SIGNAL(estimatedPositionReceivedSignal()),
1105                         this, SLOT(estimatedPositionReceived()));
1106         connect(this, SIGNAL(sharpsReceivedSignal()),
1107                         this, SLOT(sharpsReceived()));
1108         connect(this, SIGNAL(diReceivedSignal()),
1109                         this, SLOT(diReceived()));
1110         connect(this, SIGNAL(accelerometerReceivedSignal()),
1111                         this, SLOT(accelerometerReceived()));
1112         connect(this, SIGNAL(accumulatorReceivedSignal()),
1113                         this, SLOT(accumulatorReceived()));
1114         connect(this, SIGNAL(powerVoltageReceivedSignal()),
1115                         this, SLOT(powerVoltageReceived()));
1116 }
1117
1118 void RobomonExplorer::motionStatusReceived()
1119 {
1120         WDBG("ORTE received: motion status");
1121 }
1122
1123 void RobomonExplorer::actualPositionReceived()
1124 {
1125         actPosX->setText(QString("%1").arg(orte_generic.ref_pos.x, 0, 'f', 3));
1126         actPosY->setText(QString("%1").arg(orte_generic.ref_pos.y, 0, 'f', 3));
1127         actPosPhi->setText(QString("%1(%2)")
1128                         .arg(DEGREES(orte_generic.ref_pos.phi), 0, 'f', 0)
1129                         .arg(orte_generic.ref_pos.phi, 0, 'f', 1));
1130         robotActPos->moveRobot(orte_generic.ref_pos.x,
1131                 orte_generic.ref_pos.y, orte_generic.ref_pos.phi);
1132 }
1133
1134 void RobomonExplorer::estimatedPositionReceived()
1135 {
1136         estPosX->setText(QString("%1").arg(orte_generic.est_pos.x, 0, 'f', 3));
1137         estPosY->setText(QString("%1").arg(orte_generic.est_pos.y, 0, 'f', 3));
1138         estPosPhi->setText(QString("%1(%2)")
1139                         .arg(DEGREES(orte_generic.est_pos.phi), 0, 'f', 0)
1140                         .arg(orte_generic.est_pos.phi, 0, 'f', 1));
1141         robotEstPos->moveRobot(orte_generic.est_pos.x,
1142                 orte_generic.est_pos.y, orte_generic.est_pos.phi);
1143 }
1144
1145 void RobomonExplorer::sharpsReceived()
1146 {
1147         int val_long[SHARP_LONG_CNT];
1148         int val_short[SHARP_SHORT_CNT];
1149
1150 //      WDBG("ORTE received: sharps");
1151
1152         val_long[0] = (int)((double)orte_eb2008.sharps.left * 1000);
1153         val_long[1] = (int)((double)orte_eb2008.sharps.front_left * 1000);
1154         val_long[2] = (int)((double)orte_eb2008.sharps.front_right * 1000);
1155         val_long[3] = (int)((double)orte_eb2008.sharps.right * 1000);
1156
1157         val_short[0] = (int)((double)orte_eb2008.sharps.back_left * 1000);
1158         val_short[1] = (int)((double)orte_eb2008.sharps.back_right * 1000);
1159
1160         for (int i=0; i<SHARP_LONG_CNT; i++) {
1161                 sharpLongsLabel[i]->setText(QString("%1mm").arg(val_long[i]));
1162                 if (val_long[i] > sharpLongsProgressBar[i]->maximum())
1163                         val_long[i] = sharpLongsProgressBar[i]->maximum();
1164                 sharpLongsProgressBar[i]->setValue(
1165                         sharpLongsProgressBar[i]->maximum()-val_long[i]);
1166         }
1167
1168         for (int i=0; i<SHARP_SHORT_CNT; i++) {
1169                 sharpShortsLabel[i]->setText(QString("%1").arg(val_short[i]));
1170                 sharpShortsProgressBar[i]->setValue(
1171                         sharpShortsProgressBar[i]->maximum()-val_short[i]);
1172         }
1173 }
1174
1175
1176 void RobomonExplorer::diReceived()
1177 {
1178         WDBG("ORTE received: DI");
1179 }
1180
1181 void RobomonExplorer::accelerometerReceived()
1182 {
1183         WDBG("ORTE received: accelerometer");
1184 }
1185
1186 void RobomonExplorer::accumulatorReceived()
1187 {
1188         WDBG("ORTE received: accumulator");
1189 }
1190
1191 void RobomonExplorer::powerVoltageReceived()
1192 {
1193         voltage33LineEdit->setText(QString("%1").arg(
1194                         orte_generic.pwr_voltage.voltage33, 0, 'f', 3));
1195         voltage50LineEdit->setText(QString("%1").arg(
1196                         orte_generic.pwr_voltage.voltage50, 0, 'f', 3));
1197         voltage80LineEdit->setText(QString("%1").arg(
1198                         orte_generic.pwr_voltage.voltage80, 0, 'f', 3));
1199         voltageBATLineEdit->setText(QString("%1").arg(
1200                         orte_generic.pwr_voltage.voltageBAT, 0, 'f', 3));
1201
1202 }
1203
1204 /**********************************************************************
1205  * MISCELLANEOUS
1206  **********************************************************************/
1207 void RobomonExplorer::openSharedMemory()
1208 {
1209         int segmentId;
1210         int sharedSegmentSize;
1211
1212         if (sharedMemoryOpened)
1213                 return;
1214
1215         cellSize = PG_X / MAP_WIDTH;
1216
1217         sharedSegmentSize = sizeof(char) * MAP_WIDTH * MAP_HEIGHT;
1218
1219         /* Get segment identificator in a read only mode  */
1220         segmentId = shmget(SHM_MAP_KEY, sharedSegmentSize, S_IRUSR);
1221         if(segmentId == -1) {
1222                 QMessageBox::critical(this, "robomon",
1223                                 "Unable to open shared memory segment!");
1224                 return;
1225         }
1226
1227         /* Init Shmap */
1228         ShmapInit(0);
1229
1230         /* Attach the shared memory segment */
1231         //map =  (_Map*)shmat (segmentId, (void*) 0, 0);
1232
1233         sharedMemoryOpened = true;
1234 }
1235
1236 double RobomonExplorer::distanceToWall(int sharpnum)
1237 {
1238         double distance=0.8, min_distance=0.8;
1239         int i,j;
1240         Point wall;
1241         struct map *map = ShmapIsMapInit();
1242
1243         if (!map) return min_distance;
1244
1245         // Simulate obstacles
1246         for(j=0;j<MAP_HEIGHT;j++) {
1247                 for (i=0;i<MAP_WIDTH;i++) {
1248                         struct map_cell *cell = &map->cells[j][i];
1249                         if( cell->flags & MAP_FLAG_SIMULATED_WALL) {
1250                                 // WALL
1251                                 ShmapCell2Point(i, j, &wall.x, &wall.y);
1252
1253                                 distance = distanceToObstacle(sharpnum, wall, MAP_CELL_SIZE_M);
1254                                 if (distance<min_distance) min_distance = distance;
1255                         }
1256                 }
1257         }
1258
1259         return min_distance;
1260 }
1261
1262 /**
1263  * Calculation for sensor simulation. Calculates distance that would
1264  * be returned by IR sensors, if there is only one obstacle (as
1265  * specified by parameters).
1266  *
1267  * @param obstacle Position of the obstacle (x, y in meters).
1268  * @param obstacleSize Size (diameter) of the obstacle in meters.
1269  *
1270  * @return Distance measured by sensors in meters.
1271  */
1272 double RobomonExplorer::distanceToObstacle(int sharpnum, Point obstacle, double obstacleSize)
1273 {
1274         struct est_pos_type e = orte_generic.est_pos;
1275         double sensor_a;
1276         struct sharp_pos s = sharp[sharpnum];
1277         Point sensor(e.x + s.x*cos(e.phi) - s.y*sin(e.phi),
1278                      e.y + s.x*sin(e.phi) + s.y*cos(e.phi));
1279         sensor_a = e.phi + s.ang;
1280
1281         const double sensorRange = 0.8; /*[meters]*/
1282
1283         double distance, angle;
1284
1285         angle = sensor.angleTo(obstacle) - sensor_a;
1286         angle = fmod(angle, 2.0*M_PI);
1287         if (angle > +M_PI) angle -= 2.0*M_PI;
1288         if (angle < -M_PI) angle += 2.0*M_PI;
1289         angle = fabs(angle);
1290         distance = sensor.distanceTo(obstacle)-0.11;
1291         if (angle < atan(obstacleSize/2.0 / (distance+0.001))) {
1292                 // We can see the obstackle from here.
1293                 if (angle < M_PI/2.0) {
1294                     distance = distance/cos(angle);
1295                 }
1296                 if (distance > sensorRange)
1297                         distance = sensorRange;
1298         } else {
1299                 distance = sensorRange;
1300         }
1301
1302         return distance;
1303 }