commit 61b0791ab8abebee78caa6501e01a0d7c9614d80
Author: Thomas Schöpping <tschoepp@cit-ec.uni-bielefeld.de>
Date:   Fri Aug 11 19:24:46 2017 +0200

    Integrated the Wiimote demo to the PowerManagement logic.
    
    The blouetooth drivers were slightly enhanced.

diff --git a/components/bluetooth/bluetooth-connector.cpp b/components/bluetooth/bluetooth-connector.cpp
index 1ad73e4..0f7ba25 100644
--- a/components/bluetooth/bluetooth-connector.cpp
+++ b/components/bluetooth/bluetooth-connector.cpp
@@ -63,7 +63,7 @@ void BluetoothConnector::bluetoothConnectorConnect(const char *addr) {
         break;
       }
       iwrap->bluetoothIwrapSendCommand(cmd);
-      BaseThread::sleep(MS2ST(5000));    // waiting connection establish
+      BaseThread::sleep(MS2ST(1000));    // waiting connection establish
       break;
     }
   }
diff --git a/components/bluetooth/bluetooth-wiimote.cpp b/components/bluetooth/bluetooth-wiimote.cpp
index d96e59a..a8939b6 100644
--- a/components/bluetooth/bluetooth-wiimote.cpp
+++ b/components/bluetooth/bluetooth-wiimote.cpp
@@ -3,9 +3,13 @@
 
 #include <amiro/bluetooth/bluetooth-wiimote.hpp>
 
+#include <global.hpp>
+
 using namespace chibios_rt;
 using namespace amiro;
 
+extern Global global;
+
 /*
  * Class constructor
  */
