Revision a2a2cf8b

View differences:

firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.cpp
1
/***************************************************
2
  This is a library for the Adafruit DRV2605L Haptic Driver
3

  
4
  ----> http://www.adafruit.com/products/2306
5

  
6
  Check out the links above for our tutorials and wiring diagrams
7
  This motor/haptic driver uses I2C to communicate
8

  
9
  Adafruit invests time and resources providing this open source code,
10
  please support Adafruit and open-source hardware by purchasing
11
  products from Adafruit!
12

  
13
  Written by Limor Fried/Ladyada for Adafruit Industries.
14
  MIT license, all text above must be included in any redistribution
15
 ****************************************************/
16

  
17

  
18
#if ARDUINO >= 100
19
#include "Arduino.h"
20
#else
21
#include "WProgram.h"
22
#endif
23

  
24
#include "Adafruit_DRV2605.h"
25

  
26
/**************************************************************************/
27
/*!
28
    @brief  Instantiates a new DRV2605 class
29
*/
30
/**************************************************************************/
31
// I2C, no address adjustments or pins
32
Adafruit_DRV2605::Adafruit_DRV2605(uint8_t channel) {
33
  _channel = channel;
34
}
35

  
36

  
37
/**************************************************************************/
38
/*!
39
    @brief  Setups the HW
40
*/
41
/**************************************************************************/
42
boolean Adafruit_DRV2605::begin() {
43
  uint8_t id = readRegister8(DRV2605_REG_STATUS);
44
  //Serial.print("Status 0x"); Serial.println(id, HEX);
45

  
46
  writeRegister8(DRV2605_REG_MODE, 0x00); // out of standby
47

  
48
  writeRegister8(DRV2605_REG_RTPIN, 0x00); // no real-time-playback
49

  
50
  writeRegister8(DRV2605_REG_WAVESEQ1, 1); // strong click
51
  writeRegister8(DRV2605_REG_WAVESEQ2, 0);
52

  
53
  writeRegister8(DRV2605_REG_OVERDRIVE, 0); // no overdrive
54

  
55
  writeRegister8(DRV2605_REG_SUSTAINPOS, 0);
56
  writeRegister8(DRV2605_REG_SUSTAINNEG, 0);
57
  writeRegister8(DRV2605_REG_BREAK, 0);
58
  writeRegister8(DRV2605_REG_AUDIOMAX, 0x64);
59

  
60
  // ERM open loop
61

  
62
  // turn off N_ERM_LRA
63
  writeRegister8(DRV2605_REG_FEEDBACK, readRegister8(DRV2605_REG_FEEDBACK) & 0x7F);
64
  // turn on ERM_OPEN_LOOP
65
  writeRegister8(DRV2605_REG_CONTROL3, readRegister8(DRV2605_REG_CONTROL3) | 0x20);
66

  
67
  return true;
68
}
69

  
70
void Adafruit_DRV2605::setWaveform(uint8_t slot, uint8_t w) {
71
  writeRegister8(DRV2605_REG_WAVESEQ1 + slot, w);
72
}
73

  
74
void Adafruit_DRV2605::selectLibrary(uint8_t lib) {
75
  writeRegister8(DRV2605_REG_LIBRARY, lib);
76
}
77

  
78
void Adafruit_DRV2605::go() {
79
  writeRegister8(DRV2605_REG_GO, 1);
80
}
81

  
82
void Adafruit_DRV2605::setMode(uint8_t mode) {
83
  writeRegister8(DRV2605_REG_MODE, mode);
84
}
85

  
86
void Adafruit_DRV2605::setRealtimeValue(uint8_t rtp) {
87
  writeRegister8(DRV2605_REG_RTPIN, rtp);
88
}
89

  
90
/********************************************************************/
91

  
92
uint8_t Adafruit_DRV2605::readRegister8(uint8_t reg) {
93
  selectChannel(_channel);
94
  uint8_t x ;
95
  // use i2c
96
  Wire.beginTransmission(DRV2605_ADDR);
97
  Wire.write((byte)reg);
98
  Wire.endTransmission();
99
  Wire.requestFrom((byte)DRV2605_ADDR, (byte)1);
100
  x = Wire.read();
101
  //  Serial.print("$"); Serial.print(reg, HEX);
102
  //  Serial.print(": 0x"); Serial.println(x, HEX);
103
  return x;
104
}
105

  
106
void Adafruit_DRV2605::writeRegister8(uint8_t reg, uint8_t val) {
107
  selectChannel(_channel);
108
  // use i2c
109
  Wire.beginTransmission(DRV2605_ADDR);
110
  Wire.write((byte)reg);
111
  Wire.write((byte)val);
112
  Wire.endTransmission();
113
}
114

  
115
/****************/
116

  
117

  
118
// Allow users to use ERM motor or LRA motors
119

  
120
void Adafruit_DRV2605::useERM ()
121
{ 
122
  writeRegister8(DRV2605_REG_FEEDBACK, readRegister8(DRV2605_REG_FEEDBACK) & 0x7F);
123
}
124

  
125
void Adafruit_DRV2605::useLRA ()
126
{
127
  writeRegister8(DRV2605_REG_FEEDBACK, readRegister8(DRV2605_REG_FEEDBACK) | 0x80);
128
}
129

  
130
void Adafruit_DRV2605::selectChannel (uint8_t channel)
131
{
132
  // choose channel, all address bits to gnd on PCA9546A/TCA9548A = 0x70
133
  Wire.beginTransmission(0x77);
134
  Wire.write(1 << channel);
135
  Wire.endTransmission();
136
}
firmware/demo/QuadDRV-CompassBelt/Adafruit_DRV2605.h
1
/***************************************************
2
  This is a library for the Adafruit DRV2605L Haptic Driver
3

  
4
  ----> http://www.adafruit.com/products/2305
5

  
6
  Check out the links above for our tutorials and wiring diagrams
7
  This motor/haptic driver uses I2C to communicate
8

  
9
  Adafruit invests time and resources providing this open source code,
10
  please support Adafruit and open-source hardware by purchasing
11
  products from Adafruit!
12

  
13
  Written by Limor Fried/Ladyada for Adafruit Industries.
14
  MIT license, all text above must be included in any redistribution
15
 ****************************************************/
