Kristen Fernandez / Mbed 2 deprecated FINAL_PROJECT

Final version of project

Dependencies:   FSR LSM9DS1_Library_cal USBMIDI mbed

Fork of LSM9DS1_Demo_wCal by jim hamblen

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?

UserRevisionLine numberNew 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 }