Diff: main.cpp

Revision:

1:a81deeb5ba58

Parent:

0:e693d5bf0a25

Child:

2:82b2a1e84586

diff -r e693d5bf0a25 -r a81deeb5ba58 main.cpp
--- a/main.cpp	Wed Feb 03 18:47:07 2016 +0000
+++ b/main.cpp	Fri Apr 29 01:47:18 2016 +0000
@@ -1,20 +1,122 @@
 #include "mbed.h"
+#include "USBMIDI.h"
 #include "LSM9DS1.h"
+#include "math.h" 
+#include "FSR.h"
+
 #define PI 3.14159
+#define BUFFERSIZE 6
+
 // Earth's magnetic field varies by location. Add or subtract
 // a declination to get a more accurate heading. Calculate
 // your's here:
 // http://www.ngdc.noaa.gov/geomag-web/#declination
 #define DECLINATION -4.94 // Declination (degrees) in Atlanta,GA.
 
-DigitalOut myled(LED1);
+// FSR
+FSR fsr_kick(p20, 10); // Pin 20 is used as the AnalogIn pin and a 10k resistor is used as a voltage divider
+FSR fsr_hh(p19, 10); // Pin 19 is used as the AnalogIn pin and a 10k resistor is used as a voltage divider
+bool hh_close = false;  // boolean to determine if hi-hat is closed or open
+bool kicked = false;
+
+// IMU
+LSM9DS1 IMU(p9, p10, 0xD6, 0x3C);
+LSM9DS1 IMU2(p28, p27, 0xD6, 0x3C);
+
+//uLCD_4DGL uLCD(p28,p27,p30); // serial tx, serial rx, reset pin;
 Serial pc(USBTX, USBRX);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+Timer t;
+float t_prev = 0;
+float t_prev2 = 0;
+float t_curr = 0;
+float t_curr2 = 0;
+float t_gyroPrev = 0;
+float t_gyroPrev2 = 0;
+float t_gyroCurr = 0;
+float t_gyroCurr2 = 0;
+float delta_t = 0;
+
+float x_accel = 0;
+float y_accel = 0;
+float y_accel2 = 0;
+
+float x_vel_prev = 0;
+float x_vel_curr = 0;
+float y_vel_prev = 0;
+float y_vel_curr = 0;
+
+float x_pos_prev = 0;
+float x_pos_curr = 0;
+float y_pos_prev = 0;
+float y_pos_curr = 0;
+
+int resetIndex = BUFFERSIZE - 2;
+float average[BUFFERSIZE] = {0};
+int avg_index = 0;
+float total = 0;
+float avg_thresh;
+
+float average2[BUFFERSIZE] = {0};
+int avg_index2 = 0;
+float total2 = 0;
+float avg_thresh2;
+
+float z_gyro = 0;
+float gyroAverage[3] = {0};
+int avg_gyroIndex = 0;
+float gyroTotal = 0;
+float running_gyroAvg = 0;
+
+float avg_gyroThresh = 0;
+float avg_gyroThresh2 = 0;
+
+float gyroAverage2[3] = {0};
+int avg_gyroIndex2 = 0;
+float gyroTotal2 = 0;
+float running_gyroAvg2 = 0;
+
+float gyroInterval = 1.00;
+
+float prev_y_accel = 0;
+float curr_y_accel = 0;
+float prev_x_accel = 0;
+float curr_x_accel = 0;
+float y_accel_threshold = 0.8;
+float x_accel_threshold = 0.1; 
+bool check_y_accel = false;
+bool check_x_accel = false;
+float t_prev_y_accel = 0;
+
+float prev_y_accel2 = 0;
+float curr_y_accel2 = 0;
+float prev_x_accel2 = 0;
+float curr_x_accel2 = 0;
+bool check_y_accel2 = false;
+bool check_x_accel2 = false;
+float t_prev_y_accel2 = 0;
+int count2 = 0;
+
+
+// enum InputType {STILL, ACCEL_POS, ACCEL_NEG};
+enum StateType {FRONT, SIDE, HIT};
+enum StateType2 {FRONT2, SIDE2, HIT2};
+
+// InputType input = STILL; // Initial input is STILL
+StateType state = FRONT;   // Initial state is FRONT
+StateType2 state2 = FRONT2;
+
 // Calculate pitch, roll, and heading.
 // Pitch/roll calculations taken from this app note:
 // http://cache.freescale.com/files/sensors/doc/app_note/AN3461.pdf?fpsp=1
 // Heading calculations taken from this app note:
 // http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