16

  
17

  
18
#if ARDUINO >= 100
19
#include "Arduino.h"
20
#else
21
#include "WProgram.h"
22
#endif
23

  
24
#if defined(__MK20DX128__) || defined(__MK20DX256__)
25
#include "i2c_t3.h"
26
#else
27
#include "Wire.h"
28
#endif
29

  
30

  
31

  
32
#define DRV2605_ADDR 0x5A
33

  
34
#define DRV2605_REG_STATUS 0x00
35
#define DRV2605_REG_MODE 0x01
36
#define DRV2605_MODE_INTTRIG  0x00
37
#define DRV2605_MODE_EXTTRIGEDGE  0x01
38
#define DRV2605_MODE_EXTTRIGLVL  0x02
39
#define DRV2605_MODE_PWMANALOG  0x03
40
#define DRV2605_MODE_AUDIOVIBE  0x04
41
#define DRV2605_MODE_REALTIME  0x05
42
#define DRV2605_MODE_DIAGNOS  0x06
43
#define DRV2605_MODE_AUTOCAL  0x07
44

  
45

  
46
#define DRV2605_REG_RTPIN 0x02
47
#define DRV2605_REG_LIBRARY 0x03
48
#define DRV2605_REG_WAVESEQ1 0x04
49
#define DRV2605_REG_WAVESEQ2 0x05
50
#define DRV2605_REG_WAVESEQ3 0x06
51
#define DRV2605_REG_WAVESEQ4 0x07
52
#define DRV2605_REG_WAVESEQ5 0x08
53
#define DRV2605_REG_WAVESEQ6 0x09
54
#define DRV2605_REG_WAVESEQ7 0x0A
55
#define DRV2605_REG_WAVESEQ8 0x0B
56

  
57
#define DRV2605_REG_GO 0x0C
58
#define DRV2605_REG_OVERDRIVE 0x0D
59
#define DRV2605_REG_SUSTAINPOS 0x0E
60
#define DRV2605_REG_SUSTAINNEG 0x0F
61
#define DRV2605_REG_BREAK 0x10
62
#define DRV2605_REG_AUDIOCTRL 0x11
63
#define DRV2605_REG_AUDIOLVL 0x12
64
#define DRV2605_REG_AUDIOMAX 0x13
65
#define DRV2605_REG_RATEDV 0x16
66
#define DRV2605_REG_CLAMPV 0x17
67
#define DRV2605_REG_AUTOCALCOMP 0x18
68
#define DRV2605_REG_AUTOCALEMP 0x19
69
#define DRV2605_REG_FEEDBACK 0x1A
70
#define DRV2605_REG_CONTROL1 0x1B
71
#define DRV2605_REG_CONTROL2 0x1C
72
#define DRV2605_REG_CONTROL3 0x1D
73
#define DRV2605_REG_CONTROL4 0x1E
74
#define DRV2605_REG_VBAT 0x21
75
#define DRV2605_REG_LRARESON 0x22
76

  
77

  
78
class Adafruit_DRV2605 {
79
  public:
80
    Adafruit_DRV2605(uint8_t channel);
81
    boolean begin(void);
82
    void selectChannel(uint8_t channel);
83
    void writeRegister8(uint8_t reg, uint8_t val);
84
    uint8_t readRegister8(uint8_t reg);
85
    void setWaveform(uint8_t slot, uint8_t w);
86
    void selectLibrary(uint8_t lib);
87
    void go(void);
88
    void setMode(uint8_t mode);
89
    void setRealtimeValue(uint8_t rtp);
90
    // Select ERM (Eccentric Rotating Mass) or LRA (Linear Resonant Actuator) vibration motor
91
    // The default is ERM, which is more common
92
    void useERM();
93
    void useLRA();
94

  
95
  private:
96
    uint8_t _channel;
97
};
98

  
firmware/demo/QuadDRV-CompassBelt/QuadDRV-CompassBelt.ino
1
/*
2
 * Compass belt control code. Reads angle from magnetometer and activates
3
 * the two nearest LRAs for a short duration with an intensity
4
 * anti-proportional to the angle distance.
5
 * 
6
 * Operation Overview:
7
 *  - Compute angle between current orientation and north
8
 *  - operate closest pair of LRAs for 300 ms, intensity depends 
9
 *    on angular distance to north
10
 *
11
 * Operation Instructions:
12
 *  - BRIX/ Magnetometer in front
13
 *  - LRAs diagonally at 45, 135, 225, 315 degrees
14
 *  - Turn on Brix 
15
 *  - Calibrate magnetometer by moving around
16
 *  - Put on belt, should be worn firmly on body for good coupling
17
 */
