robomon: Implement motor simulation

[eurobot/public.git] / src / robomon / RobomonAtlantis.h

diff --git a/src/robomon/RobomonAtlantis.h b/src/robomon/RobomonAtlantis.h
index cf8381b089787d4e419f56283daa24ed77cd3dae..f7983628b3200db586dd4012ae648f9291e85440 100644 (file)
--- a/src/robomon/RobomonAtlantis.h
+++ b/src/robomon/RobomonAtlantis.h
@@ -13,15 +13,17 @@
 #ifndef ROBOMON_ATLANTIS_H
 #define ROBOMON_ATLANTIS_H
 
-#define SIM_OBST_SIZE_M 0.5
-
 #include <QDialog>
+#include <QDateTime>
+#include <QTimer>
 
 #include <trgen.h>
 #include "PlaygroundScene.h"
+#include "playgroundview.h"
 #include "Robot.h"
 #include <roboorte_robottype.h>
-#include <roboorte.h>
+#include "trail.h"
+#include "hokuyoscan.h"
 
 class QHBoxLayout;
 class QVBoxLayout;
@@ -38,12 +40,38 @@ class QSlider;
 class QProgressBar;
 class QFont;
 
-#define PG_X 600
-#define PG_Y 420
+class MotorSimulation : QObject {
+    Q_OBJECT
+
+    QTimer timer;
+    qint64 last_time;
+    struct robottype_orte_data &orte;
+public:
+    MotorSimulation(struct robottype_orte_data &orte) : QObject(), timer(this), orte(orte) {}
+    void start()
+    {
+        robottype_publisher_motion_irc_create(&orte, 0, 0);
+        connect(&timer, SIGNAL(timeout()), this, SLOT(updateIRC()));
+        timer.start(50);
+    }
+
+    void stop()
+    {
+        robottype_publisher_motion_irc_destroy(&orte);
+        timer.stop();
+        disconnect(&timer, SIGNAL(timeout()), this, SLOT(updateIRC()));
+    }
+private slots:
+    void updateIRC()
+    {
+        qint64 now = QDateTime::currentMSecsSinceEpoch();
+        orte.motion_irc.left += orte.motion_speed.left * (now - last_time);   // TODO: Find constant for speed to irc conversion
+        orte.motion_irc.right+= orte.motion_speed.right * (now - last_time);
+        ORTEPublicationSend(orte.publication_motion_irc);
+        last_time = now;
+    }
+};
 
