commit a2a2cf8b515d257fb2cf0ba370638dc2f3b59eaa
Author: Jan <janlauff+git@techfak.de>
Date:   Thu Feb 13 10:13:51 2020 +0100

    Add QuadDRV-CompassBelt example

diff --git a/firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.cpp b/firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.cpp
new file mode 100644
index 0000000..66be189
--- /dev/null
+++ b/firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.cpp
@@ -0,0 +1,136 @@
+/***************************************************
+  This is a library for the Adafruit DRV2605L Haptic Driver
+
+  ----> http://www.adafruit.com/products/2306
+
+  Check out the links above for our tutorials and wiring diagrams
+  This motor/haptic driver uses I2C to communicate
+
+  Adafruit invests time and resources providing this open source code,
+  please support Adafruit and open-source hardware by purchasing
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.
+  MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#include "Adafruit_DRV2605.h"
+
+/**************************************************************************/
+/*!
+    @brief  Instantiates a new DRV2605 class
+*/
+/**************************************************************************/
+// I2C, no address adjustments or pins
+Adafruit_DRV2605::Adafruit_DRV2605(uint8_t channel) {
+  _channel = channel;
+}
+
+
+/**************************************************************************/
+/*!
+    @brief  Setups the HW
+*/
+/**************************************************************************/
+boolean Adafruit_DRV2605::begin() {
+  uint8_t id = readRegister8(DRV2605_REG_STATUS);
+  //Serial.print("Status 0x"); Serial.println(id, HEX);
+
+  writeRegister8(DRV2605_REG_MODE, 0x00); // out of standby
+
+  writeRegister8(DRV2605_REG_RTPIN, 0x00); // no real-time-playback
+
+  writeRegister8(DRV2605_REG_WAVESEQ1, 1); // strong click
+  writeRegister8(DRV2605_REG_WAVESEQ2, 0);
+
+  writeRegister8(DRV2605_REG_OVERDRIVE, 0); // no overdrive
+
+  writeRegister8(DRV2605_REG_SUSTAINPOS, 0);
+  writeRegister8(DRV2605_REG_SUSTAINNEG, 0);
+  writeRegister8(DRV2605_REG_BREAK, 0);
+  writeRegister8(DRV2605_REG_AUDIOMAX, 0x64);
+
+  // ERM open loop
+
+  // turn off N_ERM_LRA
+  writeRegister8(DRV2605_REG_FEEDBACK, readRegister8(DRV2605_REG_FEEDBACK) & 0x7F);
+  // turn on ERM_OPEN_LOOP
+  writeRegister8(DRV2605_REG_CONTROL3, readRegister8(DRV2605_REG_CONTROL3) | 0x20);
+
+  return true;
+}
+
+void Adafruit_DRV2605::setWaveform(uint8_t slot, uint8_t w) {
+  writeRegister8(DRV2605_REG_WAVESEQ1 + slot, w);
+}
+
+void Adafruit_DRV2605::selectLibrary(uint8_t lib) {
+  writeRegister8(DRV2605_REG_LIBRARY, lib);
+}
+
+void Adafruit_DRV2605::go() {
+  writeRegister8(DRV2605_REG_GO, 1);
+}
+
+void Adafruit_DRV2605::setMode(uint8_t mode) {
+  writeRegister8(DRV2605_REG_MODE, mode);
+}
+
+void Adafruit_DRV2605::setRealtimeValue(uint8_t rtp) {
+  writeRegister8(DRV2605_REG_RTPIN, rtp);
+}
+
+/********************************************************************/
+
+uint8_t Adafruit_DRV2605::readRegister8(uint8_t reg) {
+  selectChannel(_channel);
+  uint8_t x ;
+  // use i2c
+  Wire.beginTransmission(DRV2605_ADDR);
+  Wire.write((byte)reg);
+  Wire.endTransmission();
+  Wire.requestFrom((byte)DRV2605_ADDR, (byte)1);
+  x = Wire.read();
+  //  Serial.print("$"); Serial.print(reg, HEX);
+  //  Serial.print(": 0x"); Serial.println(x, HEX);
+  return x;
+}
+
+void Adafruit_DRV2605::writeRegister8(uint8_t reg, uint8_t val) {
+  selectChannel(_channel);
+  // use i2c
+  Wire.beginTransmission(DRV2605_ADDR);
+  Wire.write((byte)reg);
+  Wire.write((byte)val);
+  Wire.endTransmission();
+}
+
+/****************/
+
+
+// Allow users to use ERM motor or LRA motors
+
+void Adafruit_DRV2605::useERM ()
+{ 
+  writeRegister8(DRV2605_REG_FEEDBACK, readRegister8(DRV2605_REG_FEEDBACK) & 0x7F);
+}
+
+void Adafruit_DRV2605::useLRA ()
+{
+  writeRegister8(DRV2605_REG_FEEDBACK, readRegister8(DRV2605_REG_FEEDBACK) | 0x80);
+}
+
+void Adafruit_DRV2605::selectChannel (uint8_t channel)
+{
+  // choose channel, all address bits to gnd on PCA9546A/TCA9548A = 0x70
+  Wire.beginTransmission(0x77);
+  Wire.write(1 << channel);
+  Wire.endTransmission();
+}
diff --git a/firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.h b/firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.h
new file mode 100644
index 0000000..f533bb1
--- /dev/null
+++ b/firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.h
@@ -0,0 +1,98 @@
+/***************************************************
+  This is a library for the Adafruit DRV2605L Haptic Driver
+
+  ----> http://www.adafruit.com/products/2305
+
+  Check out the links above for our tutorials and wiring diagrams
+  This motor/haptic driver uses I2C to communicate
+
+  Adafruit invests time and resources providing this open source code,
+  please support Adafruit and open-source hardware by purchasing
+  products from Adafruit!
+
+  Written by Limor Fried/Ladyada for Adafruit Industries.
+  MIT license, all text above must be included in any redistribution
+ ****************************************************/
+
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+#if defined(__MK20DX128__) || defined(__MK20DX256__)
+#include "i2c_t3.h"
+#else
+#include "Wire.h"
+#endif
+
+
+
+#define DRV2605_ADDR 0x5A
+
+#define DRV2605_REG_STATUS 0x00
+#define DRV2605_REG_MODE 0x01
+#define DRV2605_MODE_INTTRIG  0x00
+#define DRV2605_MODE_EXTTRIGEDGE  0x01
+#define DRV2605_MODE_EXTTRIGLVL  0x02
+#define DRV2605_MODE_PWMANALOG  0x03
+#define DRV2605_MODE_AUDIOVIBE  0x04
+#define DRV2605_MODE_REALTIME  0x05
+#define DRV2605_MODE_DIAGNOS  0x06
+#define DRV2605_MODE_AUTOCAL  0x07
+
+
+#define DRV2605_REG_RTPIN 0x02
+#define DRV2605_REG_LIBRARY 0x03
+#define DRV2605_REG_WAVESEQ1 0x04
+#define DRV2605_REG_WAVESEQ2 0x05
+#define DRV2605_REG_WAVESEQ3 0x06
+#define DRV2605_REG_WAVESEQ4 0x07
+#define DRV2605_REG_WAVESEQ5 0x08
+#define DRV2605_REG_WAVESEQ6 0x09
+#define DRV2605_REG_WAVESEQ7 0x0A
+#define DRV2605_REG_WAVESEQ8 0x0B
+
+#define DRV2605_REG_GO 0x0C
+#define DRV2605_REG_OVERDRIVE 0x0D
+#define DRV2605_REG_SUSTAINPOS 0x0E
+#define DRV2605_REG_SUSTAINNEG 0x0F
+#define DRV2605_REG_BREAK 0x10
+#define DRV2605_REG_AUDIOCTRL 0x11
+#define DRV2605_REG_AUDIOLVL 0x12
+#define DRV2605_REG_AUDIOMAX 0x13
+#define DRV2605_REG_RATEDV 0x16
+#define DRV2605_REG_CLAMPV 0x17
+#define DRV2605_REG_AUTOCALCOMP 0x18
+#define DRV2605_REG_AUTOCALEMP 0x19
+#define DRV2605_REG_FEEDBACK 0x1A
+#define DRV2605_REG_CONTROL1 0x1B
+#define DRV2605_REG_CONTROL2 0x1C
+#define DRV2605_REG_CONTROL3 0x1D
+#define DRV2605_REG_CONTROL4 0x1E
+#define DRV2605_REG_VBAT 0x21
+#define DRV2605_REG_LRARESON 0x22
+
+
+class Adafruit_DRV2605 {
+  public:
+    Adafruit_DRV2605(uint8_t channel);
+    boolean begin(void);
+    void selectChannel(uint8_t channel);
+    void writeRegister8(uint8_t reg, uint8_t val);
+    uint8_t readRegister8(uint8_t reg);
+    void setWaveform(uint8_t slot, uint8_t w);
+    void selectLibrary(uint8_t lib);
+    void go(void);
+    void setMode(uint8_t mode);
+    void setRealtimeValue(uint8_t rtp);
+    // Select ERM (Eccentric Rotating Mass) or LRA (Linear Resonant Actuator) vibration motor
+    // The default is ERM, which is more common
+    void useERM();
+    void useLRA();
+
+  private:
+    uint8_t _channel;
+};
+
diff --git a/firmware/demo/QuadDRV-CompassBelt/QuadDRV-CompassBelt.ino b/firmware/demo/QuadDRV-CompassBelt/QuadDRV-CompassBelt.ino
new file mode 100644
index 0000000..6669602
--- /dev/null
+++ b/firmware/demo/QuadDRV-CompassBelt/QuadDRV-CompassBelt.ino
@@ -0,0 +1,151 @@
+/*
+ * Compass belt control code. Reads angle from magnetometer and activates
+ * the two nearest LRAs for a short duration with an intensity
+ * anti-proportional to the angle distance.
+ * 
+ * Operation Overview:
+ *  - Compute angle between current orientation and north
+ *  - operate closest pair of LRAs for 300 ms, intensity depends 
+ *    on angular distance to north
+ *
+ * Operation Instructions:
+ *  - BRIX/ Magnetometer in front
+ *  - LRAs diagonally at 45, 135, 225, 315 degrees
+ *  - Turn on Brix 
+ *  - Calibrate magnetometer by moving around
+ *  - Put on belt, should be worn firmly on body for good coupling
+ */
+
+#include <Wire.h>
+#include "Adafruit_DRV2605.h"
+#include <Adafruit_Sensor.h>
+#include <Adafruit_BNO055.h>
+#include <utility/imumaths.h>
+#include <math.h>
+
+bool debug = true;
+
+/* ========== MAGNETOMETER ========== */
+
+/*
+ * Check I2C device address and correct line below.
+ * By default address is 0x29 or 0x28.
+ */
+Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);
+
+/* ============= LRAs =============== */
+
+const uint8_t channelCount = 4; // belt has 3 channels
+Adafruit_DRV2605 drvs[channelCount] = {
+  Adafruit_DRV2605(0),  
+  Adafruit_DRV2605(1),
+  Adafruit_DRV2605(2),
+  Adafruit_DRV2605(3)
+};
+
+const int thresholds[channelCount] = {45, 135, 225, 315};
+const int MAX_INTENSITY = 127;
+const int DURATION = 300;
+
+/* ================================== */
+
+void setup() {
+  Serial.begin(9600);
+  delay(500);
+
+  Serial.println("Booting");
+  /*
+   * Sensor setup.
+   */
+  if(!bno.begin()) {
+    /* There was a problem detecting the BNO055 ... check your connections */
+    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
+    while(1);
+  }
+  bno.setExtCrystalUse(true);
+
+  /*
+   * LRA setup.
+   */
+  Wire.begin();
+  
+  for (uint8_t i = 0; i < channelCount; i++) {
+    drvs[i].begin();
+    drvs[i].selectLibrary(6);  // LRA
+    drvs[i].useLRA();
+    drvs[i].setMode(DRV2605_MODE_REALTIME);
+  }
+
+  /*
+   * Sensor calibration.
+   */
+  Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
+  uint8_t system, gyro, accel, mag = 0;
+  while(mag != 3) {
+    bno.getCalibration(&system, &gyro, &accel, &mag);
+    Serial.print("CALIBRATION: Mag=");
+    Serial.println(mag, DEC);
+  }
+}
+
+void loop() {
+
+  /*
+   * Measure the relative sensor orientation.
+   */
+  imu::Vector<3> m = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
+  int angle = (int((atan2(m.z(), m.x())*180)/PI + 450)) % 360;
+  if(debug) {
+    Serial.print("Angle: ");
+    Serial.println(angle);
+  }
+
+  /*
+   * Find the two nearest channels.
+   */
+  int first_channel = 0;
+  int second_channel = 3;
+  for(int i = 0; i < channelCount; i++) {
+    if(angle < thresholds[i]) {
+      first_channel = (i-1) % 4;
+      second_channel = i;
+      break;
+    }
+  }
+
+  /*
+   * Adjust channel intensity according to angle distance.
+   */
+  int first_intensity = adjust_intensity(first_channel, angle);
+  int second_intensity = adjust_intensity(second_channel, angle);
+
+  if(debug) {
+    Serial.print("Playing intensity ");
+    Serial.print(first_intensity);
+    Serial.print(" on channel ");
+    Serial.print(first_channel);
+    Serial.print(" and intensity ");
+    Serial.print(second_intensity);
+    Serial.print(" on channel ");
+    Serial.print(second_channel);
+    Serial.print(" for ");
+    Serial.print(DURATION);
+    Serial.println(" ms");
+  }
+
+  /*
+   * Start vibration on the two nearest channels with the adjusted
+   * intensity.
+   */
+  drvs[first_channel].setRealtimeValue(first_intensity);
+  drvs[second_channel].setRealtimeValue(second_intensity);
+  delay(DURATION);
+  drvs[first_channel].setRealtimeValue(0x00);
+  drvs[second_channel].setRealtimeValue(0x00);
+
+  delay(500);
+}
+
+int adjust_intensity(int channel, int angle) {
+  return (1 - abs((thresholds[channel] - angle) / 90.0)) * MAX_INTENSITY;
+}
