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/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;
+}
+
