diff --git a/cellulorobotwrapper.h b/cellulorobotwrapper.h
index 44f2b3d..a9e66d6 100644
--- a/cellulorobotwrapper.h
+++ b/cellulorobotwrapper.h
@@ -1,125 +1,126 @@
 #ifndef CELLULOROBOTWRAPPER_H
 #define CELLULOROBOTWRAPPER_H
 
 #include <QTimer>
 #include <QObject>
 #include "cellulo-qml-plugin/src/comm/CelluloBluetooth.h"
 
 class CelluloRobotWrapper: public QObject
 { Q_OBJECT
 
     Q_INVOKABLE void init_() {
         robot = new Cellulo::CelluloBluetooth();
+        qDebug() <<"Object created";
     }
     Q_INVOKABLE void setLocalAdapterMacAddr_(QString localAdapterMacAddr) {
         robot->setLocalAdapterMacAddr(localAdapterMacAddr);
     }
     Q_INVOKABLE void setAutoConnect_(bool autoConnect) {
         robot->setAutoConnect(autoConnect);
     }
     Q_INVOKABLE void setMacAddr_(QString macAddr) {
         robot->setMacAddr(macAddr);
     }
     Q_INVOKABLE void connectToServer_() {
         robot->connectToServer();
     }
     Q_INVOKABLE void setGoalVelocity_(float vx, float vy, float w) {
         robot->setGoalVelocity(vx, vy, w);
     }
     Q_INVOKABLE void setGoalPose_(float x, float y, float theta, float v, float w) {
         robot->setGoalPose(x, y, theta, v, w);
     }
     Q_INVOKABLE void setGoalPosition_(float x, float y, float v) {
         robot->setGoalPosition(x, y, v);
     }
     Q_INVOKABLE void clearTracking_() {
         robot->clearTracking();
     }
     Q_INVOKABLE void clearHapticFeedback_() {
         robot->clearHapticFeedback();
     }
     Q_INVOKABLE void setVisualEffect_(int effect, QColor color, int value) {
         robot->setVisualEffect((Cellulo::CelluloBluetoothEnums::VisualEffect) effect, color, value);
     }
     Q_INVOKABLE void setCasualBackdriveAssistEnabled_(bool enabled) {
         robot->setCasualBackdriveAssistEnabled(enabled);
     }
     Q_INVOKABLE void setHapticBackdriveAssist_(float xAssist, float yAssist, float thetaAssist) {
         robot->setHapticBackdriveAssist(xAssist, yAssist, thetaAssist);
     }
     Q_INVOKABLE void reset_() {
         robot->reset();
     }
     Q_INVOKABLE void simpleVibrate_(float iX, float iY, float iTheta, unsigned int period, unsigned int duration) {
         robot->simpleVibrate(iX, iY, iTheta, period, duration);
     }
 
 public:
     CelluloRobotWrapper();
 
     Cellulo::CelluloBluetooth* robot;
 
     void init()
     {
-        QMetaObject::invokeMethod(this, "init_", Qt::QueuedConnection);
+        QMetaObject::invokeMethod(this, "init_", Qt::BlockingQueuedConnection);
     }
     void setLocalAdapterMacAddr(QString localAdapterMacAddr)
     {
-        QMetaObject::invokeMethod(this, "setLocalAdapterMacAddr_", Qt::QueuedConnection, Q_ARG(QString, localAdapterMacAddr));
+        QMetaObject::invokeMethod(this, "setLocalAdapterMacAddr_", Qt::BlockingQueuedConnection, Q_ARG(QString, localAdapterMacAddr));
     }
     void setAutoConnect(bool autoConnect)
     {
-        QMetaObject::invokeMethod(this, "autoConnect_", Qt::QueuedConnection, Q_ARG(bool, autoConnect));
+        QMetaObject::invokeMethod(this, "autoConnect_", Qt::BlockingQueuedConnection, Q_ARG(bool, autoConnect));
     }
     void setMacAddr(QString macAddr)
     {
-        QMetaObject::invokeMethod(this, "setMacAddr_", Qt::QueuedConnection, Q_ARG(QString, macAddr));
+        QMetaObject::invokeMethod(this, "setMacAddr_", Qt::BlockingQueuedConnection, Q_ARG(QString, macAddr));
     }
     void connectToServer()
     {
-        QMetaObject::invokeMethod(this, "connectToServer_", Qt::QueuedConnection);
+        QMetaObject::invokeMethod(this, "connectToServer_", Qt::BlockingQueuedConnection);
     }
     void setGoalVelocity(float vx, float vy, float w)
     {
         QMetaObject::invokeMethod(this, "setGoalVelocity_", Qt::QueuedConnection, Q_ARG(float, vx), Q_ARG(float, vy), Q_ARG(float, w));
     }
     void setGoalPose(float x, float y, float theta, float v, float w)
     {
         QMetaObject::invokeMethod(this, "setGoalPose_", Qt::QueuedConnection, Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, theta), Q_ARG(float, v), Q_ARG(float, w));
     }
     void setGoalPosition(float x, float y, float v)
     {
         QMetaObject::invokeMethod(this, "setGoalPosition_", Qt::QueuedConnection, Q_ARG(float, x), Q_ARG(float, y), Q_ARG(float, v));
     }
     void clearTracking()
     {
         QMetaObject::invokeMethod(this, "clearTracking_", Qt::QueuedConnection);
     }
     void clearHapticFeedback()
     {
         QMetaObject::invokeMethod(this, "clearHapticFeedback_", Qt::QueuedConnection);
     }
     void setVisualEffect(int effect, QColor color, int value)
     {
         QMetaObject::invokeMethod(this, "setVisualEffect_", Qt::QueuedConnection, Q_ARG(int, effect), Q_ARG(QColor, color), Q_ARG(int, value));
     }
     void setCasualBackdriveAssistEnabled(bool enabled)
     {
         QMetaObject::invokeMethod(this, "setCasualBackdriveAssistEnabled_", Qt::QueuedConnection, Q_ARG(bool, enabled));
     }
     void setHapticBackdriveAssist(float xAssist, float yAssist, float thetaAssist)
     {
         QMetaObject::invokeMethod(this, "setCasualBackdriveAssistEnabled_", Qt::QueuedConnection, Q_ARG(float, xAssist), Q_ARG(float, yAssist), Q_ARG(float, thetaAssist));
     }
     void reset()
     {
-        QMetaObject::invokeMethod(this, "reset_", Qt::QueuedConnection);
+        QMetaObject::invokeMethod(this, "reset_", Qt::BlockingQueuedConnection);
     }
     void simpleVibrate(float iX, float iY, float iTheta, unsigned int period, unsigned int duration)
     {
         QMetaObject::invokeMethod(this, "setCasualBackdriveAssistEnabled_", Qt::QueuedConnection, Q_ARG(float, iX), Q_ARG(float, iY), Q_ARG(float, iTheta), Q_ARG(unsigned int, period),
                                   Q_ARG(unsigned int, duration));
     }
 };
 
 #endif // CELLULOROBOTWRAPPER_H
