+
+// Based on:
 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU9150
 // 1/4/2013 original by Jeff Rowberg <jeff@rowberg.net> at https://github.com/jrowberg/i2cdevlib
 //          modified by Aaron Weiss <aaron@sparkfun.com>
+//          modified by Leif Bloomquist <sonicwear@leifbloomquist.net>
 //
 // Changelog:
 //     2011-10-07 - initial release
-//     2013-1-4 - added raw magnetometer output
+//     2013-1-4   - added raw magnetometer output
+//     2014-02-17 - modified to demonstrate SoMo v4.0 board (LB)
 
 /* ============================================
 I2Cdev device library code is placed under the MIT license

    //accelgyro.getAcceleration(&ax, &ay, &az);
    //accelgyro.getRotation(&gx, &gy, &gz);

    // display tab-separated accel/gyro x/y/z values
    
   /*
    Serial.print("a/g/m:\t");
    Serial.print(ax); Serial.print("\t");
    Serial.print(ay); Serial.print("\t");
    Serial.print(az); Serial.print("\t");
    Serial.print(gx); Serial.print("\t");
    Serial.print(gy); Serial.print("\t");
    Serial.print(gz); Serial.print("\t");
    Serial.print(mx); Serial.print("\t");
    Serial.print(my); Serial.print("\t");
    Serial.println(mz);
    */
    
    
    // Convert acceleration to MIDI
    byte xcc = AccelToCC(ax);
    byte ycc = AccelToCC(ay);
    byte zcc = AccelToCC(az);
    
    // Transmit     
    sendMIDI(0xB0,20,xcc);   // X = Channel 1, CC#20
    sendMIDI(0xB0,21,ycc);   // Y = Channel 1, CC#21
    sendMIDI(0xB0,22,zcc);   // Z = Channel 1, CC#22
    
    delay(50);
}

void sendMIDI(byte cmd, byte data1, byte data2)
{
   mySerial.write(cmd);
   mySerial.write(data1);
   mySerial.write(data2);
}

// Map an input between MIN and MAX to 0 to 127
byte AccelToCC(long in)
{   
  float temp = Scale(in);
  return temp*127;
}


// Scale one of the inputs to between 0.0 and 1.0
float Scale(long in)
{
  // Bound
  if (in > MAX) in=MAX;
  if (in < MIN) in=MIN;
  
  // Change zero-offset
  in = in-MIN;
  
  // Scale between 0.0 and 1.0
  float temp = (float)in/((float)MAX-(float)MIN);
  return temp;   
}


    /*
    Serial.print("ax=");
    Serial.print(ax);
    Serial.print("\tScaled=");
    Serial.println(xscaled);
    Serial.print("\tCC=");
    Serial.println(xcc);
    */


#include <SoftwareSerial.h>

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#include "Wire.h"
