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@2:82b2a1e84586, 2016-04-29 (annotated)

Committer:
KrissyHam
Date:
Fri Apr 29 13:21:09 2016 +0000
Revision:
2:82b2a1e84586
Parent:
1:a81deeb5ba58
Child:
3:de36d456f684
Final version of project, updated with comments

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 // Calculate pitch, roll, and heading.
4180_1 0:e693d5bf0a25 94 // Pitch/roll calculations taken from this app note:
4180_1 0:e693d5bf0a25 95 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
4180_1 0:e693d5bf0a25 96 // Heading calculations taken from this app note:
4180_1 0:e693d5bf0a25 97 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
KrissyHam 1:a81deeb5ba58 98
KrissyHam 1:a81deeb5ba58 99 void printAltitude(float ax, float ay, float az, float mx, float my, float mz)
4180_1 0:e693d5bf0a25 100 {
4180_1 0:e693d5bf0a25 101 float roll = atan2(ay, az);
4180_1 0:e693d5bf0a25 102 float pitch = atan2(-ax, sqrt(ay * ay + az * az));
4180_1 0:e693d5bf0a25 103 // touchy trig stuff to use arctan to get compass heading (scale is 0..360)
4180_1 0:e693d5bf0a25 104 mx = -mx;
4180_1 0:e693d5bf0a25 105 float heading;
4180_1 0:e693d5bf0a25 106 if (my == 0.0)
4180_1 0:e693d5bf0a25 107 heading = (mx < 0.0) ? 180.0 : 0.0;
4180_1 0:e693d5bf0a25 108 else
4180_1 0:e693d5bf0a25 109 heading = atan2(mx, my)*360.0/(2.0*PI);
KrissyHam 1:a81deeb5ba58 110 pc.printf("heading atan=%f \n\r",heading);
4180_1 0:e693d5bf0a25 111 heading -= DECLINATION; //correct for geo location
4180_1 0:e693d5bf0a25 112 if(heading>180.0) heading = heading - 360.0;
4180_1 0:e693d5bf0a25 113 else if(heading<-180.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 114 else if(heading<0.0) heading = 360.0 + heading;
4180_1 0:e693d5bf0a25 115
4180_1 0:e693d5bf0a25 116
4180_1 0:e693d5bf0a25 117 // Convert everything from radians to degrees:
4180_1 0:e693d5bf0a25 118 //heading *= 180.0 / PI;
4180_1 0:e693d5bf0a25 119 pitch *= 180.0 / PI;
4180_1 0:e693d5bf0a25 120 roll *= 180.0 / PI;
4180_1 0:e693d5bf0a25 121 }
4180_1 0:e693d5bf0a25 122
KrissyHam 1:a81deeb5ba58 123 void show_message(MIDIMessage msg) {
KrissyHam 1:a81deeb5ba58 124 switch (msg.type()) {
KrissyHam 1:a81deeb5ba58 125 case MIDIMessage::NoteOnType:
KrissyHam 1:a81deeb5ba58 126 printf("NoteOn key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel());
KrissyHam 1:a81deeb5ba58 127 break;
KrissyHam 1:a81deeb5ba58 128 case MIDIMessage::NoteOffType:
KrissyHam 1:a81deeb5ba58 129 printf("NoteOff key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel());
KrissyHam 1:a81deeb5ba58 130 break;
KrissyHam 1:a81deeb5ba58 131 case MIDIMessage::ControlChangeType:
KrissyHam 1:a81deeb5ba58 132 printf("ControlChange controller: %d, data: %d\n", msg.controller(), msg.value());
KrissyHam 1:a81deeb5ba58 133 break;
KrissyHam 1:a81deeb5ba58 134 case MIDIMessage::PitchWheelType:
KrissyHam 1:a81deeb5ba58 135 printf("PitchWheel channel: %d, pitch: %d\n", msg.channel(), msg.pitch());
KrissyHam 1:a81deeb5ba58 136 break;
KrissyHam 1:a81deeb5ba58 137 default:
KrissyHam 1:a81deeb5ba58 138 printf("Another message\n");
KrissyHam 1:a81deeb5ba58 139 }
KrissyHam 1:a81deeb5ba58 140 }
4180_1 0:e693d5bf0a25 141
4180_1 0:e693d5bf0a25 142
4180_1 0:e693d5bf0a25 143 int main()
4180_1 0:e693d5bf0a25 144 {
KrissyHam 1:a81deeb5ba58 145
KrissyHam 2:82b2a1e84586 146 midi.attach(show_message); // call back for messages received
KrissyHam 1:a81deeb5ba58 147 pc.baud(9600);
KrissyHam 1:a81deeb5ba58 148 pc.printf("Hello world!\n");
4180_1 0:e693d5bf0a25 149 IMU.begin();
4180_1 0:e693d5bf0a25 150 if (!IMU.begin()) {
KrissyHam 1:a81deeb5ba58 151 pc.printf("Failed to communicate with LSM9DS1 - first.\n");
4180_1 0:e693d5bf0a25 152 }
4180_1 0:e693d5bf0a25 153 IMU.calibrate(1);
KrissyHam 1:a81deeb5ba58 154
KrissyHam 1:a81deeb5ba58 155 IMU2.begin();
KrissyHam 1:a81deeb5ba58 156 if (!IMU2.begin()) {
KrissyHam 1:a81deeb5ba58 157 pc.printf("Failed to communicate with LSM9DS1 - second.\n");
KrissyHam 1:a81deeb5ba58 158 }
KrissyHam 1:a81deeb5ba58 159 IMU2.calibrate(1);
KrissyHam 1:a81deeb5ba58 160
KrissyHam 1:a81deeb5ba58 161 t.start();
KrissyHam 1:a81deeb5ba58 162
4180_1 0:e693d5bf0a25 163 while(1) {
KrissyHam 1:a81deeb5ba58 164
KrissyHam 2:82b2a1e84586 165 // Initialize acceleration and gyroscope data for both IMUs
4180_1 0:e693d5bf0a25 166 while(!IMU.accelAvailable());
4180_1 0:e693d5bf0a25 167 IMU.readAccel();
4180_1 0:e693d5bf0a25 168 while(!IMU.gyroAvailable());
4180_1 0:e693d5bf0a25 169 IMU.readGyro();
KrissyHam 1:a81deeb5ba58 170
KrissyHam 1:a81deeb5ba58 171 while(!IMU2.accelAvailable());
KrissyHam 1:a81deeb5ba58 172 IMU2.readAccel();
KrissyHam 1:a81deeb5ba58 173 while(!IMU2.gyroAvailable());
KrissyHam 1:a81deeb5ba58 174 IMU2.readGyro();
KrissyHam 1:a81deeb5ba58 175
KrissyHam 2:82b2a1e84586 176 // Create variables for ease of use for acceleration
KrissyHam 1:a81deeb5ba58 177 y_accel = IMU.calcAccel(IMU.ay);
KrissyHam 1:a81deeb5ba58 178 y_accel2 = IMU2.calcAccel(IMU2.ay);
KrissyHam 1:a81deeb5ba58 179
KrissyHam 2:82b2a1e84586 180 // Initialize timer
KrissyHam 1:a81deeb5ba58 181 t_curr = t.read();
KrissyHam 1:a81deeb5ba58 182 t_curr2 = t_curr;
KrissyHam 1:a81deeb5ba58 183
KrissyHam 1:a81deeb5ba58 184
KrissyHam 2:82b2a1e84586 185 /**
KrissyHam 2:82b2a1e84586 186 * FSR detection
KrissyHam 2:82b2a1e84586 187 */
KrissyHam 2:82b2a1e84586 188
KrissyHam 2:82b2a1e84586 189 if (fsr_kick.readRaw() > 0.3){
KrissyHam 2:82b2a1e84586 190 if (kicked == false){
KrissyHam 2:82b2a1e84586 191 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 192 }
KrissyHam 2:82b2a1e84586 193 kicked = true;
KrissyHam 2:82b2a1e84586 194 }
KrissyHam 2:82b2a1e84586 195 else {kicked = false;}
KrissyHam 2:82b2a1e84586 196
KrissyHam 2:82b2a1e84586 197 if (fsr_hh.readRaw() > 0.3){
KrissyHam 2:82b2a1e84586 198 if (hh_close == false){
KrissyHam 2:82b2a1e84586 199 midi.write(MIDIMessage::NoteOn(42, fsr_hh.readRaw()*127, 10)); // Play a hi-hat pedal, map the volume, channel 10
KrissyHam 2:82b2a1e84586 200 }
KrissyHam 2:82b2a1e84586 201 hh_close = true;
KrissyHam 2:82b2a1e84586 202 }
KrissyHam 2:82b2a1e84586 203 else {hh_close = false;}
KrissyHam 2:82b2a1e84586 204
KrissyHam 1:a81deeb5ba58 205
KrissyHam 1:a81deeb5ba58 206 /**
KrissyHam 2:82b2a1e84586 207 * Running average for hit detection
KrissyHam 1:a81deeb5ba58 208 */
KrissyHam 1:a81deeb5ba58 209
KrissyHam 1:a81deeb5ba58 210 // First IMU
KrissyHam 2:82b2a1e84586 211
KrissyHam 1:a81deeb5ba58 212 total -= average[avg_index];
KrissyHam 1:a81deeb5ba58 213 average[avg_index] = IMU.calcGyro(IMU.gy);
KrissyHam 1:a81deeb5ba58 214 total += average[avg_index];
KrissyHam 1:a81deeb5ba58 215 if (avg_index > resetIndex) {
KrissyHam 1:a81deeb5ba58 216 avg_index = 0;
KrissyHam 1:a81deeb5ba58 217 } else {
KrissyHam 1:a81deeb5ba58 218 avg_index++;
KrissyHam 1:a81deeb5ba58 219 }
KrissyHam 1:a81deeb5ba58 220
KrissyHam 1:a81deeb5ba58 221 // Second IMU
KrissyHam 1:a81deeb5ba58 222 total2 -= average2[avg_index2];
KrissyHam 1:a81deeb5ba58 223 average2[avg_index2] = IMU2.calcGyro(IMU2.gy);
KrissyHam 1:a81deeb5ba58 224 total2 += average2[avg_index2];
KrissyHam 1:a81deeb5ba58 225 if (avg_index2 > resetIndex) {
KrissyHam 1:a81deeb5ba58 226 avg_index2 = 0;
KrissyHam 1:a81deeb5ba58 227 } else {
KrissyHam 1:a81deeb5ba58 228 avg_index2++;
KrissyHam 1:a81deeb5ba58 229 }
KrissyHam 1:a81deeb5ba58 230
KrissyHam 1:a81deeb5ba58 231 /**
KrissyHam 2:82b2a1e84586 232 * Detect downward hit
KrissyHam 1:a81deeb5ba58 233 */
KrissyHam 1:a81deeb5ba58 234 if (IMU.calcGyro(IMU.gy) > 35) {
KrissyHam 1:a81deeb5ba58 235 detectHit = 1;
KrissyHam 1:a81deeb5ba58 236 }
KrissyHam 1:a81deeb5ba58 237
KrissyHam 1:a81deeb5ba58 238 if (IMU2.calcGyro(IMU2.gy) > 35) {
KrissyHam 1:a81deeb5ba58 239 detectHit2 = 1;
KrissyHam 1:a81deeb5ba58 240 }
KrissyHam 1:a81deeb5ba58 241
KrissyHam 2:82b2a1e84586 242
KrissyHam 1:a81deeb5ba58 243 /**
KrissyHam 1:a81deeb5ba58 244 * Check all conditions for hit
KrissyHam 1:a81deeb5ba58 245 */
KrissyHam 1:a81deeb5ba58 246
KrissyHam 1:a81deeb5ba58 247 // Map gyroscope value ranges to volume ranges
KrissyHam 1:a81deeb5ba58 248 hit_volume = (runningAvg + 245) * (127) / (490);
KrissyHam 1:a81deeb5ba58 249 hit_volume2 = (runningAvg2 + 245) * (127) / (490) + 15;
KrissyHam 1:a81deeb5ba58 250
KrissyHam 1:a81deeb5ba58 251 // First IMU
KrissyHam 1:a81deeb5ba58 252 detectUp = IMU.calcGyro(IMU.gy) <= 0;
KrissyHam 1:a81deeb5ba58 253 runningAvg = total / BUFFERSIZE;
KrissyHam 2:82b2a1e84586 254
KrissyHam 2:82b2a1e84586 255 // 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 256 // Then check if running average is greater than a threshold (to elimate noise)
KrissyHam 2:82b2a1e84586 257 // Elimate debouncing by only allowing a hit to play if the time interval has passed
KrissyHam 2:82b2a1e84586 258 if (detectHit && detectUp && (runningAvg > avg_thresh) && (t_curr - t_prev) > interval) {
KrissyHam 2:82b2a1e84586 259 // Depending on the state, play the corresponding instrument
KrissyHam 1:a81deeb5ba58 260 switch (state) {
KrissyHam 1:a81deeb5ba58 261 case (FRONT):
KrissyHam 2:82b2a1e84586 262 midi.write(MIDIMessage::NoteOn(46, hit_volume, 10)); // Play ride sound
KrissyHam 1:a81deeb5ba58 263 break;
KrissyHam 1:a81deeb5ba58 264 case (SIDE):
KrissyHam 1:a81deeb5ba58 265 if (hh_close) {
KrissyHam 2:82b2a1e84586 266 midi.write(MIDIMessage::NoteOn(40, hit_volume, 10)); // Play closed hi-hat sound
KrissyHam 1:a81deeb5ba58 267 } else {
KrissyHam 2:82b2a1e84586 268 midi.write(MIDIMessage::NoteOn(41, hit_volume, 10)); // Play open hi-hat sound
KrissyHam 1:a81deeb5ba58 269 }
KrissyHam 1:a81deeb5ba58 270 break;
KrissyHam 1:a81deeb5ba58 271 }
KrissyHam 1:a81deeb5ba58 272 detectHit = 0;
KrissyHam 1:a81deeb5ba58 273 t_prev = t_curr;
KrissyHam 1:a81deeb5ba58 274 count = 0;
KrissyHam 1:a81deeb5ba58 275 }
KrissyHam 1:a81deeb5ba58 276
KrissyHam 1:a81deeb5ba58 277 // Second IMU
KrissyHam 1:a81deeb5ba58 278 detectUp2 = IMU2.calcGyro(IMU2.gy) <= 0;
KrissyHam 1:a81deeb5ba58 279 runningAvg2 = total2 / BUFFERSIZE;
KrissyHam 1:a81deeb5ba58 280
KrissyHam 1:a81deeb5ba58 281 if (detectHit2 && detectUp2 && runningAvg2 > avg_thresh2 && (t_curr2 - t_prev2) > interval) {
KrissyHam 1:a81deeb5ba58 282 switch (state2) {
KrissyHam 1:a81deeb5ba58 283 case (FRONT2):
KrissyHam 2:82b2a1e84586 284 midi.write(MIDIMessage::NoteOn(47, hit_volume2, 10)); // Play snare sound
KrissyHam 1:a81deeb5ba58 285 break;
KrissyHam 1:a81deeb5ba58 286 case (SIDE2):
KrissyHam 2:82b2a1e84586 287 midi.write(MIDIMessage::NoteOn(51, hit_volume2, 10)); // Play clap sound
KrissyHam 1:a81deeb5ba58 288 break;
KrissyHam 1:a81deeb5ba58 289 }
KrissyHam 1:a81deeb5ba58 290 detectHit2 = 0;
KrissyHam 1:a81deeb5ba58 291 t_prev2 = t_curr2;
KrissyHam 1:a81deeb5ba58 292 }
KrissyHam 1:a81deeb5ba58 293
KrissyHam 1:a81deeb5ba58 294
KrissyHam 1:a81deeb5ba58 295 /**
KrissyHam 2:82b2a1e84586 296 * Instrument switching detection
KrissyHam 1:a81deeb5ba58 297 */
KrissyHam 1:a81deeb5ba58 298
KrissyHam 1:a81deeb5ba58 299 curr_y_accel = y_accel;
KrissyHam 1:a81deeb5ba58 300 curr_y_accel2 = y_accel2;
KrissyHam 1:a81deeb5ba58 301
KrissyHam 1:a81deeb5ba58 302 check_y_accel = abs(curr_y_accel - prev_y_accel) > y_accel_threshold;
KrissyHam 1:a81deeb5ba58 303 check_y_accel2 = abs(curr_y_accel2 - prev_y_accel2) > y_accel_threshold;
KrissyHam 1:a81deeb5ba58 304
KrissyHam 2:82b2a1e84586 305 // 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 306 if (check_y_accel) {
KrissyHam 1:a81deeb5ba58 307 count++;
KrissyHam 1:a81deeb5ba58 308 }
KrissyHam 1:a81deeb5ba58 309
KrissyHam 1:a81deeb5ba58 310 if (check_y_accel2) {
KrissyHam 1:a81deeb5ba58 311 count2++;
KrissyHam 1:a81deeb5ba58 312 }
KrissyHam 1:a81deeb5ba58 313
KrissyHam 1:a81deeb5ba58 314 // First IMU
KrissyHam 1:a81deeb5ba58 315 switch (state) {
KrissyHam 1:a81deeb5ba58 316 case (FRONT):
KrissyHam 2:82b2a1e84586 317 // Check that y_accleration is above the threshold for at least 3 cycles
KrissyHam 2:82b2a1e84586 318 // Elimnate debouncing by only switching if a time interval has passed
KrissyHam 1:a81deeb5ba58 319 if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) {
KrissyHam 1:a81deeb5ba58 320 count = 0;
KrissyHam 1:a81deeb5ba58 321 state = SIDE;
KrissyHam 1:a81deeb5ba58 322 led1 = 1;
KrissyHam 1:a81deeb5ba58 323 led2 = 0;
KrissyHam 1:a81deeb5ba58 324 t_prev_y_accel = t_curr;
KrissyHam 1:a81deeb5ba58 325 }
KrissyHam 1:a81deeb5ba58 326 break;
KrissyHam 1:a81deeb5ba58 327 case (SIDE):
KrissyHam 1:a81deeb5ba58 328 if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) {
KrissyHam 1:a81deeb5ba58 329 count = 0;
KrissyHam 1:a81deeb5ba58 330 state = FRONT;
KrissyHam 1:a81deeb5ba58 331 led1 = 0;
KrissyHam 1:a81deeb5ba58 332 led2 = 1;
KrissyHam 1:a81deeb5ba58 333 t_prev_y_accel = t_curr;
KrissyHam 1:a81deeb5ba58 334 }
KrissyHam 1:a81deeb5ba58 335 break;
KrissyHam 1:a81deeb5ba58 336 }
KrissyHam 1:a81deeb5ba58 337 prev_y_accel = curr_y_accel;
KrissyHam 1:a81deeb5ba58 338
KrissyHam 1:a81deeb5ba58 339 //Second IMU
KrissyHam 1:a81deeb5ba58 340 switch (state2) {
KrissyHam 1:a81deeb5ba58 341 case (FRONT2):
KrissyHam 1:a81deeb5ba58 342 if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){
KrissyHam 1:a81deeb5ba58 343 state2 = SIDE2;
KrissyHam 1:a81deeb5ba58 344 count2 = 0;
KrissyHam 1:a81deeb5ba58 345 led4 = 1;
KrissyHam 1:a81deeb5ba58 346 led3 = 0;
KrissyHam 1:a81deeb5ba58 347 t_prev_y_accel2 = t_curr;
KrissyHam 1:a81deeb5ba58 348 }
KrissyHam 1:a81deeb5ba58 349 break;
KrissyHam 1:a81deeb5ba58 350 case (SIDE2):
KrissyHam 1:a81deeb5ba58 351 if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){
KrissyHam 1:a81deeb5ba58 352 state2 = FRONT2;
KrissyHam 1:a81deeb5ba58 353 count2 = 0;
KrissyHam 1:a81deeb5ba58 354 led4 = 0;
KrissyHam 1:a81deeb5ba58 355 led3 = 1;
KrissyHam 1:a81deeb5ba58 356 t_prev_y_accel2 = t_curr;
KrissyHam 1:a81deeb5ba58 357 }
KrissyHam 1:a81deeb5ba58 358 break;
KrissyHam 1:a81deeb5ba58 359 }
KrissyHam 1:a81deeb5ba58 360 prev_y_accel2 = curr_y_accel2;
4180_1 0:e693d5bf0a25 361 }
KrissyHam 2:82b2a1e84586 362 }