![](/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@1:a81deeb5ba58, 2016-04-29 (annotated)
- Committer:
- KrissyHam
- Date:
- Fri Apr 29 01:47:18 2016 +0000
- Revision:
- 1:a81deeb5ba58
- Parent:
- 0:e693d5bf0a25
- Child:
- 2:82b2a1e84586
Final version of project
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 | 1:a81deeb5ba58 | 26 | //uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin; |
4180_1 | 0:e693d5bf0a25 | 27 | Serial pc(USBTX, USBRX); |
KrissyHam | 1:a81deeb5ba58 | 28 | DigitalOut led1(LED1); |
KrissyHam | 1:a81deeb5ba58 | 29 | DigitalOut led2(LED2); |
KrissyHam | 1:a81deeb5ba58 | 30 | DigitalOut led3(LED3); |
KrissyHam | 1:a81deeb5ba58 | 31 | DigitalOut led4(LED4); |
KrissyHam | 1:a81deeb5ba58 | 32 | |
KrissyHam | 1:a81deeb5ba58 | 33 | Timer t; |
KrissyHam | 1:a81deeb5ba58 | 34 | float t_prev = 0; |
KrissyHam | 1:a81deeb5ba58 | 35 | float t_prev2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 36 | float t_curr = 0; |
KrissyHam | 1:a81deeb5ba58 | 37 | float t_curr2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 38 | float t_gyroPrev = 0; |
KrissyHam | 1:a81deeb5ba58 | 39 | float t_gyroPrev2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 40 | float t_gyroCurr = 0; |
KrissyHam | 1:a81deeb5ba58 | 41 | float t_gyroCurr2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 42 | float delta_t = 0; |
KrissyHam | 1:a81deeb5ba58 | 43 | |
KrissyHam | 1:a81deeb5ba58 | 44 | float x_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 45 | float y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 46 | float y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 47 | |
KrissyHam | 1:a81deeb5ba58 | 48 | float x_vel_prev = 0; |
KrissyHam | 1:a81deeb5ba58 | 49 | float x_vel_curr = 0; |
KrissyHam | 1:a81deeb5ba58 | 50 | float y_vel_prev = 0; |
KrissyHam | 1:a81deeb5ba58 | 51 | float y_vel_curr = 0; |
KrissyHam | 1:a81deeb5ba58 | 52 | |
KrissyHam | 1:a81deeb5ba58 | 53 | float x_pos_prev = 0; |
KrissyHam | 1:a81deeb5ba58 | 54 | float x_pos_curr = 0; |
KrissyHam | 1:a81deeb5ba58 | 55 | float y_pos_prev = 0; |
KrissyHam | 1:a81deeb5ba58 | 56 | float y_pos_curr = 0; |
KrissyHam | 1:a81deeb5ba58 | 57 | |
KrissyHam | 1:a81deeb5ba58 | 58 | int resetIndex = BUFFERSIZE - 2; |
KrissyHam | 1:a81deeb5ba58 | 59 | float average[BUFFERSIZE] = {0}; |
KrissyHam | 1:a81deeb5ba58 | 60 | int avg_index = 0; |
KrissyHam | 1:a81deeb5ba58 | 61 | float total = 0; |
KrissyHam | 1:a81deeb5ba58 | 62 | float avg_thresh; |
KrissyHam | 1:a81deeb5ba58 | 63 | |
KrissyHam | 1:a81deeb5ba58 | 64 | float average2[BUFFERSIZE] = {0}; |
KrissyHam | 1:a81deeb5ba58 | 65 | int avg_index2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 66 | float total2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 67 | float avg_thresh2; |
KrissyHam | 1:a81deeb5ba58 | 68 | |
KrissyHam | 1:a81deeb5ba58 | 69 | float z_gyro = 0; |
KrissyHam | 1:a81deeb5ba58 | 70 | float gyroAverage[3] = {0}; |
KrissyHam | 1:a81deeb5ba58 | 71 | int avg_gyroIndex = 0; |
KrissyHam | 1:a81deeb5ba58 | 72 | float gyroTotal = 0; |
KrissyHam | 1:a81deeb5ba58 | 73 | float running_gyroAvg = 0; |
KrissyHam | 1:a81deeb5ba58 | 74 | |
KrissyHam | 1:a81deeb5ba58 | 75 | float avg_gyroThresh = 0; |
KrissyHam | 1:a81deeb5ba58 | 76 | float avg_gyroThresh2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 77 | |
KrissyHam | 1:a81deeb5ba58 | 78 | float gyroAverage2[3] = {0}; |
KrissyHam | 1:a81deeb5ba58 | 79 | int avg_gyroIndex2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 80 | float gyroTotal2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 81 | float running_gyroAvg2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 82 | |
KrissyHam | 1:a81deeb5ba58 | 83 | float gyroInterval = 1.00; |
KrissyHam | 1:a81deeb5ba58 | 84 | |
KrissyHam | 1:a81deeb5ba58 | 85 | float prev_y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 86 | float curr_y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 87 | float prev_x_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 88 | float curr_x_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 89 | float y_accel_threshold = 0.8; |
KrissyHam | 1:a81deeb5ba58 | 90 | float x_accel_threshold = 0.1; |
KrissyHam | 1:a81deeb5ba58 | 91 | bool check_y_accel = false; |
KrissyHam | 1:a81deeb5ba58 | 92 | bool check_x_accel = false; |
KrissyHam | 1:a81deeb5ba58 | 93 | float t_prev_y_accel = 0; |
KrissyHam | 1:a81deeb5ba58 | 94 | |
KrissyHam | 1:a81deeb5ba58 | 95 | float prev_y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 96 | float curr_y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 97 | float prev_x_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 98 | float curr_x_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 99 | bool check_y_accel2 = false; |
KrissyHam | 1:a81deeb5ba58 | 100 | bool check_x_accel2 = false; |
KrissyHam | 1:a81deeb5ba58 | 101 | float t_prev_y_accel2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 102 | int count2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 103 | |
KrissyHam | 1:a81deeb5ba58 | 104 | |
KrissyHam | 1:a81deeb5ba58 | 105 | // enum InputType {STILL, ACCEL_POS, ACCEL_NEG}; |
KrissyHam | 1:a81deeb5ba58 | 106 | enum StateType {FRONT, SIDE, HIT}; |
KrissyHam | 1:a81deeb5ba58 | 107 | enum StateType2 {FRONT2, SIDE2, HIT2}; |
KrissyHam | 1:a81deeb5ba58 | 108 | |
KrissyHam | 1:a81deeb5ba58 | 109 | // InputType input = STILL; // Initial input is STILL |
KrissyHam | 1:a81deeb5ba58 | 110 | StateType state = FRONT; // Initial state is FRONT |
KrissyHam | 1:a81deeb5ba58 | 111 | StateType2 state2 = FRONT2; |
KrissyHam | 1:a81deeb5ba58 | 112 | |
4180_1 | 0:e693d5bf0a25 | 113 | // Calculate pitch, roll, and heading. |
4180_1 | 0:e693d5bf0a25 | 114 | // Pitch/roll calculations taken from this app note: |
4180_1 | 0:e693d5bf0a25 | 115 | // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1 |
4180_1 | 0:e693d5bf0a25 | 116 | // Heading calculations taken from this app note: |
4180_1 | 0:e693d5bf0a25 | 117 | // 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 | 118 | |
KrissyHam | 1:a81deeb5ba58 | 119 | void printAltitude(float ax, float ay, float az, float mx, float my, float mz) |
4180_1 | 0:e693d5bf0a25 | 120 | { |
4180_1 | 0:e693d5bf0a25 | 121 | float roll = atan2(ay, az); |
4180_1 | 0:e693d5bf0a25 | 122 | float pitch = atan2(-ax, sqrt(ay * ay + az * az)); |
4180_1 | 0:e693d5bf0a25 | 123 | // touchy trig stuff to use arctan to get compass heading (scale is 0..360) |
4180_1 | 0:e693d5bf0a25 | 124 | mx = -mx; |
4180_1 | 0:e693d5bf0a25 | 125 | float heading; |
4180_1 | 0:e693d5bf0a25 | 126 | if (my == 0.0) |
4180_1 | 0:e693d5bf0a25 | 127 | heading = (mx < 0.0) ? 180.0 : 0.0; |
4180_1 | 0:e693d5bf0a25 | 128 | else |
4180_1 | 0:e693d5bf0a25 | 129 | heading = atan2(mx, my)*360.0/(2.0*PI); |
KrissyHam | 1:a81deeb5ba58 | 130 | pc.printf("heading atan=%f \n\r",heading); |
4180_1 | 0:e693d5bf0a25 | 131 | heading -= DECLINATION; //correct for geo location |
4180_1 | 0:e693d5bf0a25 | 132 | if(heading>180.0) heading = heading - 360.0; |
4180_1 | 0:e693d5bf0a25 | 133 | else if(heading<-180.0) heading = 360.0 + heading; |
4180_1 | 0:e693d5bf0a25 | 134 | else if(heading<0.0) heading = 360.0 + heading; |
4180_1 | 0:e693d5bf0a25 | 135 | |
4180_1 | 0:e693d5bf0a25 | 136 | |
4180_1 | 0:e693d5bf0a25 | 137 | // Convert everything from radians to degrees: |
4180_1 | 0:e693d5bf0a25 | 138 | //heading *= 180.0 / PI; |
4180_1 | 0:e693d5bf0a25 | 139 | pitch *= 180.0 / PI; |
4180_1 | 0:e693d5bf0a25 | 140 | roll *= 180.0 / PI; |
4180_1 | 0:e693d5bf0a25 | 141 | } |
4180_1 | 0:e693d5bf0a25 | 142 | |
KrissyHam | 1:a81deeb5ba58 | 143 | void show_message(MIDIMessage msg) { |
KrissyHam | 1:a81deeb5ba58 | 144 | switch (msg.type()) { |
KrissyHam | 1:a81deeb5ba58 | 145 | case MIDIMessage::NoteOnType: |
KrissyHam | 1:a81deeb5ba58 | 146 | printf("NoteOn key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel()); |
KrissyHam | 1:a81deeb5ba58 | 147 | break; |
KrissyHam | 1:a81deeb5ba58 | 148 | case MIDIMessage::NoteOffType: |
KrissyHam | 1:a81deeb5ba58 | 149 | printf("NoteOff key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel()); |
KrissyHam | 1:a81deeb5ba58 | 150 | break; |
KrissyHam | 1:a81deeb5ba58 | 151 | case MIDIMessage::ControlChangeType: |
KrissyHam | 1:a81deeb5ba58 | 152 | printf("ControlChange controller: %d, data: %d\n", msg.controller(), msg.value()); |
KrissyHam | 1:a81deeb5ba58 | 153 | break; |
KrissyHam | 1:a81deeb5ba58 | 154 | case MIDIMessage::PitchWheelType: |
KrissyHam | 1:a81deeb5ba58 | 155 | printf("PitchWheel channel: %d, pitch: %d\n", msg.channel(), msg.pitch()); |
KrissyHam | 1:a81deeb5ba58 | 156 | break; |
KrissyHam | 1:a81deeb5ba58 | 157 | default: |
KrissyHam | 1:a81deeb5ba58 | 158 | printf("Another message\n"); |
KrissyHam | 1:a81deeb5ba58 | 159 | } |
KrissyHam | 1:a81deeb5ba58 | 160 | } |
4180_1 | 0:e693d5bf0a25 | 161 | |
KrissyHam | 1:a81deeb5ba58 | 162 | int count = 0; |
KrissyHam | 1:a81deeb5ba58 | 163 | USBMIDI midi; |
KrissyHam | 1:a81deeb5ba58 | 164 | bool detectHit = 0; |
KrissyHam | 1:a81deeb5ba58 | 165 | bool detectHit2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 166 | bool detectUp = 0; |
KrissyHam | 1:a81deeb5ba58 | 167 | bool detectUp2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 168 | float runningAvg = 0; |
KrissyHam | 1:a81deeb5ba58 | 169 | float runningAvg2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 170 | float interval; |
KrissyHam | 1:a81deeb5ba58 | 171 | float hit_volume = 0; |
KrissyHam | 1:a81deeb5ba58 | 172 | float hit_volume2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 173 | |
4180_1 | 0:e693d5bf0a25 | 174 | |
4180_1 | 0:e693d5bf0a25 | 175 | int main() |
4180_1 | 0:e693d5bf0a25 | 176 | { |
KrissyHam | 1:a81deeb5ba58 | 177 | |
KrissyHam | 1:a81deeb5ba58 | 178 | midi.attach(show_message); // call back for messages received |
KrissyHam | 1:a81deeb5ba58 | 179 | pc.baud(9600); |
KrissyHam | 1:a81deeb5ba58 | 180 | pc.printf("Hello world!\n"); |
4180_1 | 0:e693d5bf0a25 | 181 | IMU.begin(); |
4180_1 | 0:e693d5bf0a25 | 182 | if (!IMU.begin()) { |
KrissyHam | 1:a81deeb5ba58 | 183 | pc.printf("Failed to communicate with LSM9DS1 - first.\n"); |
4180_1 | 0:e693d5bf0a25 | 184 | } |
4180_1 | 0:e693d5bf0a25 | 185 | IMU.calibrate(1); |
KrissyHam | 1:a81deeb5ba58 | 186 | |
KrissyHam | 1:a81deeb5ba58 | 187 | IMU2.begin(); |
KrissyHam | 1:a81deeb5ba58 | 188 | if (!IMU2.begin()) { |
KrissyHam | 1:a81deeb5ba58 | 189 | pc.printf("Failed to communicate with LSM9DS1 - second.\n"); |
KrissyHam | 1:a81deeb5ba58 | 190 | } |
KrissyHam | 1:a81deeb5ba58 | 191 | IMU2.calibrate(1); |
KrissyHam | 1:a81deeb5ba58 | 192 | |
KrissyHam | 1:a81deeb5ba58 | 193 | t.start(); |
KrissyHam | 1:a81deeb5ba58 | 194 | |
4180_1 | 0:e693d5bf0a25 | 195 | while(1) { |
KrissyHam | 1:a81deeb5ba58 | 196 | |
4180_1 | 0:e693d5bf0a25 | 197 | while(!IMU.accelAvailable()); |
4180_1 | 0:e693d5bf0a25 | 198 | IMU.readAccel(); |
4180_1 | 0:e693d5bf0a25 | 199 | while(!IMU.gyroAvailable()); |
4180_1 | 0:e693d5bf0a25 | 200 | IMU.readGyro(); |
KrissyHam | 1:a81deeb5ba58 | 201 | |
KrissyHam | 1:a81deeb5ba58 | 202 | while(!IMU2.accelAvailable()); |
KrissyHam | 1:a81deeb5ba58 | 203 | IMU2.readAccel(); |
KrissyHam | 1:a81deeb5ba58 | 204 | while(!IMU2.gyroAvailable()); |
KrissyHam | 1:a81deeb5ba58 | 205 | IMU2.readGyro(); |
KrissyHam | 1:a81deeb5ba58 | 206 | |
KrissyHam | 1:a81deeb5ba58 | 207 | /** |
KrissyHam | 1:a81deeb5ba58 | 208 | * FSR |
KrissyHam | 1:a81deeb5ba58 | 209 | */ |
KrissyHam | 1:a81deeb5ba58 | 210 | |
KrissyHam | 1:a81deeb5ba58 | 211 | if(fsr_kick.readRaw()>0.3){ |
KrissyHam | 1:a81deeb5ba58 | 212 | if(kicked == false){ |
KrissyHam | 1:a81deeb5ba58 | 213 | midi.write(MIDIMessage::NoteOn(45, fsr_kick.readRaw()*127 + 30, 10)); |
KrissyHam | 1:a81deeb5ba58 | 214 | } |
KrissyHam | 1:a81deeb5ba58 | 215 | kicked = true; |
KrissyHam | 1:a81deeb5ba58 | 216 | } |
KrissyHam | 1:a81deeb5ba58 | 217 | else{kicked = false;} |
KrissyHam | 1:a81deeb5ba58 | 218 | |
KrissyHam | 1:a81deeb5ba58 | 219 | if(fsr_hh.readRaw()>0.3){ |
KrissyHam | 1:a81deeb5ba58 | 220 | if(hh_close == false){ |
KrissyHam | 1:a81deeb5ba58 | 221 | midi.write(MIDIMessage::NoteOn(42, fsr_hh.readRaw()*127, 10)); |
KrissyHam | 1:a81deeb5ba58 | 222 | } |
KrissyHam | 1:a81deeb5ba58 | 223 | hh_close = true; |
KrissyHam | 1:a81deeb5ba58 | 224 | } |
KrissyHam | 1:a81deeb5ba58 | 225 | else{hh_close = false;} |
KrissyHam | 1:a81deeb5ba58 | 226 | |
KrissyHam | 1:a81deeb5ba58 | 227 | /** |
KrissyHam | 1:a81deeb5ba58 | 228 | End FSR |
KrissyHam | 1:a81deeb5ba58 | 229 | **/ |
KrissyHam | 1:a81deeb5ba58 | 230 | |
KrissyHam | 1:a81deeb5ba58 | 231 | // pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0); |
KrissyHam | 1:a81deeb5ba58 | 232 | // pc.printf(" X axis Y axis Z axis\n\r"); |
KrissyHam | 1:a81deeb5ba58 | 233 | //pc.printf("gyro: %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz)); |
KrissyHam | 1:a81deeb5ba58 | 234 | //pc.printf("gyro: %9f in deg/s\n\r", IMU.calcGyro(IMU.gy)); |
KrissyHam | 1:a81deeb5ba58 | 235 | //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az)); |
KrissyHam | 1:a81deeb5ba58 | 236 | // pc.printf("mag: %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); |
KrissyHam | 1:a81deeb5ba58 | 237 | // printAltitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx), |
KrissyHam | 1:a81deeb5ba58 | 238 | // IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz)); |
KrissyHam | 1:a81deeb5ba58 | 239 | y_accel = IMU.calcAccel(IMU.ay); |
KrissyHam | 1:a81deeb5ba58 | 240 | y_accel2 = IMU2.calcAccel(IMU2.ay); |
KrissyHam | 1:a81deeb5ba58 | 241 | x_accel = IMU.calcAccel(IMU.ax); |
KrissyHam | 1:a81deeb5ba58 | 242 | |
KrissyHam | 1:a81deeb5ba58 | 243 | t_curr = t.read(); |
KrissyHam | 1:a81deeb5ba58 | 244 | t_curr2 = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 245 | |
KrissyHam | 1:a81deeb5ba58 | 246 | |
KrissyHam | 1:a81deeb5ba58 | 247 | |
KrissyHam | 1:a81deeb5ba58 | 248 | /** |
KrissyHam | 1:a81deeb5ba58 | 249 | * Averaging for hit detection |
KrissyHam | 1:a81deeb5ba58 | 250 | */ |
KrissyHam | 1:a81deeb5ba58 | 251 | |
KrissyHam | 1:a81deeb5ba58 | 252 | // First IMU |
KrissyHam | 1:a81deeb5ba58 | 253 | total -= average[avg_index]; |
KrissyHam | 1:a81deeb5ba58 | 254 | average[avg_index] = IMU.calcGyro(IMU.gy); |
KrissyHam | 1:a81deeb5ba58 | 255 | total += average[avg_index]; |
KrissyHam | 1:a81deeb5ba58 | 256 | if (avg_index > resetIndex) { |
KrissyHam | 1:a81deeb5ba58 | 257 | avg_index = 0; |
KrissyHam | 1:a81deeb5ba58 | 258 | } else { |
KrissyHam | 1:a81deeb5ba58 | 259 | avg_index++; |
KrissyHam | 1:a81deeb5ba58 | 260 | } |
KrissyHam | 1:a81deeb5ba58 | 261 | |
KrissyHam | 1:a81deeb5ba58 | 262 | // Second IMU |
KrissyHam | 1:a81deeb5ba58 | 263 | total2 -= average2[avg_index2]; |
KrissyHam | 1:a81deeb5ba58 | 264 | average2[avg_index2] = IMU2.calcGyro(IMU2.gy); |
KrissyHam | 1:a81deeb5ba58 | 265 | total2 += average2[avg_index2]; |
KrissyHam | 1:a81deeb5ba58 | 266 | if (avg_index2 > resetIndex) { |
KrissyHam | 1:a81deeb5ba58 | 267 | avg_index2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 268 | } else { |
KrissyHam | 1:a81deeb5ba58 | 269 | avg_index2++; |
KrissyHam | 1:a81deeb5ba58 | 270 | } |
KrissyHam | 1:a81deeb5ba58 | 271 | |
KrissyHam | 1:a81deeb5ba58 | 272 | /** |
KrissyHam | 1:a81deeb5ba58 | 273 | * Detect hit |
KrissyHam | 1:a81deeb5ba58 | 274 | */ |
KrissyHam | 1:a81deeb5ba58 | 275 | if (IMU.calcGyro(IMU.gy) > 35) { |
KrissyHam | 1:a81deeb5ba58 | 276 | detectHit = 1; |
KrissyHam | 1:a81deeb5ba58 | 277 | } |
KrissyHam | 1:a81deeb5ba58 | 278 | |
KrissyHam | 1:a81deeb5ba58 | 279 | if (IMU2.calcGyro(IMU2.gy) > 35) { |
KrissyHam | 1:a81deeb5ba58 | 280 | detectHit2 = 1; |
KrissyHam | 1:a81deeb5ba58 | 281 | } |
KrissyHam | 1:a81deeb5ba58 | 282 | |
KrissyHam | 1:a81deeb5ba58 | 283 | /** |
KrissyHam | 1:a81deeb5ba58 | 284 | * Check all conditions for hit |
KrissyHam | 1:a81deeb5ba58 | 285 | */ |
KrissyHam | 1:a81deeb5ba58 | 286 | |
KrissyHam | 1:a81deeb5ba58 | 287 | // Map gyroscope value ranges to volume ranges |
KrissyHam | 1:a81deeb5ba58 | 288 | hit_volume = (runningAvg + 245) * (127) / (490); |
KrissyHam | 1:a81deeb5ba58 | 289 | hit_volume2 = (runningAvg2 + 245) * (127) / (490) + 15; |
KrissyHam | 1:a81deeb5ba58 | 290 | |
KrissyHam | 1:a81deeb5ba58 | 291 | // First IMU |
KrissyHam | 1:a81deeb5ba58 | 292 | detectUp = IMU.calcGyro(IMU.gy) <= 0; |
KrissyHam | 1:a81deeb5ba58 | 293 | runningAvg = total / BUFFERSIZE; |
KrissyHam | 1:a81deeb5ba58 | 294 | interval = 0.20; |
KrissyHam | 1:a81deeb5ba58 | 295 | avg_thresh = 20; |
KrissyHam | 1:a81deeb5ba58 | 296 | |
KrissyHam | 1:a81deeb5ba58 | 297 | if (detectHit && detectUp && runningAvg > avg_thresh && (t_curr - t_prev) > interval) { |
KrissyHam | 1:a81deeb5ba58 | 298 | switch (state) { |
KrissyHam | 1:a81deeb5ba58 | 299 | case (FRONT): |
KrissyHam | 1:a81deeb5ba58 | 300 | midi.write(MIDIMessage::NoteOn(46, runningAvg, 10)); |
KrissyHam | 1:a81deeb5ba58 | 301 | break; |
KrissyHam | 1:a81deeb5ba58 | 302 | case (SIDE): |
KrissyHam | 1:a81deeb5ba58 | 303 | if (hh_close) { |
KrissyHam | 1:a81deeb5ba58 | 304 | midi.write(MIDIMessage::NoteOn(40, hit_volume, 10)); |
KrissyHam | 1:a81deeb5ba58 | 305 | } else { |
KrissyHam | 1:a81deeb5ba58 | 306 | midi.write(MIDIMessage::NoteOn(41, hit_volume, 10)); |
KrissyHam | 1:a81deeb5ba58 | 307 | } |
KrissyHam | 1:a81deeb5ba58 | 308 | break; |
KrissyHam | 1:a81deeb5ba58 | 309 | } |
KrissyHam | 1:a81deeb5ba58 | 310 | detectHit = 0; |
KrissyHam | 1:a81deeb5ba58 | 311 | t_prev = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 312 | count = 0; |
KrissyHam | 1:a81deeb5ba58 | 313 | } |
KrissyHam | 1:a81deeb5ba58 | 314 | |
KrissyHam | 1:a81deeb5ba58 | 315 | // Second IMU |
KrissyHam | 1:a81deeb5ba58 | 316 | detectUp2 = IMU2.calcGyro(IMU2.gy) <= 0; |
KrissyHam | 1:a81deeb5ba58 | 317 | runningAvg2 = total2 / BUFFERSIZE; |
KrissyHam | 1:a81deeb5ba58 | 318 | |
KrissyHam | 1:a81deeb5ba58 | 319 | if (detectHit2 && detectUp2 && runningAvg2 > avg_thresh2 && (t_curr2 - t_prev2) > interval) { |
KrissyHam | 1:a81deeb5ba58 | 320 | switch (state2) { |
KrissyHam | 1:a81deeb5ba58 | 321 | case (FRONT2): |
KrissyHam | 1:a81deeb5ba58 | 322 | midi.write(MIDIMessage::NoteOn(47, hit_volume2, 10)); |
KrissyHam | 1:a81deeb5ba58 | 323 | break; |
KrissyHam | 1:a81deeb5ba58 | 324 | case (SIDE2): |
KrissyHam | 1:a81deeb5ba58 | 325 | midi.write(MIDIMessage::NoteOn(51, hit_volume2, 10)); |
KrissyHam | 1:a81deeb5ba58 | 326 | break; |
KrissyHam | 1:a81deeb5ba58 | 327 | } |
KrissyHam | 1:a81deeb5ba58 | 328 | detectHit2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 329 | t_prev2 = t_curr2; |
KrissyHam | 1:a81deeb5ba58 | 330 | } |
KrissyHam | 1:a81deeb5ba58 | 331 | |
KrissyHam | 1:a81deeb5ba58 | 332 | |
KrissyHam | 1:a81deeb5ba58 | 333 | /** |
KrissyHam | 1:a81deeb5ba58 | 334 | * Switching instruments detection |
KrissyHam | 1:a81deeb5ba58 | 335 | */ |
KrissyHam | 1:a81deeb5ba58 | 336 | |
KrissyHam | 1:a81deeb5ba58 | 337 | curr_y_accel = y_accel; |
KrissyHam | 1:a81deeb5ba58 | 338 | curr_x_accel = x_accel; |
KrissyHam | 1:a81deeb5ba58 | 339 | |
KrissyHam | 1:a81deeb5ba58 | 340 | curr_y_accel2 = y_accel2; |
KrissyHam | 1:a81deeb5ba58 | 341 | |
KrissyHam | 1:a81deeb5ba58 | 342 | check_y_accel = abs(curr_y_accel - prev_y_accel) > y_accel_threshold; |
KrissyHam | 1:a81deeb5ba58 | 343 | check_y_accel2 = abs(curr_y_accel2 - prev_y_accel2) > y_accel_threshold; |
KrissyHam | 1:a81deeb5ba58 | 344 | |
KrissyHam | 1:a81deeb5ba58 | 345 | check_x_accel = abs(curr_x_accel - prev_x_accel) > x_accel_threshold; |
KrissyHam | 1:a81deeb5ba58 | 346 | |
KrissyHam | 1:a81deeb5ba58 | 347 | if (check_y_accel) { |
KrissyHam | 1:a81deeb5ba58 | 348 | count++; |
KrissyHam | 1:a81deeb5ba58 | 349 | } |
KrissyHam | 1:a81deeb5ba58 | 350 | |
KrissyHam | 1:a81deeb5ba58 | 351 | if (check_y_accel2) { |
KrissyHam | 1:a81deeb5ba58 | 352 | count2++; |
KrissyHam | 1:a81deeb5ba58 | 353 | } |
KrissyHam | 1:a81deeb5ba58 | 354 | |
KrissyHam | 1:a81deeb5ba58 | 355 | // First IMU |
KrissyHam | 1:a81deeb5ba58 | 356 | switch (state) { |
KrissyHam | 1:a81deeb5ba58 | 357 | case (FRONT): |
KrissyHam | 1:a81deeb5ba58 | 358 | if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) { |
KrissyHam | 1:a81deeb5ba58 | 359 | count = 0; |
KrissyHam | 1:a81deeb5ba58 | 360 | state = SIDE; |
KrissyHam | 1:a81deeb5ba58 | 361 | led1 = 1; |
KrissyHam | 1:a81deeb5ba58 | 362 | led2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 363 | t_prev_y_accel = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 364 | } |
KrissyHam | 1:a81deeb5ba58 | 365 | break; |
KrissyHam | 1:a81deeb5ba58 | 366 | case (SIDE): |
KrissyHam | 1:a81deeb5ba58 | 367 | if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) { |
KrissyHam | 1:a81deeb5ba58 | 368 | count = 0; |
KrissyHam | 1:a81deeb5ba58 | 369 | state = FRONT; |
KrissyHam | 1:a81deeb5ba58 | 370 | led1 = 0; |
KrissyHam | 1:a81deeb5ba58 | 371 | led2 = 1; |
KrissyHam | 1:a81deeb5ba58 | 372 | t_prev_y_accel = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 373 | } |
KrissyHam | 1:a81deeb5ba58 | 374 | break; |
KrissyHam | 1:a81deeb5ba58 | 375 | } |
KrissyHam | 1:a81deeb5ba58 | 376 | prev_y_accel = curr_y_accel; |
KrissyHam | 1:a81deeb5ba58 | 377 | prev_x_accel = curr_x_accel; |
KrissyHam | 1:a81deeb5ba58 | 378 | |
KrissyHam | 1:a81deeb5ba58 | 379 | //Second IMU |
KrissyHam | 1:a81deeb5ba58 | 380 | switch (state2) { |
KrissyHam | 1:a81deeb5ba58 | 381 | case (FRONT2): |
KrissyHam | 1:a81deeb5ba58 | 382 | if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){ |
KrissyHam | 1:a81deeb5ba58 | 383 | state2 = SIDE2; |
KrissyHam | 1:a81deeb5ba58 | 384 | count2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 385 | led4 = 1; |
KrissyHam | 1:a81deeb5ba58 | 386 | led3 = 0; |
KrissyHam | 1:a81deeb5ba58 | 387 | t_prev_y_accel2 = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 388 | } |
KrissyHam | 1:a81deeb5ba58 | 389 | break; |
KrissyHam | 1:a81deeb5ba58 | 390 | case (SIDE2): |
KrissyHam | 1:a81deeb5ba58 | 391 | if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){ |
KrissyHam | 1:a81deeb5ba58 | 392 | state2 = FRONT2; |
KrissyHam | 1:a81deeb5ba58 | 393 | count2 = 0; |
KrissyHam | 1:a81deeb5ba58 | 394 | led4 = 0; |
KrissyHam | 1:a81deeb5ba58 | 395 | led3 = 1; |
KrissyHam | 1:a81deeb5ba58 | 396 | t_prev_y_accel2 = t_curr; |
KrissyHam | 1:a81deeb5ba58 | 397 | } |
KrissyHam | 1:a81deeb5ba58 | 398 | break; |
KrissyHam | 1:a81deeb5ba58 | 399 | } |
KrissyHam | 1:a81deeb5ba58 | 400 | prev_y_accel2 = curr_y_accel2; |
4180_1 | 0:e693d5bf0a25 | 401 | } |
4180_1 | 0:e693d5bf0a25 | 402 | } |
4180_1 | 0:e693d5bf0a25 | 403 |