@@ -39,18 +43,6 @@ msg_t BluetoothWiimote::wiimoteReceive() {
   size_t length;
   msg_t msg;
 
-  static uint8_t button_up;
-  static uint8_t button_down;
-  static uint8_t button_right;
-  static uint8_t button_left;
-  static uint8_t button_plus;
-  static uint8_t button_home;
-  static uint8_t button_minus;
-  static uint8_t button_A;
-  static uint8_t button_B;
-  static uint8_t button_1;
-  static uint8_t button_2;
-
   msg = mailbox.fetch((msg_t*) &recv_descriptor, TIME_INFINITE);
   if ((msg == RDY_RESET) || stopflag)
     return RDY_RESET;
@@ -58,68 +50,26 @@ msg_t BluetoothWiimote::wiimoteReceive() {
   buffer = recv_descriptor->bluetoothDescriptorGetPayload();
   length = recv_descriptor->bluetoothDescriptorGetPayloadLength();
 
-  if (buffer[0] == 0xA1 && buffer[1] == 0x31) {
-    accelerometer.x_axis = (buffer[4] << 2) + ((buffer[2] & 0x60) >> 5) - 0x1EC;
-    accelerometer.y_axis = (buffer[5] << 2) + ((buffer[3] & 0x20) >> 4) - 0x1EA;
-    accelerometer.z_axis = (buffer[6] << 2) + ((buffer[3] & 0x40) >> 5) - 0x1EE;
-
-    if (buffer[3] & 0x80) {             // Press home to return button reporting
-      bluetoothWiimoteDataBtn();
-      accelerometer.x_axis = 0;
-      accelerometer.y_axis = 0;
-      accelerometer.z_axis = 0;
-    }
-
-  } else if (buffer[0] == 0xA1 && buffer[1] == 0x30) {
-    button_up    = (buffer[2] & 0x08) >> 3;
-    button_down  = (buffer[2] & 0x04) >> 2;
-    button_right = (buffer[2] & 0x02) >> 1;
-    button_left  = (buffer[2] & 0x01) >> 0;
-    button_plus  = (buffer[2] & 0x10) >> 4;
-    button_home  = (buffer[3] & 0x80) >> 7;
-    button_minus = (buffer[3] & 0x10) >> 4;
-    button_A     = (buffer[3] & 0x08) >> 3;
-    button_B     = (buffer[3] & 0x04) >> 2;
-    button_1     = (buffer[3] & 0x02) >> 1;
-    button_2     = (buffer[3] & 0x01) >> 0;
-
-    if (button_up)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'U');
-
-    if (button_down)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'D');
-
-    if (button_right)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'R');
-
-    if (button_left)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'L');
+  if (buffer[0] == 0xA1 && (buffer[1] == 0x30 || buffer[1] == 0x31)) {
+      buttons.left  = (buffer[2] & 0x01) ? 1 : 0;
+      buttons.right = (buffer[2] & 0x02) ? 1 : 0;
+      buttons.down  = (buffer[2] & 0x04) ? 1 : 0;
+      buttons.up    = (buffer[2] & 0x08) ? 1 : 0;
+      buttons.plus  = (buffer[2] & 0x10) ? 1 : 0;
+      buttons.two   = (buffer[3] & 0x01) ? 1 : 0;
+      buttons.one   = (buffer[3] & 0x02) ? 1 : 0;
+      buttons.B     = (buffer[3] & 0x04) ? 1 : 0;
+      buttons.A     = (buffer[3] & 0x08) ? 1 : 0;
+      buttons.minus = (buffer[3] & 0x10) ? 1 : 0;
+      buttons.home  = (buffer[3] & 0x80) ? 1 : 0;
+
+      accelerometer.x_axis = (buffer[4] << 2) + ((buffer[2] & 0x60) >> 5) - 0x1FF;
+      accelerometer.y_axis = (buffer[5] << 2) + ((buffer[3] & 0x20) >> 4) - 0x1FF;
+      accelerometer.z_axis = (buffer[6] << 2) + ((buffer[3] & 0x40) >> 5) - 0x1FF;
 
-    if (button_plus)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, '+');
-
-    if (button_home)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'H');
-
-    if (button_minus)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, '-');
-
-    if (button_A)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'A');
-
-    if (button_B)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, 'B');
-
-    if (button_1)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, '1');
-
-    if (button_2)
-      chSequentialStreamPut((BaseSequentialStream*) &SD1, '2');
-
-    if (button_minus && button_plus)   // Press minus and plue to return accelerometer reporting
       bluetoothWiimoteDataBtnAcc();
   } else {
-    chSequentialStreamWrite((BaseSequentialStream*) &SD1, buffer, length);
+    chSequentialStreamWrite((BaseSequentialStream*) &global.sercanmux1, buffer, length);
   }
 
   msg = iwrap->transport.bluetoothTransportGetStorageMailbox()->post((msg_t) recv_descriptor, TIME_INFINITE);
@@ -179,6 +129,10 @@ BluetoothWiimote::Accelerometer * BluetoothWiimote::getAccelerometer() {
   return &accelerometer;
 }
 
