BRIX5

/***************************************************

  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();

}

/***************************************************

  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;

};

/*

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

}