Statistics
| Branch: | Revision:

brix5 / firmware / demo / QuadDRV-CompassBelt / QuadDRV-CompassBelt.ino @ a2a2cf8b

History | View | Annotate | Download (3.75 KB)

1 a2a2cf8b Jan
/*
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
}