+BluetoothWiimote::Buttons * BluetoothWiimote::getButtons() {
+  return &buttons;
+}
+
 /*
  * @brief :  On-off LEDs and Motor of Wiimote.
  *
diff --git a/devices/PowerManagement/main.cpp b/devices/PowerManagement/main.cpp
index 488aaa2..f87e9ba 100644
--- a/devices/PowerManagement/main.cpp
+++ b/devices/PowerManagement/main.cpp
@@ -858,6 +858,36 @@ void shellRequestGetBootloaderInfo(BaseSequentialStream* chp, int argc, char *ar
   return;
 }
 
+void shellRequestWiiSteering(BaseSequentialStream* chp, int argc, char *argv[]) {
+  // if Wii steering is currently active, stop it
+  if (global.userThread.getCurrenState() == UserThread::WII_STEERING) {
+    global.userThread.setNextState(UserThread::IDLE);
+  }
+  // check arguments and (try to) enable Wii steering
+  else {
+    // if arguments invalid
+    if (argc < 1 || argc > 2 || global.userThread.setWiiAddress(argv[0]) != RDY_OK) {
+      chprintf(chp, "Warning: invalid arguments\n");
+      chprintf(chp, "Usage: %s\n", "wii_steering <address> [<deadzone>]");
+      chprintf(chp, "\n");
+      chprintf(chp, "\taddress\n");
+      chprintf(chp, "bluetooth address of the Wiimote controller to pair with.\n");
+      chprintf(chp, "\tdeadzone\n");
+      chprintf(chp, "deadzone to set for the Wiimote controller [default = 10%%].\n");
+      return;
+    }
+    else {
+      // set deadzone
+      const float deadzone = global.userThread.setWiiDeadzone((argc == 2) ? std::atof(argv[1]) : 0.1f);
+      chprintf(chp, "deadzone set to %u%%\n", (unsigned int)(deadzone * 100.0f));
+
+      // start Wii steering behaviour
+      global.userThread.setNextState(UserThread::WII_STEERING);
+    }
+  }
+  return;
+}
+
 static const ShellCommand commands[] = {
   {"shutdown", shellRequestShutdown},
   {"check", shellRequestCheck},
@@ -876,6 +906,7 @@ static const ShellCommand commands[] = {
   {"print_vcnl", shellRequestPrintVCNL},
   {"shell_board", shellSwitchBoardCmd},
   {"get_bootloader_info", shellRequestGetBootloaderInfo},
+  {"wii_steering", shellRequestWiiSteering},
   {NULL, NULL}
 };
 
diff --git a/devices/PowerManagement/userthread.cpp b/devices/PowerManagement/userthread.cpp
index da4acfb..f4ead67 100644
--- a/devices/PowerManagement/userthread.cpp
+++ b/devices/PowerManagement/userthread.cpp
@@ -3,16 +3,17 @@
 #include "global.hpp"
 #include <array>
 #include <chprintf.h>
+#include <cmath>
 
 using namespace amiro;
 
 extern Global global;
 
-uint16_t touch;
-std::array<uint16_t, 8> proximity;
-std::array<float, 8> proxNormalized;
+volatile UserThread::State current_state;
+volatile UserThread::State next_state;
+types::kinematic kinematic;
 
-bool running;
+namespace obstacle_avoidance {
 
 uint16_t constexpr proxThresholdLow = 0x0000;
 uint16_t constexpr proxThresholdHigh = 0x1000;
@@ -40,8 +41,6 @@ types::kinematic constexpr defaultKinematic = {
     /* w_z [µrad/s] */ 0
 };
 
-
-
 inline uint8_t ProxId2LedId(const uint8_t proxId) {
     return (proxId < 4) ? proxId+4 : proxId-4;
 }
@@ -57,6 +56,22 @@ Color Prox2Color(const float prox) {
   }
 }
 
