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