File content as of revision 2:82b2a1e84586:

#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.

// 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);

USBMIDI midi;

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 y_accel = 0;
float y_accel2 = 0;


int resetIndex = BUFFERSIZE - 2;
float average[BUFFERSIZE] = {0};
int avg_index = 0;
float total = 0;
float avg_thresh = 20;

float average2[BUFFERSIZE] = {0};
int avg_index2 = 0;
float total2 = 0;
float avg_thresh2;

float prev_y_accel = 0;
float curr_y_accel = 0;
float y_accel_threshold = 0.8;
bool check_y_accel = false;
float t_prev_y_accel = 0;

float prev_y_accel2 = 0;
float curr_y_accel2 = 0;
bool check_y_accel2 = false;
float t_prev_y_accel2 = 0;

int count2 = 0;
int count = 0;

bool detectHit = 0;
bool detectHit2 = 0;
bool detectUp = 0;
bool detectUp2 = 0;

float runningAvg = 0;
float runningAvg2 = 0;

float interval = 0.20;
float hit_volume = 0;
float hit_volume2 = 0;


enum StateType {FRONT, SIDE, HIT};
enum StateType2 {FRONT2, SIDE2, HIT2};

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 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));
// touchy trig stuff to use arctan to get compass heading (scale is 0..360)
    mx = -mx;
    float heading;
    if (my == 0.0)
        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);
    heading -= DECLINATION; //correct for geo location
    if(heading>180.0) heading = heading - 360.0;
    else if(heading<-180.0) heading = 360.0 + heading;
    else if(heading<0.0) heading = 360.0  + heading;


    // Convert everything from radians to degrees:
    //heading *= 180.0 / PI;
    pitch *= 180.0 / PI;
    roll  *= 180.0 / PI;
}

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 main()
{
     
    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 - first.\n");
    }
    IMU.calibrate(1);
    
    IMU2.begin();
    if (!IMU2.begin()) {
        pc.printf("Failed to communicate with LSM9DS1 - second.\n");
    }
    IMU2.calibrate(1);    
    
    t.start();
    
    while(1) {
        
        // Initialize acceleration and gyroscope data for both IMUs
        while(!IMU.accelAvailable());
        IMU.readAccel();
        while(!IMU.gyroAvailable());
        IMU.readGyro();
        
        while(!IMU2.accelAvailable());
        IMU2.readAccel();
        while(!IMU2.gyroAvailable());
        IMU2.readGyro();
        
        // Create variables for ease of use for acceleration
        y_accel = IMU.calcAccel(IMU.ay);
        y_accel2 = IMU2.calcAccel(IMU2.ay);
        
        // Initialize timer
        t_curr = t.read();
        t_curr2 = t_curr;
        
        
        /**
        * FSR detection
        */
        
        if (fsr_kick.readRaw() > 0.3){
            if (kicked == false){
                midi.write(MIDIMessage::NoteOn(45, fsr_kick.readRaw() * 127 + 30, 10)); // Play a kick, map the volume and boost the amplitude, channel 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)); // Play a hi-hat pedal, map the volume, channel 10
            }           
            hh_close = true;
        }
        else {hh_close = false;} 
        
        
        /** 
        * Running average 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 downward 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;
        
        // Check if drumstick is brought down and then brought back up (eliminates continous hit detection if drumstick is just held tilted down)
        // Then check if running average is greater than a threshold (to elimate noise)
        // Elimate debouncing by only allowing a hit to play if the time interval has passed   
        if (detectHit && detectUp && (runningAvg > avg_thresh) && (t_curr - t_prev) > interval) {
            // Depending on the state, play the corresponding instrument
            switch (state) {
                case (FRONT):
                    midi.write(MIDIMessage::NoteOn(46, hit_volume, 10)); // Play ride sound
                    break;
                case (SIDE):
                    if (hh_close) {
                        midi.write(MIDIMessage::NoteOn(40, hit_volume, 10)); // Play closed hi-hat sound
                    } else {
                        midi.write(MIDIMessage::NoteOn(41, hit_volume, 10)); // Play open hi-hat sound
                    }
                    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)); // Play snare sound
                    break;
                case (SIDE2):
                    midi.write(MIDIMessage::NoteOn(51, hit_volume2, 10)); // Play clap sound
                    break;
            }
            detectHit2 = 0;
            t_prev2 = t_curr2;
        }
        
        
        /**
        * Instrument switching detection
        */
        
        curr_y_accel = y_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 that y accleration is above threshold; if it is, increase the count for the number of cycles it is above this threshold
        if (check_y_accel) {
            count++;
        }
        
        if (check_y_accel2) {
            count2++;
        }
            
        // First IMU
        switch (state) {
            case (FRONT):
                // Check that y_accleration is above the threshold for at least 3 cycles
                // Elimnate debouncing by only switching if a time interval has passed
                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;
        
        //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;
    }
}