src/robomon/RobomonAtlantis.cpp

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