-void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
+
+void printAltitude(float ax, float ay, float az, float mx, float my, float mz)
 {
     float roll = atan2(ay, az);
     float pitch = atan2(-ax, sqrt(ay * ay + az * az));
@@ -25,7 +127,7 @@
         heading = (mx < 0.0) ? 180.0 : 0.0;
     else
         heading = atan2(mx, my)*360.0/(2.0*PI);
-    //pc.printf("heading atan=%f \n\r",heading);
+    pc.printf("heading atan=%f \n\r",heading);
     heading -= DECLINATION; //correct for geo location
     if(heading>180.0) heading = heading - 360.0;
     else if(heading<-180.0) heading = 360.0 + heading;
@@ -36,44 +138,266 @@
     //heading *= 180.0 / PI;
     pitch *= 180.0 / PI;
     roll  *= 180.0 / PI;
-
-    pc.printf("Pitch: %f,    Roll: %f degress\n\r",pitch,roll);
-    pc.printf("Magnetic Heading: %f degress\n\r",heading);
 }
 
+void show_message(MIDIMessage msg) {
+    switch (msg.type()) {
+        case MIDIMessage::NoteOnType:
+            printf("NoteOn key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel());
+            break;
+        case MIDIMessage::NoteOffType:
+            printf("NoteOff key:%d, velocity: %d, channel: %d\n", msg.key(), msg.velocity(), msg.channel());
+            break;
+        case MIDIMessage::ControlChangeType:    
+            printf("ControlChange controller: %d, data: %d\n", msg.controller(), msg.value());
+            break;
+        case MIDIMessage::PitchWheelType:
+            printf("PitchWheel channel: %d, pitch: %d\n", msg.channel(), msg.pitch());
+            break;
+        default:
+            printf("Another message\n");
+    }    
+}
 
-
+    int count = 0;
+    USBMIDI midi;
+    bool detectHit = 0;
+    bool detectHit2 = 0;
+    bool detectUp = 0;
+    bool detectUp2 = 0;
+    float runningAvg = 0;
+    float runningAvg2 = 0;
+    float interval;
+    float hit_volume = 0;
+    float hit_volume2 = 0;
+    
 
 int main()
 {
-    //LSM9DS1 lol(p9, p10, 0x6B, 0x1E);
-    LSM9DS1 IMU(p28, p27, 0xD6, 0x3C);
+     
+     midi.attach(show_message);         // call back for messages received 
+    pc.baud(9600);
+    pc.printf("Hello world!\n");
     IMU.begin();
     if (!IMU.begin()) {
-        pc.printf("Failed to communicate with LSM9DS1.\n");
+        pc.printf("Failed to communicate with LSM9DS1 - first.\n");
     }
     IMU.calibrate(1);
-    IMU.calibrateMag(0);
+    
+    IMU2.begin();
+    if (!IMU2.begin()) {
+        pc.printf("Failed to communicate with LSM9DS1 - second.\n");
+    }
+    IMU2.calibrate(1);    
+    
+    t.start();
+    
     while(1) {
-        while(!IMU.tempAvailable());
-        IMU.readTemp();
-        while(!IMU.magAvailable(X_AXIS));
-        IMU.readMag();
+        
         while(!IMU.accelAvailable());
         IMU.readAccel();
         while(!IMU.gyroAvailable());
         IMU.readGyro();
-        pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
-        pc.printf("        X axis    Y axis    Z axis\n\r");
-        pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
-        pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
-        pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-        printAttitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
-                      IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
-        myled = 1;
-        wait(0.5);
-        myled = 0;
-        wait(0.5);
+        
+        while(!IMU2.accelAvailable());
+        IMU2.readAccel();
+        while(!IMU2.gyroAvailable());
+        IMU2.readGyro();
+        
+        /**
+        * FSR
+        */
+        
+        if(fsr_kick.readRaw()>0.3){
+            if(kicked == false){
+                midi.write(MIDIMessage::NoteOn(45, fsr_kick.readRaw()*127 + 30, 10));
+                }
+            kicked = true;
+            }
+        else{kicked = false;}
+                
+        if(fsr_hh.readRaw()>0.3){
+            if(hh_close == false){
+                midi.write(MIDIMessage::NoteOn(42, fsr_hh.readRaw()*127, 10));
+                }           
+            hh_close = true;
+        }
+        else{hh_close = false;} 
+        
+        /**
+        End FSR
+        **/
+        
+//         pc.printf("\nIMU Temperature = %f C\n\r",25.0 + IMU.temperature/16.0);
+//        pc.printf("        X axis    Y axis    Z axis\n\r");
+        //pc.printf("gyro:  %9f %9f %9f in deg/s\n\r", IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz));
+        //pc.printf("gyro:   %9f in deg/s\n\r", IMU.calcGyro(IMU.gy));
+        //pc.printf("accel: %9f %9f %9f in Gs\n\r", IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az));
+//        pc.printf("mag:   %9f %9f %9f in gauss\n\r", IMU.calcMag(IMU.mx), IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+//        printAltitude(IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), IMU.calcMag(IMU.mx),
+//                       IMU.calcMag(IMU.my), IMU.calcMag(IMU.mz));
+        y_accel = IMU.calcAccel(IMU.ay);
+        y_accel2 = IMU2.calcAccel(IMU2.ay);
+        x_accel = IMU.calcAccel(IMU.ax);
+        
+        t_curr = t.read();
+        t_curr2 = t_curr;
+        
+        
+        
+        /** 
+        * Averaging for hit detection
+        */
+        
+        // First IMU
+        total -= average[avg_index];
+        average[avg_index] = IMU.calcGyro(IMU.gy);
+        total += average[avg_index];
+        if (avg_index > resetIndex) {
+            avg_index = 0;
+        } else {
+            avg_index++;
+        }
+        
+        // Second IMU
+        total2 -= average2[avg_index2];
+        average2[avg_index2] = IMU2.calcGyro(IMU2.gy);
+        total2 += average2[avg_index2];
+        if (avg_index2 > resetIndex) {
+            avg_index2 = 0;
+        } else {
+            avg_index2++;
+        }        
+        
+        /**
+        * Detect hit
+        */
+         if (IMU.calcGyro(IMU.gy) > 35) {
+            detectHit = 1; 
+        }
+        
+        if (IMU2.calcGyro(IMU2.gy) > 35) {
+            detectHit2 = 1; 
+        }
+        
+        /**
+        * Check all conditions for hit
+        */
+        
+        // Map gyroscope value ranges to volume ranges
+        hit_volume = (runningAvg + 245) * (127) / (490);            
+        hit_volume2 = (runningAvg2 + 245) * (127) / (490) + 15;
+        
+        // First IMU
+        detectUp = IMU.calcGyro(IMU.gy) <= 0;
+        runningAvg = total / BUFFERSIZE;
+        interval = 0.20;
+        avg_thresh = 20;
+               
+        if (detectHit && detectUp && runningAvg > avg_thresh && (t_curr - t_prev) > interval) {
+            switch (state) {
+                case (FRONT):
+                    midi.write(MIDIMessage::NoteOn(46, runningAvg, 10));
+                    break;
+                case (SIDE):
+                    if (hh_close) {
+                        midi.write(MIDIMessage::NoteOn(40, hit_volume, 10));
+                    } else {
+                        midi.write(MIDIMessage::NoteOn(41, hit_volume, 10));   
+                    }
+                    break;
+            }
+            detectHit = 0;
+            t_prev = t_curr;
+            count = 0;
+        }   
+        
+        // Second IMU
+        detectUp2 = IMU2.calcGyro(IMU2.gy) <= 0;
+        runningAvg2 = total2 / BUFFERSIZE;
+        
+        if (detectHit2 && detectUp2 && runningAvg2 > avg_thresh2  && (t_curr2 - t_prev2) > interval) {
+            switch (state2) {
+                case (FRONT2):
+                    midi.write(MIDIMessage::NoteOn(47, hit_volume2, 10));
+                    break;
+                case (SIDE2):
+                    midi.write(MIDIMessage::NoteOn(51, hit_volume2, 10));
+                    break;
+            }
+            detectHit2 = 0;
+            t_prev2 = t_curr2;
+        }
+        
+        
+        /**
+        * Switching instruments detection
+        */
+        
+        curr_y_accel = y_accel;
+        curr_x_accel = x_accel;
+        
+        curr_y_accel2 = y_accel2;
+        
+        check_y_accel = abs(curr_y_accel - prev_y_accel) >  y_accel_threshold;
+        check_y_accel2 = abs(curr_y_accel2 - prev_y_accel2) >  y_accel_threshold;
+        
+        check_x_accel = abs(curr_x_accel - prev_x_accel) >  x_accel_threshold;
+        
+        if (check_y_accel) {
+            count++;
+        }
+        
+        if (check_y_accel2) {
+            count2++;
+        }
+            
+        // First IMU
+        switch (state) {
+            case (FRONT):
+                if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) {
+                    count = 0;
+                    state = SIDE;
+                    led1 = 1;
+                    led2 = 0;
+                    t_prev_y_accel = t_curr;
+                }      
+                break;
+            case (SIDE):
+                if (check_y_accel && (count >= 3) && (t_curr - t_prev_y_accel) > 0.3) {
+                    count = 0;
+                    state = FRONT;
+                    led1 = 0;
+                    led2 = 1;
+                    t_prev_y_accel = t_curr;
+                }
+                break;
+        }
+        prev_y_accel = curr_y_accel;
+        prev_x_accel = curr_x_accel; 
+        
+        //Second IMU
+        switch (state2) {
+            case (FRONT2):
+                if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){
+                    state2 = SIDE2;
+                    count2 = 0; 
+                    led4 = 1;
+                    led3 = 0;
+                    t_prev_y_accel2 = t_curr;
+                }       
+                break;
+            case (SIDE2):
+                if (check_y_accel2 && (count2 >= 3) && (t_curr - t_prev_y_accel2) > 0.3){
+                    state2 = FRONT2;
+                    count2 = 0;
+                    led4 = 0;
+                    led3 = 1;
+                    t_prev_y_accel2 = t_curr;
+                }
+                break;
+        }
+        prev_y_accel2 = curr_y_accel2;
     }
 }