diff --git a/cellulounity.cpp b/cellulounity.cpp
index bf8b99e..520798f 100644
--- a/cellulounity.cpp
+++ b/cellulounity.cpp
@@ -1,143 +1,141 @@
 #include "cellulounity.h"
 #include "cellulothread.h"
 #include "cellulorobotwrapper.h"
 #include "democlass.h"
 #include <QDebug>
 
 CelluloThread* thread = nullptr;
 DemoClass* obj = nullptr;
 
 int nRobots = 0;
 const int MAXROBOTS = 100;
 CelluloRobotWrapper* robots[MAXROBOTS];
 
 void demoPrint()
 {
     if(thread == nullptr) {
         thread = new CelluloThread;
         thread->start();
         obj = new DemoClass;
         qDebug() << "Move to thread...";
         obj->moveToThread(thread);
         qDebug() << "Call init...";
         obj->init();
         qDebug() << "Starting thread...";
     }
     obj->print();
 }
 
 int getVar()
 {
     return obj->getVar();
 }
 
 void setLocalAdapterMacAddr(int robot, const char *localAdapterMacAddr)
 {
     robots[robot]->setLocalAdapterMacAddr(localAdapterMacAddr);
 }
 
 int newRobot()
 {
     if(thread == nullptr) {
         thread = new CelluloThread;
         thread->start();
     }
 
     if(nRobots == MAXROBOTS) {
         qDebug("Have used all slots already");
         return -1;
     }
 
     robots[nRobots] = new CelluloRobotWrapper();
     robots[nRobots]->moveToThread(thread);
     robots[nRobots]->init();
 
-    nRobots++;
-
-    return nRobots - 1;
+    return nRobots++;
 }
 
 void setAutoConnect(int robot, bool autoConnect)
 {
     robots[robot]->setAutoConnect(autoConnect);
 }
 
 void setMacAddr(int robot, const char *macAddr)
 {
     robots[robot]->setMacAddr(macAddr);
 }
 
 void connectToServer(int robot)
 {
     robots[robot]->connectToServer();
 }
 
 void setGoalVelocity(int robot, float vx, float vy, float w)
 {
     robots[robot]->setGoalVelocity(vx, vy, w);
 }
 
 void setGoalPose(int robot, float x, float y, float theta, float v, float w)
 {
     robots[robot]->setGoalPose(x, y, theta, v, w);
 }
 
 void setGoalPosition(int robot, float x, float y, float v)
 {
     robots[robot]->setGoalPosition(x, y, v);
 }
 
 void clearTracking(int robot)
 {
     robots[robot]->clearTracking();
 }
 
 void clearHapticFeedback(int robot)
 {
     robots[robot]->clearHapticFeedback();
 }
 
 void setVisualEffect(int robot, int effect, int r, int g, int b, int value)
 {
     robots[robot]->setVisualEffect(effect, QColor(r, g, b), value);
 }
 
 void setCasualBackdriveAssistEnabled(int robot, bool enabled)
 {
     robots[robot]->setCasualBackdriveAssistEnabled(enabled);
 }
 
 void setHapticBackdriveAssist(int robot, float xAssist, float yAssist, float thetaAssist)
 {
     robots[robot]->setHapticBackdriveAssist(xAssist, yAssist, thetaAssist);
 }
 
 void reset(int robot)
 {
     robots[robot]->reset();
 }
 
 void simpleVibrate(int robot, float iX, float iY, float iTheta, unsigned int period, unsigned int duration)
 {
     robots[robot]->simpleVibrate(iX, iY, iTheta, period, duration);
 }
 
 float getX(int robot)
 {
     return robots[robot]->robot->getX();
 }
 
 float getY(int robot)
 {
     return robots[robot]->robot->getY();
 }
 
 float getTheta(int robot)
 {
     return robots[robot]->robot->getTheta();
 }
 
 bool getKidnapped(int robot)
 {
     return robots[robot]->robot->getKidnapped();
 }
diff --git a/test/main.cpp b/test/main.cpp
index 4c5f6da..c329ac7 100644
--- a/test/main.cpp
+++ b/test/main.cpp
@@ -1,16 +1,24 @@
 #include <stdio.h>
 #include "cellulounity.h"
 #include <unistd.h>
 
 int main() {
     int robot = newRobot();
     fprintf(stderr, "Got robot %d\n", robot);
-    setMacAddr(robot, "aabbccdd");
+    setLocalAdapterMacAddr(robot, "00:1A:7D:DA:71:03");
+    setMacAddr(robot, "00:06:66:74:40:FE");
     connectToServer(robot);
 
+    int r, g, b;
+    g = 0;
+    b = 0;
+    r = 0;
     while(true)
     {
-        fprintf(stderr, "x=%f y=%f theta=%f\n", get);
-        sleep(1);
+        fprintf(stderr, "x=%f y=%f theta=%f kidnapped=%s\n", getX(robot), getY(robot), getTheta(robot), getKidnapped(robot) ? "true" : "false");
+        setVisualEffect(robot, 0, r, g, b, 255);
+        r += 50;
+        r %= 255;
+        usleep(10000);
     }
 }