BRIX5

/*

 * 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;

}