+} /* namespace obstacle_avoidance */
+
+namespace wii_steering {
+
+BluetoothWiimote wiimote(&global.wt12, RX_TX);
+BluetoothSerial btserial(&global.wt12, RX_TX);
+
+float deadzone;
+char bt_address[18] = {'\0'};
+float wiimoteCalib[3] = {0.0f};
+
+uint32_t constexpr maxTranslation = 500e3;
+uint32_t constexpr maxRotation = 3.1415927f * 1000000.0f * 2.0f;
+
+}
+
 UserThread::UserThread() :
   chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>()
 {
@@ -69,102 +84,315 @@ UserThread::~UserThread()
 msg_t
 UserThread::main()
 {
-    uint8_t sensor = 0;
-    float factor_x = 0.0f;
-    float factor_wz = 0.0f;
-    types::kinematic kinematic = defaultKinematic;
+  /*
+   * initialize some variables
+   */
+  current_state = IDLE;
 
-    for (uint8_t led = 0; led < 8; ++led) {
-      global.robot.setLightColor(led, Color(Color::BLACK));
-    }
-    running = false;
-
-    while (!this->shouldTerminate())
-    {
-        /*
-         * read touch sensor values
-         */
-        touch = global.mpr121.getButtonStatus();
-
-        /*
-         * evaluate touch input
-         */
-        if (touch == 0x0F) {
-            if (running) {
-                // stop the robot
-                running = false;
-                kinematic = {0, 0, 0, 0, 0, 0};
-                global.robot.setTargetSpeed(kinematic);
-            } else {
-                // start the robot
-                running = true;
+  /*
+   * set all LEDs black (off)
+   */
+  for (uint8_t led = 0; led < 8; ++led) {
+    global.robot.setLightColor(led, Color(Color::BLACK));
+  }
+
+  /*
+   * thread loop
+   */
+  while (!this->shouldTerminate()) {
+    /*
+     * handle changes of the state
+     */
+    if (next_state != current_state) {
+      switch (current_state) {
+        case IDLE:
+        {
+          if (next_state == OBSTACLE_AVOIDANCE) {
+            // set all LEDs to white for one second
+            for (uint8_t led = 0; led < 8; ++led) {
+              global.robot.setLightColor(led, Color(Color::WHITE));
+            }
+            this->sleep(MS2ST(1000));
+            for (uint8_t led = 0; led < 8; ++led) {
+              global.robot.setLightColor(led, Color(Color::BLACK));
             }
+          }
+          /* if (this->next_state == WII_STEERING) */ else {
+            // setup bluetooth
+            wii_steering::wiimote.bluetoothWiimoteListen(wii_steering::bt_address);
+            wii_steering::btserial.bluetoothSerialListen("ALL");
+
+            // set LEDs: front = green; rear = red; sides = blue
+            global.robot.setLightColor(constants::LightRing::LED_NNW, Color(Color::GREEN));
+            global.robot.setLightColor(constants::LightRing::LED_NNE, Color(Color::GREEN));
+            global.robot.setLightColor(constants::LightRing::LED_SSW, Color(Color::RED));
+            global.robot.setLightColor(constants::LightRing::LED_SSE, Color(Color::RED));
+            global.robot.setLightColor(constants::LightRing::LED_WNW, Color(Color::BLUE));
+            global.robot.setLightColor(constants::LightRing::LED_WSW, Color(Color::BLUE));
+            global.robot.setLightColor(constants::LightRing::LED_ENE, Color(Color::BLUE));
+            global.robot.setLightColor(constants::LightRing::LED_ESE, Color(Color::BLUE));
+
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "press buttons '1' and '2' to connect\n");
+          }
+          break;
+        }
+        case OBSTACLE_AVOIDANCE:
+        {
+          if (next_state == IDLE) {
+            // stop the robot
+            kinematic = {0, 0, 0, 0, 0, 0};
+            global.robot.setTargetSpeed(kinematic);
 
             // set all LEDs to white for one second
             for (uint8_t led = 0; led < 8; ++led) {
-                global.robot.setLightColor(led, Color(Color::WHITE));
+              global.robot.setLightColor(led, Color(Color::WHITE));
             }
             this->sleep(MS2ST(1000));
             for (uint8_t led = 0; led < 8; ++led) {
-                global.robot.setLightColor(led, Color(Color::BLACK));
+              global.robot.setLightColor(led, Color(Color::BLACK));
             }
+          }
+          /* if (this->next_state == WII_STEERING) */ else {
+            // must turn off obstacle avoidance first
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "ERROR: turn off obstacle avoidance first!\n");
+            next_state = OBSTACLE_AVOIDANCE;
+          }
+          break;
         }
+        case WII_STEERING: {
+          if (next_state == IDLE) {
+            // stop the robot
+            kinematic = {0, 0, 0, 0, 0, 0};
+            global.robot.setTargetSpeed(kinematic);
 
-        if (running) {
-            /*
-             * read proximity values
-             */
-            for (sensor = 0; sensor < 8; ++sensor) {
-                proximity[sensor] = global.vcnl4020[sensor].getProximityScaledWoOffset();
-                //proxNormalized[sensor] += 2.0f * (proxNormalized[sensor] * (1.0f - proxNormalized[sensor])); // non linearity
+            // disconnect from Wiimote controller
+            wii_steering::wiimote.bluetoothWiimoteDisconnect(wii_steering::bt_address);
+            wii_steering::btserial.bluetoothSerialStop();
+            wii_steering::wiimote.bluetoothWiimoteStop();
+
+            // set all LEDs to black
+            for (uint8_t led = 0; led < 8; ++led) {
+              global.robot.setLightColor(led, Color(Color::BLACK));
             }
+          }
+          /* if (this->next_state == OBSTACLE_AVOIDANCE) */ else {
+            // must turn off wii steering first
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "ERROR: turn off wii steering first!\n");
+            next_state = WII_STEERING;
+          }
+          break;
+        }
+      }
+      current_state = next_state;
+    }
 
-            /*
-             * normalize proximity values
-             */
-            for (sensor = 0; sensor < 8; ++sensor) {
-                register uint16_t prox = proximity[sensor];
-                // limit to high treshold
-                if (prox > proxThresholdHigh)
-                    prox = proxThresholdHigh;
-                // limit to low threshold
-                else if (prox < proxThresholdLow)
-                    prox = proxThresholdLow;
-                // apply low threshold
-                prox -= proxThresholdLow;
-                // normalize to [0, 1]
-                proxNormalized[sensor] = float(prox) / float(proxRange);
+    // sleep here so the loop is executed as quickly as possible
+    this->sleep(CAN::UPDATE_PERIOD);
+
+    /*
+     * exeute behaviour depending on the current state
+     */
+    switch (current_state) {
+      case IDLE:
+      {
+        // read touch sensors
+        if (global.mpr121.getButtonStatus() == 0x0F) {
+          next_state = OBSTACLE_AVOIDANCE;
+        }
+        break;
+      }
+      case OBSTACLE_AVOIDANCE:
+      {
+        // read touch sensors
+        if (global.mpr121.getButtonStatus() == 0x0F) {
+          next_state = IDLE;
+          break;
+        }
+
+        // initialize some variables
+        uint8_t sensor = 0;
+        std::array<uint16_t, 8> proximity;
+        std::array<float, 8> proxNormalized;
+        float factor_x = 0.0f;
+        float factor_wz = 0.0f;
+
+        // read proximity values
+        for (sensor = 0; sensor < 8; ++sensor) {
+          proximity[sensor] = global.vcnl4020[sensor].getProximityScaledWoOffset();
+        }
+
+        // normalize proximity values
+        for (sensor = 0; sensor < 8; ++sensor) {
+          register uint16_t prox = proximity[sensor];
+          // limit to high treshold
+          if (prox > obstacle_avoidance::proxThresholdHigh)
+              prox = obstacle_avoidance::proxThresholdHigh;
+          // limit to low threshold
+          else if (prox < obstacle_avoidance::proxThresholdLow)
+              prox = obstacle_avoidance::proxThresholdLow;
+          // apply low threshold
+          prox -= obstacle_avoidance::proxThresholdLow;
+          // normalize to [0, 1]
+          proxNormalized[sensor] = float(prox) / float(obstacle_avoidance::proxRange);
+        }
+
+        // map the sensor values to the top LEDs
+        for (sensor = 0; sensor < 8; ++sensor) {
+          global.robot.setLightColor(obstacle_avoidance::ProxId2LedId(sensor), obstacle_avoidance::Prox2Color(proxNormalized[sensor]));
+        }
+
+        // evaluate NAM
+        for (sensor = 0; sensor < 8; ++sensor) {
+          factor_x += proxNormalized[sensor] * obstacle_avoidance::namMatrix[sensor][0];
+          factor_wz += proxNormalized[sensor] * obstacle_avoidance::namMatrix[sensor][1];
+        }
+
+        // set motor commands
+        kinematic = obstacle_avoidance::defaultKinematic;
+        kinematic.x += (factor_x * obstacle_avoidance::baseTranslation) + 0.5f;
+        kinematic.w_z += (factor_wz * obstacle_avoidance::baseRotation) + 0.5f;
+        global.robot.setTargetSpeed(kinematic);
+
+        break;
+      }
+      case WII_STEERING:
+      {
+        // if not yet connected to the Wiimote controller
+        if (!wii_steering::wiimote.bluetoothWiimoteIsConnected()) {
+          // try to connect
+          chprintf((BaseSequentialStream*)&global.sercanmux1, "connecting...\n");
+          wii_steering::wiimote.bluetoothWiimoteConnect(wii_steering::bt_address);
+
+          if (wii_steering::wiimote.bluetoothWiimoteIsConnected()) {
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "connection established\n");
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "\n");
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "Wiimote control:\n");
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "\tpress 'home' to calibrate\n");
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "\thold 'A' to steer\n");
+          }
+        }
+        // steer AMiRo using the Wiimote controller like a joystick
+        else {
+          // initialize some variables
+          float wiimoteAcc[3] = {0.0f, 0.0f, 0.0f};
+
+          // get Wiimote accelerometer data
+          wiimoteAcc[0] = wii_steering::wiimote.getAccelerometer()->x_axis;
+          wiimoteAcc[1] = wii_steering::wiimote.getAccelerometer()->y_axis;
+          wiimoteAcc[2] = wii_steering::wiimote.getAccelerometer()->z_axis;
+
+          // calibrate accelerometer offset
+          if (wii_steering::wiimote.getButtons()->home) {
+            wii_steering::wiimoteCalib[0] = wiimoteAcc[0];
+            wii_steering::wiimoteCalib[1] = wiimoteAcc[1] + 100.0f;
+            wii_steering::wiimoteCalib[2] = wiimoteAcc[2];
+
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "accelerometer calibrated:\n");
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "\tX = %d\n", (int32_t)wii_steering::wiimoteCalib[0]);
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "\tY = %d\n", (int32_t)wii_steering::wiimoteCalib[1]);
+            chprintf((BaseSequentialStream*)&global.sercanmux1, "\tZ = %d\n", (int32_t)wii_steering::wiimoteCalib[2]);
+          }
+
+          for (uint8_t axis = 0; axis < 3; ++axis) {
+            // apply calibration values
+            wiimoteAcc[axis] -= wii_steering::wiimoteCalib[axis];
+
+            // normalize to (-1, 1)
+            wiimoteAcc[axis] /= 100.0f;
+
+            // limit to 1G
+            if (wiimoteAcc[axis] > 1.0f) {
+              wiimoteAcc[axis] = 1.0f;
+            } else if (wiimoteAcc[axis] < -1.0f) {
+              wiimoteAcc[axis] = -1.0f;
             }
 
-            /*
-             * map the sensor values to the top LEDs
-             */
-            for (sensor = 0; sensor < 8; ++sensor) {
-                global.robot.setLightColor(ProxId2LedId(sensor), Prox2Color(proxNormalized[sensor]));
+            // apply deadzone
+            if (std::fabs(wiimoteAcc[axis]) < wii_steering::deadzone) {
+              wiimoteAcc[axis] = 0.0f;
             }
 
             /*
-             * evaluate NAM
+             * the value is now in (-1 .. -deazone, 0, deadzone .. 1)
+             * note the gaps [-deadzone .. 0] and [0 .. deadzone]
              */
-            factor_x = 0.0f;
-            factor_wz = 0.0f;
-            for (sensor = 0; sensor < 8; ++sensor) {
-                factor_x += proxNormalized[sensor] * namMatrix[sensor][0];
-                factor_wz += proxNormalized[sensor] * namMatrix[sensor][1];
+
+            // normalize (deadzone, 1) to (0, 1) and (-1, -deadzone) tpo (-1, 0)
+            if (wiimoteAcc[axis] > 0) {
+              wiimoteAcc[axis] -= wii_steering::deadzone;
+            } else if (wiimoteAcc[axis] < 0){
+              wiimoteAcc[axis] += wii_steering::deadzone;
             }
+            wiimoteAcc[axis] *= (1.0f / (1.0f - wii_steering::deadzone));
+          }
 
-            /*
-             * set motor commands
-             */
-            kinematic = defaultKinematic;
-            kinematic.x += (factor_x * baseTranslation) + 0.5f;
-            kinematic.w_z += (factor_wz * baseRotation) + 0.5f;
-            global.robot.setTargetSpeed(kinematic);
+          // only move when A is pressed
+          if (wii_steering::wiimote.getButtons()->A) {
+            // set kinematic relaive to maximum speeds
+            kinematic.x = wii_steering::maxTranslation * wiimoteAcc[2];
+            kinematic.w_z = wii_steering::maxRotation * wiimoteAcc[0] * ((wiimoteAcc[2] < 0.0f) ? 1.0f : -1.0f);
+          } else {
+            kinematic = {0, 0, 0, 0, 0, 0};
+          }
+
+          // set speed
+          global.robot.setTargetSpeed(kinematic);
         }
 
-        this->sleep(MS2ST(10));
+        break;
+      }
     }
