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