![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Final version of project
Dependencies: FSR LSM9DS1_Library_cal USBMIDI mbed
Fork of LSM9DS1_Demo_wCal by
main.cpp@3:de36d456f684, 2016-04-29 (annotated)
- Committer:
- KrissyHam
- Date:
- Fri Apr 29 13:23:44 2016 +0000
- Revision:
- 3:de36d456f684
- Parent:
- 2:82b2a1e84586
- Child:
- 4:f0cd8d252c08
Removed unused PrintAltitude function
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
4180_1 | 0:e693d5bf0a25 | 1 | #include "mbed.h" |
KrissyHam | 1:a81deeb5ba58 | 2 | #include "USBMIDI.h" |
4180_1 | 0:e693d5bf0a25 | 3 | #include "LSM9DS1.h" |
KrissyHam | 1:a81deeb5ba58 | 4 | #include "math.h" |
KrissyHam | 1:a81deeb5ba58 | 5 | #include "FSR.h" |
KrissyHam | 1:a81deeb5ba58 | 6 | |
4180_1 | 0:e693d5bf0a25 | 7 | #define PI 3.14159 |
KrissyHam | 1:a81deeb5ba58 | 8 | #define BUFFERSIZE 6 |
KrissyHam | 1:a81deeb5ba58 | 9 | |
4180_1 | 0:e693d5bf0a25 | 10 | // Earth's magnetic field varies by location. Add or subtract |
4180_1 | 0:e693d5bf0a25 | 11 | // a declination to get a more accurate heading. Calculate |
4180_1 | 0:e693d5bf0a25 | 12 | // your's here: |
4180_1 | 0:e693d5bf0a25 | 13 | // http://www.ngdc.noaa.gov/geomag-web/#declination |
4180_1 | 0:e693d5bf0a25 | 14 | #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA. |
4180_1 | 0:e693d5bf0a25 | 15 | |
KrissyHam | 1:a81deeb5ba58 | 16 | // FSR |
KrissyHam | 1:a81deeb5ba58 | 17 | FSR fsr_kick(p20, 10); // Pin 20 is used as the AnalogIn pin and a 10k resistor is used as a voltage divider |
KrissyHam | 1:a81deeb5ba58 | 18 | FSR fsr_hh(p19, 10); // Pin 19 is used as the AnalogIn pin and a 10k resistor is used as a voltage divider |
KrissyHam | 1:a81deeb5ba58 | 19 | bool hh_close = false; // boolean to determine if hi-hat is closed or open |
KrissyHam | 1:a81deeb5ba58 | 20 | bool kicked = false; |
KrissyHam | 1:a81deeb5ba58 | 21 | |
KrissyHam | 1:a81deeb5ba58 | 22 | // IMU |
KrissyHam | 1:a81deeb5ba58 | 23 | LSM9DS1 IMU(p9, p10, 0xD6, 0x3C); |
KrissyHam | 1:a81deeb5ba58 | 24 | LSM9DS1 IMU2(p28, p27, 0xD6, 0x3C); |
KrissyHam | 1:a81deeb5ba58 | 25 | |
KrissyHam | 2:82b2a1e84586 | 26 | USBMIDI midi; |
KrissyHam | 2:82b2a1e84586 | 27 | |
4180_1 | 0:e693d5bf0a25 | 28 | Serial pc(USBTX, USBRX); |
KrissyHam | 1:a81deeb5ba58 | 29 | DigitalOut led1(LED1); |
KrissyHam | 1:a81deeb5ba58 | 30 | DigitalOut led2(LED2); |
KrissyHam | 1:a81deeb5ba58 | 31 | DigitalOut led3(LED3); |
KrissyHam | 1:a81deeb5ba58 | 32 | DigitalOut led4(LED4); |
KrissyHam | 1:a81deeb5ba58 | 33 | |
KrissyHam | 1:a81deeb5ba58 | 34 | Timer t; |
KrissyHam | 1:a81deeb5ba58 | 35 | float t_prev = 0; |
KrissyHam | 1:a81deeb5ba58 | 36 | float t_prev2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 37 | float t_curr = 0; |
KrissyHam | 1:a81deeb5ba58 | 38 | float t_curr2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 39 | float t_gyroPrev = 0; |
KrissyHam | 1:a81deeb5ba58 | 40 | float t_gyroPrev2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 41 | float t_gyroCurr = 0; |
KrissyHam | 1:a81deeb5ba58 | 42 | float t_gyroCurr2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 43 | float delta_t = 0; |
KrissyHam | 1:a81deeb5ba58 | 44 | |
KrissyHam | 1:a81deeb5ba58 | 45 | float y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 46 | float y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 47 | |
KrissyHam | 1:a81deeb5ba58 | 48 | |
KrissyHam | 1:a81deeb5ba58 | 49 | int resetIndex = BUFFERSIZE - 2; |
KrissyHam | 1:a81deeb5ba58 | 50 | float average[BUFFERSIZE] = {0}; |
KrissyHam | 1:a81deeb5ba58 | 51 | int avg_index = 0; |
KrissyHam | 1:a81deeb5ba58 | 52 | float total = 0; |
KrissyHam | 2:82b2a1e84586 | 53 | float avg_thresh = 20; |
KrissyHam | 1:a81deeb5ba58 | 54 | |
KrissyHam | 1:a81deeb5ba58 | 55 | float average2[BUFFERSIZE] = {0}; |
KrissyHam | 1:a81deeb5ba58 | 56 | int avg_index2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 57 | float total2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 58 | float avg_thresh2; |
KrissyHam | 1:a81deeb5ba58 | 59 | |
KrissyHam | 1:a81deeb5ba58 | 60 | float prev_y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 61 | float curr_y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 62 | float y_accel_threshold = 0.8; |
KrissyHam | 1:a81deeb5ba58 | 63 | bool check_y_accel = false; |
KrissyHam | 1:a81deeb5ba58 | 64 | float t_prev_y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 65 | |
KrissyHam | 1:a81deeb5ba58 | 66 | float prev_y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 67 | float curr_y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 68 | bool check_y_accel2 = false; |
KrissyHam | 1:a81deeb5ba58 | 69 | float t_prev_y_accel2 = 0; |
KrissyHam | 2:82b2a1e84586 | 70 | |
KrissyHam | 1:a81deeb5ba58 | 71 | int count2 = 0; |
KrissyHam | 2:82b2a1e84586 | 72 | int count = 0; |
KrissyHam | 2:82b2a1e84586 | 73 | |
KrissyHam | 2:82b2a1e84586 | 74 | bool detectHit = 0; |
KrissyHam | 2:82b2a1e84586 | 75 | bool detectHit2 = 0; |
KrissyHam | 2:82b2a1e84586 | 76 | bool detectUp = 0; |
KrissyHam | 2:82b2a1e84586 | 77 | bool detectUp2 = 0; |
KrissyHam | 2:82b2a1e84586 | 78 | |
KrissyHam | 2:82b2a1e84586 | 79 | float runningAvg = 0; |
KrissyHam | 2:82b2a1e84586 | 80 | float runningAvg2 = 0; |
KrissyHam | 2:82b2a1e84586 | 81 | |
KrissyHam | 2:82b2a1e84586 | 82 | float interval = 0.20; |
KrissyHam | 2:82b2a1e84586 | 83 | float hit_volume = 0; |
KrissyHam | 2:82b2a1e84586 | 84 | float hit_volume2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 85 | |
KrissyHam | 1:a81deeb5ba58 | 86 | |
KrissyHam | 1:a81deeb5ba58 | 87 | enum StateType {FRONT, SIDE, HIT}; |
KrissyHam | 1:a81deeb5ba58 | 88 | enum StateType2 {FRONT2, SIDE2, HIT2}; |
KrissyHam | 1:a81deeb5ba58 | 89 | |
KrissyHam | 1:a81deeb5ba58 | 90 | StateType state = FRONT; // Initial state is FRONT |
KrissyHam | 1:a81deeb5ba58 | 91 | StateType2 state2 = FRONT2; |
KrissyHam | 1:a81deeb5ba58 | 92 | |
4180_1 | 0:e693d5bf0a25 | 93 | |
KrissyHam | 1:a81deeb5ba58 | 94 | void show_message(MIDIMessage msg) { |
KrissyHam | 1:a81deeb5ba58 | 95 | switch (msg.type()) { |
KrissyHam | 1:a81deeb5ba58 | 96 | case MIDIMessage::NoteOnType: |
KrissyHam | 1:a81deeb5ba58 | 97 | printf("NoteOn key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel()); |
KrissyHam | 1:a81deeb5ba58 | 98 | break; |
KrissyHam | 1:a81deeb5ba58 | 99 | case MIDIMessage::NoteOffType: |
KrissyHam | 1:a81deeb5ba58 | 100 | printf("NoteOff key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel()); |
KrissyHam | 1:a81deeb5ba58 | 101 | break; |
KrissyHam | 1:a81deeb5ba58 | 102 | case MIDIMessage::ControlChangeType: |
KrissyHam | 1:a81deeb5ba58 | 103 | printf("ControlChange controller: %d, data: %d\n", msg.controller(), msg.value()); |
KrissyHam | 1:a81deeb5ba58 | 104 | break; |
KrissyHam | 1:a81deeb5ba58 | 105 | case MIDIMessage::PitchWheelType: |
KrissyHam | 1:a81deeb5ba58 | 106 | printf("PitchWheel channel: %d, pitch: %d\n", msg.channel(), msg.pitch()); |
KrissyHam | 1:a81deeb5ba58 | 107 | break; |
KrissyHam | 1:a81deeb5ba58 | 108 | default: |
KrissyHam | 1:a81deeb5ba58 | 109 | printf("Another message\n"); |
KrissyHam | 1:a81deeb5ba58 | 110 | } |
KrissyHam | 1:a81deeb5ba58 | 111 | } |
4180_1 | 0:e693d5bf0a25 | 112 | |
4180_1 | 0:e693d5bf0a25 | 113 | |
4180_1 | 0:e693d5bf0a25 | 114 | int main() |
4180_1 | 0:e693d5bf0a25 | 115 | { |
KrissyHam | 1:a81deeb5ba58 | 116 | |
KrissyHam | 2:82b2a1e84586 | 117 | midi.attach(show_message); // call back for messages received |
KrissyHam | 1:a81deeb5ba58 | 118 | pc.baud(9600); |
KrissyHam | 1:a81deeb5ba58 | 119 | pc.printf("Hello world!\n"); |
4180_1 | 0:e693d5bf0a25 | 120 | IMU.begin(); |
4180_1 | 0:e693d5bf0a25 | 121 | if (!IMU.begin()) { |
KrissyHam | 1:a81deeb5ba58 | 122 | pc.printf("Failed to communicate with LSM9DS1 - first.\n"); |
4180_1 | 0:e693d5bf0a25 | 123 | } |
4180_1 | 0:e693d5bf0a25 | 124 | IMU.calibrate(1); |
KrissyHam | 1:a81deeb5ba58 | 125 | |
KrissyHam | 1:a81deeb5ba58 | 126 | IMU2.begin(); |
KrissyHam | 1:a81deeb5ba58 | 127 | if (!IMU2.begin()) { |
KrissyHam | 1:a81deeb5ba58 | 128 | pc.printf("Failed to communicate with LSM9DS1 - second.\n"); |
KrissyHam | 1:a81deeb5ba58 | 129 | } |
KrissyHam | 1:a81deeb5ba58 | 130 | IMU2.calibrate(1); |
KrissyHam | 1:a81deeb5ba58 | 131 | |
KrissyHam | 1:a81deeb5ba58 | 132 | t.start(); |
KrissyHam | 1:a81deeb5ba58 | 133 | |
4180_1 | 0:e693d5bf0a25 | 134 | while(1) { |
KrissyHam | 1:a81deeb5ba58 | 135 | |
KrissyHam | 2:82b2a1e84586 | 136 | // Initialize acceleration and gyroscope data for both IMUs |
4180_1 | 0:e693d5bf0a25 | 137 | while(!IMU.accelAvailable()); |
4180_1 | 0:e693d5bf0a25 | 138 | IMU.readAccel(); |
4180_1 | 0:e693d5bf0a25 | 139 | while(!IMU.gyroAvailable()); |
4180_1 | 0:e693d5bf0a25 | 140 | IMU.readGyro(); |
KrissyHam | 1:a81deeb5ba58 | 141 | |
KrissyHam | 1:a81deeb5ba58 | 142 | while(!IMU2.accelAvailable()); |
KrissyHam | 1:a81deeb5ba58 | 143 | IMU2.readAccel(); |
KrissyHam | 1:a81deeb5ba58 | 144 | while(!IMU2.gyroAvailable()); |
KrissyHam | 1:a81deeb5ba58 | 145 | IMU2.readGyro(); |
KrissyHam | 1:a81deeb5ba58 | 146 | |
KrissyHam | 2:82b2a1e84586 | 147 | // Create variables for ease of use for acceleration |
KrissyHam | 1:a81deeb5ba58 | 148 | y_accel = IMU.calcAccel(IMU.ay); |
KrissyHam | 1:a81deeb5ba58 | 149 | y_accel2 = IMU2.calcAccel(IMU2.ay); |
KrissyHam | 1:a81deeb5ba58 | 150 | |
KrissyHam | 2:82b2a1e84586 | 151 | // Initialize timer |
KrissyHam | 1:a81deeb5ba58 | 152 | t_curr = t.read(); |
KrissyHam | 1:a81deeb5ba58 | 153 | t_curr2 = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 154 | |
KrissyHam | 1:a81deeb5ba58 | 155 | |
KrissyHam | 2:82b2a1e84586 | 156 | /** |
KrissyHam | 2:82b2a1e84586 | 157 | * FSR detection |
KrissyHam | 2:82b2a1e84586 | 158 | */ |
KrissyHam | 2:82b2a1e84586 | 159 | |
KrissyHam | 2:82b2a1e84586 | 160 | if (fsr_kick.readRaw() > 0.3){ |
KrissyHam | 2:82b2a1e84586 | 161 | if (kicked == false){ |
KrissyHam | 2:82b2a1e84586 | 162 | midi.write(MIDIMessage::NoteOn(45, fsr_kick.readRaw() * 127 + 30, 10)); // Play a kick, map the volume and boost the amplitude, channel 10 |
KrissyHam | 2:82b2a1e84586 | 163 | } |
KrissyHam | 2:82b2a1e84586 | 164 | kicked = true; |
KrissyHam | 2:82b2a1e84586 | 165 | } |
KrissyHam | 2:82b2a1e84586 | 166 | else {kicked = false;} |
KrissyHam | 2:82b2a1e84586 | 167 | |
KrissyHam | 2:82b2a1e84586 | 168 | if (fsr_hh.readRaw() > 0.3){ |
KrissyHam | 2:82b2a1e84586 | 169 | if (hh_close == false){ |
KrissyHam | 2:82b2a1e84586 | 170 | midi.write(MIDIMessage::NoteOn(42, fsr_hh.readRaw()*127, 10)); // Play a hi-hat pedal, map the volume, channel 10 |
KrissyHam | 2:82b2a1e84586 | 171 | } |
KrissyHam | 2:82b2a1e84586 | 172 | hh_close = true; |
KrissyHam | 2:82b2a1e84586 | 173 | } |
KrissyHam | 2:82b2a1e84586 | 174 | else {hh_close = false;} |
KrissyHam | 2:82b2a1e84586 | 175 | |
KrissyHam | 1:a81deeb5ba58 | 176 | |
KrissyHam | 1:a81deeb5ba58 | 177 | /** |
KrissyHam | 2:82b2a1e84586 | 178 | * Running average for hit detection |
KrissyHam | 1:a81deeb5ba58 | 179 | */ |
KrissyHam | 1:a81deeb5ba58 | 180 | |
KrissyHam | 1:a81deeb5ba58 | 181 | // First IMU |
KrissyHam | 2:82b2a1e84586 | 182 | |
KrissyHam | 1:a81deeb5ba58 | 183 | total -= average[avg_index]; |
KrissyHam | 1:a81deeb5ba58 | 184 | average[avg_index] = IMU.calcGyro(IMU.gy); |
KrissyHam | 1:a81deeb5ba58 | 185 | total += average[avg_index]; |
KrissyHam | 1:a81deeb5ba58 | 186 | if (avg_index > resetIndex) { |
KrissyHam | 1:a81deeb5ba58 | 187 | avg_index = 0; |
KrissyHam | 1:a81deeb5ba58 | 188 | } else { |
KrissyHam | 1:a81deeb5ba58 | 189 | avg_index++; |
KrissyHam | 1:a81deeb5ba58 | 190 | } |
KrissyHam | 1:a81deeb5ba58 | 191 | |
KrissyHam | 1:a81deeb5ba58 | 192 | // Second IMU |
KrissyHam | 1:a81deeb5ba58 | 193 | total2 -= average2[avg_index2]; |
KrissyHam | 1:a81deeb5ba58 | 194 | average2[avg_index2] = IMU2.calcGyro(IMU2.gy); |
KrissyHam | 1:a81deeb5ba58 | 195 | total2 += average2[avg_index2]; |
KrissyHam | 1:a81deeb5ba58 | 196 | if (avg_index2 > resetIndex) { |
KrissyHam | 1:a81deeb5ba58 | 197 | avg_index2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 198 | } else { |
KrissyHam | 1:a81deeb5ba58 | 199 | avg_index2++; |
KrissyHam | 1:a81deeb5ba58 | 200 | } |
KrissyHam | 1:a81deeb5ba58 | 201 | |
KrissyHam | 1:a81deeb5ba58 | 202 | /** |
KrissyHam | 2:82b2a1e84586 | 203 | * Detect downward hit |
KrissyHam | 1:a81deeb5ba58 | 204 | */ |
KrissyHam | 1:a81deeb5ba58 | 205 | if (IMU.calcGyro(IMU.gy) > 35) { |
KrissyHam | 1:a81deeb5ba58 | 206 | detectHit = 1; |
KrissyHam | 1:a81deeb5ba58 | 207 | } |
KrissyHam | 1:a81deeb5ba58 | 208 | |
KrissyHam | 1:a81deeb5ba58 | 209 | if (IMU2.calcGyro(IMU2.gy) > 35) { |
KrissyHam | 1:a81deeb5ba58 | 210 | detectHit2 = 1; |
KrissyHam | 1:a81deeb5ba58 | 211 | } |
KrissyHam | 1:a81deeb5ba58 | 212 | |
KrissyHam | 2:82b2a1e84586 | 213 | |
KrissyHam | 1:a81deeb5ba58 | 214 | /** |
KrissyHam | 1:a81deeb5ba58 | 215 | * Check all conditions for hit |
KrissyHam | 1:a81deeb5ba58 | 216 | */ |
KrissyHam | 1:a81deeb5ba58 | 217 | |
KrissyHam | 1:a81deeb5ba58 | 218 | // Map gyroscope value ranges to volume ranges |
KrissyHam | 1:a81deeb5ba58 | 219 | hit_volume = (runningAvg + 245) * (127) / (490); |
KrissyHam | 1:a81deeb5ba58 | 220 | hit_volume2 = (runningAvg2 + 245) * (127) / (490) + 15; |
KrissyHam | 1:a81deeb5ba58 | 221 | |
KrissyHam | 1:a81deeb5ba58 | 222 | // First IMU |
KrissyHam | 1:a81deeb5ba58 | 223 | detectUp = IMU.calcGyro(IMU.gy) <= 0; |
KrissyHam | 1:a81deeb5ba58 | 224 | runningAvg = total / BUFFERSIZE; |
KrissyHam | 2:82b2a1e84586 | 225 | |
KrissyHam | 2:82b2a1e84586 | 226 | // Check if drumstick is brought down and then brought back up (eliminates continous hit detection if drumstick is just held tilted down) |
KrissyHam | 2:82b2a1e84586 | 227 | // Then check if running average is greater than a threshold (to elimate noise) |
KrissyHam | 2:82b2a1e84586 | 228 | // Elimate debouncing by only allowing a hit to play if the time interval has passed |
KrissyHam | 2:82b2a1e84586 | 229 | if (detectHit && detectUp && (runningAvg > avg_thresh) && (t_curr - t_prev) > interval) { |
KrissyHam | 2:82b2a1e84586 | 230 | // Depending on the state, play the corresponding instrument |
KrissyHam | 1:a81deeb5ba58 | 231 | switch (state) { |
KrissyHam | 1:a81deeb5ba58 | 232 | case (FRONT): |
KrissyHam | 2:82b2a1e84586 | 233 | midi.write(MIDIMessage::NoteOn(46, hit_volume, 10)); // Play ride sound |
KrissyHam | 1:a81deeb5ba58 | 234 | break; |
KrissyHam | 1:a81deeb5ba58 | 235 | case (SIDE): |
KrissyHam | 1:a81deeb5ba58 | 236 | if (hh_close) { |
KrissyHam | 2:82b2a1e84586 | 237 | midi.write(MIDIMessage::NoteOn(40, hit_volume, 10)); // Play closed hi-hat sound |
KrissyHam | 1:a81deeb5ba58 | 238 | } else { |
KrissyHam | 2:82b2a1e84586 | 239 | midi.write(MIDIMessage::NoteOn(41, hit_volume, 10)); // Play open hi-hat sound |
KrissyHam | 1:a81deeb5ba58 | 240 | } |
KrissyHam | 1:a81deeb5ba58 | 241 | break; |
KrissyHam | 1:a81deeb5ba58 | 242 | } |
KrissyHam | 1:a81deeb5ba58 | 243 | detectHit = 0; |
KrissyHam | 1:a81deeb5ba58 | 244 | t_prev = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 245 | count = 0; |
KrissyHam | 1:a81deeb5ba58 | 246 | } |
KrissyHam | 1:a81deeb5ba58 | 247 | |
KrissyHam | 1:a81deeb5ba58 | 248 | // Second IMU |
KrissyHam | 1:a81deeb5ba58 | 249 | detectUp2 = IMU2.calcGyro(IMU2.gy) <= 0; |
KrissyHam | 1:a81deeb5ba58 | 250 | runningAvg2 = total2 / BUFFERSIZE; |
KrissyHam | 1:a81deeb5ba58 | 251 | |
KrissyHam | 1:a81deeb5ba58 | 252 | if (detectHit2 && detectUp2 && runningAvg2 > avg_thresh2 && (t_curr2 - t_prev2) > interval) { |
KrissyHam | 1:a81deeb5ba58 | 253 | switch (state2) { |
KrissyHam | 1:a81deeb5ba58 | 254 | case (FRONT2): |
KrissyHam | 2:82b2a1e84586 | 255 | midi.write(MIDIMessage::NoteOn(47, hit_volume2, 10)); // Play snare sound |
KrissyHam | 1:a81deeb5ba58 | 256 | break; |
KrissyHam | 1:a81deeb5ba58 | 257 | case (SIDE2): |
KrissyHam | 2:82b2a1e84586 | 258 | midi.write(MIDIMessage::NoteOn(51, hit_volume2, 10)); // Play clap sound |
KrissyHam | 1:a81deeb5ba58 | 259 | break; |
KrissyHam | 1:a81deeb5ba58 | 260 | } |
KrissyHam | 1:a81deeb5ba58 | 261 | detectHit2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 262 | t_prev2 = t_curr2; |
KrissyHam | 1:a81deeb5ba58 | 263 | } |
KrissyHam | 1:a81deeb5ba58 | 264 | |
KrissyHam | 1:a81deeb5ba58 | 265 | |
KrissyHam | 1:a81deeb5ba58 | 266 | /** |
KrissyHam | 2:82b2a1e84586 | 267 | * Instrument switching detection |
KrissyHam | 1:a81deeb5ba58 | 268 | */ |
KrissyHam | 1:a81deeb5ba58 | 269 | |
KrissyHam | 1:a81deeb5ba58 | 270 | curr_y_accel = y_accel; |
KrissyHam | 1:a81deeb5ba58 | 271 | curr_y_accel2 = y_accel2; |
KrissyHam | 1:a81deeb5ba58 | 272 | |
KrissyHam | 1:a81deeb5ba58 | 273 | check_y_accel = abs(curr_y_accel - prev_y_accel) > y_accel_threshold; |
KrissyHam | 1:a81deeb5ba58 | 274 | check_y_accel2 = abs(curr_y_accel2 - prev_y_accel2) > y_accel_threshold; |
KrissyHam | 1:a81deeb5ba58 | 275 | |
KrissyHam | 2:82b2a1e84586 | 276 | // Check that y accleration is above threshold; if it is, increase the count for the number of cycles it is above this threshold |
KrissyHam | 1:a81deeb5ba58 | 277 | if (check_y_accel) { |
KrissyHam | 1:a81deeb5ba58 | 278 | count++; |
KrissyHam | 1:a81deeb5ba58 | 279 | } |
KrissyHam | 1:a81deeb5ba58 | 280 | |
KrissyHam | 1:a81deeb5ba58 | 281 | if (check_y_accel2) { |
KrissyHam | 1:a81deeb5ba58 | 282 | count2++; |
KrissyHam | 1:a81deeb5ba58 | 283 | } |
KrissyHam | 1:a81deeb5ba58 | 284 | |
KrissyHam | 1:a81deeb5ba58 | 285 | // First IMU |
KrissyHam | 1:a81deeb5ba58 | 286 | switch (state) { |
KrissyHam | 1:a81deeb5ba58 | 287 | case (FRONT): |
KrissyHam | 2:82b2a1e84586 | 288 | // Check that y_accleration is above the threshold for at least 3 cycles |
KrissyHam | 2:82b2a1e84586 | 289 | // Elimnate debouncing by only switching if a time interval has passed |
KrissyHam | 1:a81deeb5ba58 | 290 | if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) { |
KrissyHam | 1:a81deeb5ba58 | 291 | count = 0; |
KrissyHam | 1:a81deeb5ba58 | 292 | state = SIDE; |
KrissyHam | 1:a81deeb5ba58 | 293 | led1 = 1; |
KrissyHam | 1:a81deeb5ba58 | 294 | led2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 295 | t_prev_y_accel = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 296 | } |
KrissyHam | 1:a81deeb5ba58 | 297 | break; |
KrissyHam | 1:a81deeb5ba58 | 298 | case (SIDE): |
KrissyHam | 1:a81deeb5ba58 | 299 | if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) { |
KrissyHam | 1:a81deeb5ba58 | 300 | count = 0; |
KrissyHam | 1:a81deeb5ba58 | 301 | state = FRONT; |
KrissyHam | 1:a81deeb5ba58 | 302 | led1 = 0; |
KrissyHam | 1:a81deeb5ba58 | 303 | led2 = 1; |
KrissyHam | 1:a81deeb5ba58 | 304 | t_prev_y_accel = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 305 | } |
KrissyHam | 1:a81deeb5ba58 | 306 | break; |
KrissyHam | 1:a81deeb5ba58 | 307 | } |
KrissyHam | 1:a81deeb5ba58 | 308 | prev_y_accel = curr_y_accel; |
KrissyHam | 1:a81deeb5ba58 | 309 | |
KrissyHam | 1:a81deeb5ba58 | 310 | //Second IMU |
KrissyHam | 1:a81deeb5ba58 | 311 | switch (state2) { |
KrissyHam | 1:a81deeb5ba58 | 312 | case (FRONT2): |
KrissyHam | 1:a81deeb5ba58 | 313 | if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){ |
KrissyHam | 1:a81deeb5ba58 | 314 | state2 = SIDE2; |
KrissyHam | 1:a81deeb5ba58 | 315 | count2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 316 | led4 = 1; |
KrissyHam | 1:a81deeb5ba58 | 317 | led3 = 0; |
KrissyHam | 1:a81deeb5ba58 | 318 | t_prev_y_accel2 = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 319 | } |
KrissyHam | 1:a81deeb5ba58 | 320 | break; |
KrissyHam | 1:a81deeb5ba58 | 321 | case (SIDE2): |
KrissyHam | 1:a81deeb5ba58 | 322 | if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){ |
KrissyHam | 1:a81deeb5ba58 | 323 | state2 = FRONT2; |
KrissyHam | 1:a81deeb5ba58 | 324 | count2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 325 | led4 = 0; |
KrissyHam | 1:a81deeb5ba58 | 326 | led3 = 1; |
KrissyHam | 1:a81deeb5ba58 | 327 | t_prev_y_accel2 = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 328 | } |
KrissyHam | 1:a81deeb5ba58 | 329 | break; |
KrissyHam | 1:a81deeb5ba58 | 330 | } |
KrissyHam | 1:a81deeb5ba58 | 331 | prev_y_accel2 = curr_y_accel2; |
4180_1 | 0:e693d5bf0a25 | 332 | } |
KrissyHam | 2:82b2a1e84586 | 333 | } |