+  }
+
+  // stop the robot
+  kinematic = {0, 0, 0, 0, 0, 0};
+  global.robot.setTargetSpeed(kinematic);
 
   return RDY_OK;
 }
 
+void
+UserThread::setNextState(const UserThread::State state)
+{
+  next_state = state;
+  return;
+}
+
+UserThread::State
+UserThread::getCurrenState() const
+{
+  return current_state;
+}
+
+msg_t
+UserThread::setWiiAddress(const char* address)
+{
+  if (strlen(address) != 17) {
+    return RDY_RESET;
+  }
+  else {
+    strcpy(wii_steering::bt_address, address);
+    return RDY_OK;
+  }
+}
+
+float
+UserThread::setWiiDeadzone(const float deadzone)
+{
+  // check for negative value and limit to zero
+  float dz = (deadzone < 0.0f) ? 0.0f : deadzone;
+
+  // if value is >1, range is assumed to be (0, 100)
+  if (dz > 1.0f) {
+    // limit to 100
+    if (dz > 100.0f) {
+      dz = 100.0f;
+    }
+    dz /= 100.0f;
+  }
+
+  // set value and return it
+  wii_steering::deadzone = dz;
+  return dz;
+}
+
diff --git a/devices/PowerManagement/userthread.h b/devices/PowerManagement/userthread.h
index ac056b1..2c2eeba 100644
--- a/devices/PowerManagement/userthread.h
+++ b/devices/PowerManagement/userthread.h
@@ -9,11 +9,27 @@ namespace amiro {
 class UserThread : public chibios_rt::BaseStaticThread<USER_THREAD_STACK_SIZE>
 {
 public:
+
+  enum State {
+    IDLE,
+    OBSTACLE_AVOIDANCE,
+    WII_STEERING,
+  };
+
+public:
   explicit UserThread();
 
   virtual ~UserThread();
 
   virtual msg_t main();
+
+  void setNextState(const State state);
+
+  State getCurrenState() const;
+
+  msg_t setWiiAddress(const char* address);
+
+  float setWiiDeadzone(const float deadzone);
 };
 
 } // end of namespace amiro
diff --git a/include/amiro/bluetooth/bluetooth-wiimote.hpp b/include/amiro/bluetooth/bluetooth-wiimote.hpp
index c23065f..b1d312d 100644
--- a/include/amiro/bluetooth/bluetooth-wiimote.hpp
+++ b/include/amiro/bluetooth/bluetooth-wiimote.hpp
@@ -33,6 +33,21 @@ namespace amiro {
     };
     Accelerometer *getAccelerometer();
 
+    struct Buttons {
+      uint16_t left : 1;
+      uint16_t right : 1;
+      uint16_t down : 1;
+      uint16_t up : 1;
+      uint16_t plus : 1;
+      uint16_t two : 1;
+      uint16_t one : 1;
+      uint16_t B : 1;
+      uint16_t A : 1;
+      uint16_t minus : 1;
+      uint16_t home : 1;
+    };
+    Buttons* getButtons();
+
   protected:
     virtual msg_t main(void);
 
@@ -50,6 +65,7 @@ namespace amiro {
     uint8_t stopflag;
 
     Accelerometer accelerometer;
+    Buttons buttons;
   };
 }
 