18

  
19
#include <Wire.h>
20
#include "Adafruit_DRV2605.h"
21
#include <Adafruit_Sensor.h>
22
#include <Adafruit_BNO055.h>
23
#include <utility/imumaths.h>
24
#include <math.h>
25

  
26
bool debug = true;
27

  
28
/* ========== MAGNETOMETER ========== */
29

  
30
/*
31
 * Check I2C device address and correct line below.
32
 * By default address is 0x29 or 0x28.
33
 */
34
Adafruit_BNO055 bno = Adafruit_BNO055(-1, 0x28);
35

  
36
/* ============= LRAs =============== */
37

  
38
const uint8_t channelCount = 4; // belt has 3 channels
39
Adafruit_DRV2605 drvs[channelCount] = {
40
  Adafruit_DRV2605(0),  
41
  Adafruit_DRV2605(1),
42
  Adafruit_DRV2605(2),
43
  Adafruit_DRV2605(3)
44
};
45

  
46
const int thresholds[channelCount] = {45, 135, 225, 315};
47
const int MAX_INTENSITY = 127;
48
const int DURATION = 300;
49

  
50
/* ================================== */
51

  
52
void setup() {
53
  Serial.begin(9600);
54
  delay(500);
55

  
56
  Serial.println("Booting");
57
  /*
58
   * Sensor setup.
59
   */
60
  if(!bno.begin()) {
61
    /* There was a problem detecting the BNO055 ... check your connections */
62
    Serial.print("Ooops, no BNO055 detected ... Check your wiring or I2C ADDR!");
63
    while(1);
64
  }
65
  bno.setExtCrystalUse(true);
66

  
67
  /*
68
   * LRA setup.
69
   */
70
  Wire.begin();
71
  
72
  for (uint8_t i = 0; i < channelCount; i++) {
73
    drvs[i].begin();
74
    drvs[i].selectLibrary(6);  // LRA
75
    drvs[i].useLRA();
76
    drvs[i].setMode(DRV2605_MODE_REALTIME);
77
  }
78

  
79
  /*
80
   * Sensor calibration.
81
   */
82
  Serial.println("Calibration status values: 0=uncalibrated, 3=fully calibrated");
83
  uint8_t system, gyro, accel, mag = 0;
84
  while(mag != 3) {
85
    bno.getCalibration(&system, &gyro, &accel, &mag);
86
    Serial.print("CALIBRATION: Mag=");
87
    Serial.println(mag, DEC);
88
  }
89
}
90

  
91
void loop() {
92

  
93
  /*
94
   * Measure the relative sensor orientation.
95
   */
96
  imu::Vector<3> m = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
97
  int angle = (int((atan2(m.z(), m.x())*180)/PI + 450)) % 360;
98
  if(debug) {
99
    Serial.print("Angle: ");
100
    Serial.println(angle);
101
  }
102

  
103
  /*
104
   * Find the two nearest channels.
105
   */
106
  int first_channel = 0;
107
  int second_channel = 3;
108
  for(int i = 0; i < channelCount; i++) {
109
    if(angle < thresholds[i]) {
110
      first_channel = (i-1) % 4;
111
      second_channel = i;
112
      break;
113
    }
114
  }
115

  
116
  /*
117
   * Adjust channel intensity according to angle distance.
118
   */
119
  int first_intensity = adjust_intensity(first_channel, angle);
120
  int second_intensity = adjust_intensity(second_channel, angle);
121

  
122
  if(debug) {
123
    Serial.print("Playing intensity ");
124
    Serial.print(first_intensity);
125
    Serial.print(" on channel ");
126
    Serial.print(first_channel);
127
    Serial.print(" and intensity ");
128
    Serial.print(second_intensity);
129
    Serial.print(" on channel ");
130
    Serial.print(second_channel);
131
    Serial.print(" for ");
132
    Serial.print(DURATION);
133
    Serial.println(" ms");
134
  }
135

  
136
  /*
137
   * Start vibration on the two nearest channels with the adjusted
138
   * intensity.
139
   */
140
  drvs[first_channel].setRealtimeValue(first_intensity);
141
  drvs[second_channel].setRealtimeValue(second_intensity);
142
  delay(DURATION);
143
  drvs[first_channel].setRealtimeValue(0x00);
144
  drvs[second_channel].setRealtimeValue(0x00);
145

  
146
  delay(500);
147
}
148

  
149
int adjust_intensity(int channel, int angle) {
150
  return (1 - abs((thresholds[channel] - angle) / 90.0)) * MAX_INTENSITY;
151
}

Also available in: Unified diff