-#define TOP_LEFT QPointF(10,0)
-#define TOP_LEFT_X 10
-#define TOP_LEFT_Y 0
 
 class RobomonAtlantis : public QWidget
 {
@@ -61,12 +89,14 @@ protected:
 signals:
        void motionStatusReceivedSignal();
        void actualPositionReceivedSignal();
-       void estimatedPositionReceivedSignal();
-       void diReceivedSignal();
-       void accelerometerReceivedSignal();
-       void accumulatorReceivedSignal();
        void powerVoltageReceivedSignal();
-
+       
+public slots:
+       void showMap(bool show);
+       void useOpenGL(bool use);
+       void showTrails(bool show);
+       void showShapeDetect(bool show);
+       void resetTrails();
 private slots:
        /************************************************************
         * GUI actions 
@@ -74,35 +104,23 @@ private slots:
        void setVoltage33(int state);
        void setVoltage50(int state);
        void setVoltage80(int state);
-       void setLeftMotor(int value);
-       void setRightMotor(int value);
-       void setDrives(int value);
-       void setLeftBrushDriveCB(int value);
-       void setRightBrushDriveCB(int value);
-       void setBagrDriveCB(int value);
-       void stopMotors();
-       void moveServos(int value);
-       void moveLeftBrush(int state);
-       void moveRightBrush(int state);
-       void setDO(int state);
-       void setLaser(int state);
-       void showMap();
-       void showPlayground();
+/*     void setLeftMotor(int value); */
+/*     void setRightMotor(int value); */
+/*     void stopMotors(); */
        void paintMap();
        void setSimulation(int state);
        void setObstacleSimulation(int state);
        void simulateObstaclesHokuyo();
        void changeObstacle(QPointF position);
+       void sendStart(int plug);
+       void setTeamColor(int plug);
+       void setMotorSimulation(int state);
 
        /************************************************************
         * ORTE 
         ************************************************************/
        void motionStatusReceived();
        void actualPositionReceived();
-       void estimatedPositionReceived();
-       void diReceived();
-       void accelerometerReceived();
-       void accumulatorReceived();
        void powerVoltageReceived();
 
 private:
@@ -118,35 +136,30 @@ private:
        void createDebugGroupBox();
        void createActuatorsGroupBox();
        void createMotorsGroupBox();
-       void createServosGroupBox();
-       void createDrivesGroupBox();
        void createDIOGroupBox();
        void createSensorsGroupBox();
        void createPowerGroupBox();
-
-       void createSharpSensorsLayout();
+       void createPickerGroupBox();
+       void createFSMGroupBox();
 
        void createRobots();
        void createActions();
 
        QVBoxLayout *leftLayout;
        QVBoxLayout *rightLayout;
-       QVBoxLayout *sharpSensorsLayout;
 
        QGroupBox *playgroundGroupBox;
        QGroupBox *positionGroupBox;
        QGroupBox *miscGroupBox;
        QGroupBox *debugGroupBox;
        QGroupBox *actuatorsGroupBox;
-       QGroupBox *enginesGroupBox;
-       QGroupBox *servosGroupBox;
-       QGroupBox *drivesGroupBox;
-       QGroupBox *dioGroupBox;
-       QGroupBox *sensorsGroupBox;
        QGroupBox *powerGroupBox;
+       QGroupBox *fsmGroupBox;
 
+public:
        PlaygroundScene *playgroundScene;
-       QGraphicsView *playgroundSceneView;
+private:
+       PlaygroundView *playgroundSceneView;
 
        /* position state */
        QLineEdit *actPosX;
@@ -162,23 +175,11 @@ private:
        bool debugWindowEnabled;
 
        /* actuators */
-       QSlider *leftMotorSlider;
-       QSlider *rightMotorSlider;
-       QCheckBox *bothMotorsCheckBox;
-       QPushButton *stopMotorsPushButton;
-
-       QCheckBox *leftBrushCheckBox;
-       QCheckBox *rightBrushCheckBox;
-       QCheckBox *pusherCheckBox;
-
-       QDial *leftBrushDial;
-       QDial *rightBrushDial;
-       QDial *bagrDial;
-       QDial *carouselDial;
-
-       QCheckBox *leftBrushDriveCheckBox;
-       QCheckBox *rightBrushDriveCheckBox;
-       QCheckBox *bagrDriveCheckBox;
+/*     QSlider *leftMotorSlider; */
+/*     QSlider *rightMotorSlider; */
+/*     QCheckBox *bothMotorsCheckBox; */
+/*     QPushButton *stopMotorsPushButton; */
+       //QDial *vidle;
 
        /* power management */
        QCheckBox *voltage33CheckBox;
@@ -189,17 +190,27 @@ private:
        QLineEdit *voltage80LineEdit;
        QLineEdit *voltageBATLineEdit;
 
-       QCheckBox *diCheckBox[8];
-       QCheckBox *doCheckBox[8];
-
        /* misc */
        QCheckBox *obstacleSimulationCheckBox;
-       QPushButton *showMapPushButton;
-       QCheckBox *laserEngineCheckBox;
-
+       QCheckBox *motorSimulationCheckBox;
+       QLabel *fsm_main_state;
+       QLabel *fsm_act_state;
+       QLabel *fsm_motion_state;
+       QCheckBox *startPlug;
+       QCheckBox *colorChoser;
+public:
        /* robot */
-       Robot *robotActPos;
-       Robot *robotEstPos;
+       Robot *robotRefPos;
+       Robot *robotEstPosBest;
+       Robot *robotEstPosIndepOdo;
+       Robot *robotEstPosOdo;
+private:
+       Trail *trailRefPos;
+       Trail *trailEstPosBest;
+       Trail *trailPosIndepOdo;
+       Trail *trailOdoPos;
+
+        HokuyoScan *hokuyoScan;
 
        /* keypad */
        double leftMotorValue;
@@ -208,9 +219,8 @@ private:
        /* map */
        void openSharedMemory();
        bool sharedMemoryOpened;
-       int cellSize;
        QTimer *mapTimer;
-
+       
        /* obstacle simulation */
        double distanceToWallHokuyo(int beamnum);
        double distanceToObstacleHokuyo(int beamnum, Point obstacle, double obstacleSize);
@@ -219,6 +229,8 @@ private:
        QTimer *obstacleSimulationTimer;
        Point simulatedObstacle;
 
+       class MotorSimulation motorSimulation;
+
        /************************************************************
         * ORTE 
         ************************************************************/