File content as of revision 1:a81deeb5ba58:

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

//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 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 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()
{
     
     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) {
        
        while(!IMU.accelAvailable());
        IMU.readAccel();
        while(!IMU.gyroAvailable());
        IMU.readGyro();
        
        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;
    }
}