#include #include "SensorFusion.h" //SF #include //#include //#include //#include #include BLEMIDI_CREATE_DEFAULT_INSTANCE() unsigned long tm = millis(); unsigned long t0 = millis(); bool isConnected = false; // midi // offsets CC: // 0-63: left hand // 64-127: right hand enum side {LEFT, RIGHT=64}; int hand = LEFT; int midichannel = 1; // accelerometer float accx, accy, accz; float maccx, maccy, maccz; // midi mapped float accmag; // magnitude // gyroscope float gyrox, gyroy, gyroz; float mgyrox, mgyroy, mgyroz; // magnetometer float magx, magy, magz; float mmagx, mmagy, mmagz; // sensor fusion float mroll, mpitch, myaw; float maxx, maxy, maxz; //SF - SF fusion; float gx, gy, gz, ax, ay, az, mx, my, mz; float pitch, roll, yaw; float deltat; float gxoff = 0; float gyoff = 0; float gzoff = 0; float fmap(float x, float in_min, float in_max, float out_min, float out_max) { return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } byte highbits(float f) { return int(f) % 128; } byte lowbits(float f) { return int(f * 128) % 128; }; unsigned long loops = 0; unsigned long loopmillis = 0; unsigned long midis = 0; unsigned long midimillis = 0; // ----------------------------------------------------------------------------- // When BLE connected, LED will turn on (indication that connection was successful) // When receiving a NoteOn, LED will go out, on NoteOff, light comes back on. // This is an easy and conveniant way to show that the connection is alive and working. // ----------------------------------------------------------------------------- void setup() { Serial.begin(115200); // while (!Serial) { ; // wait for serial port to connect. Needed for native USB // } loopmillis = millis(); midimillis = millis(); Serial.println("MIDI-BLE TEST 3"); Serial.println("2022-06-16"); MIDI.begin(); pinMode(LED_BUILTIN, OUTPUT); digitalWrite(LED_BUILTIN, LOW); BLEMIDI.setHandleConnected([]() { isConnected = true; digitalWrite(LED_BUILTIN, HIGH); Serial.println("MIDI-BLE CONNECTED"); }); BLEMIDI.setHandleDisconnected([]() { isConnected = false; digitalWrite(LED_BUILTIN, LOW); Serial.println("MIDI-BLE DISCONNECTED"); }); MIDI.setHandleNoteOn([](byte channel, byte note, byte velocity) { digitalWrite(LED_BUILTIN, LOW); }); MIDI.setHandleNoteOff([](byte channel, byte note, byte velocity) { digitalWrite(LED_BUILTIN, HIGH); }); // gyro calibration Serial.print("Loading calibration: "); if (loadCalibration()) { gxoff=getGyroXCal(); gyoff=getGyroYCal(); gyoff=getGyroZCal(); Serial.println("success :)"); } else { Serial.println("failed :("); } printCalibration(); // motion sensor if (!IMU.begin()) { Serial.println("Failed to initialize IMU!"); while (1); } Serial.print("Accelerometer sample rate = "); Serial.print(IMU.accelerationSampleRate()); Serial.println(" Hz"); Serial.println(); Serial.println("Acceleration in G's"); Serial.println("X\tY\tZ"); Serial.print("Gyroscope sample rate = "); Serial.print(IMU.gyroscopeSampleRate()); Serial.println(" Hz"); Serial.println(); Serial.println("Gyroscope in degrees/second"); Serial.println("X\tY\tZ"); Serial.print("Magnetic field sample rate = "); Serial.print(IMU.magneticFieldSampleRate()); Serial.println(" uT"); Serial.println(); Serial.println("Magnetic Field in uT"); Serial.println("X\tY\tZ"); Serial.print("Setup took (ms): "); Serial.println(millis()-loopmillis); loopmillis = millis(); midimillis = millis(); } // ----------------------------------------------------------------------------- // // ----------------------------------------------------------------------------- void loop() { // now you should read the gyroscope, accelerometer (and magnetometer if you have it also) // NOTE: the gyroscope data have to be in radians // if you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD if (IMU.magneticFieldAvailable()) { IMU.readMagneticField(mx, my, mz); } if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) { tm = millis(); loops++; if (!(loops%1000)) { Serial.print("1000 loops took (ms): "); Serial.println(tm-loopmillis); loopmillis=tm; } MIDI.read(); IMU.readAcceleration(ax, ay, az); /* ax *= 9.81; ay *= 9.81; az *= 9.81; */ IMU.readGyroscope(gx, gy, gz); gx -= gxoff; gy -= gxoff; gz -= gxoff; gx *= DEG_TO_RAD; gy *= DEG_TO_RAD; gz *= DEG_TO_RAD; deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update //choose only one of these two: //fusion.MahonyUpdate(gx, gy, gz, ax, ay, az, deltat); //mahony is suggested if there isn't the mag and the mcu is slow fusion.MadgwickUpdate(gx, gy, gz, ax, ay, az, -mx, my, mz, deltat); //else use the magwick, it is slower but more accurate if (isConnected && (tm - t0) > 6) { t0 = tm; switch (midis) { case 0: maccx = fmap(ax,-4.0,4.0, 0.0,127.0); MIDI.sendControlChange(0 + hand, maccx, midichannel); break; case 1: maccy = fmap(ay,-4.0,4.0, 0.0,127.0); MIDI.sendControlChange(1 + hand, maccy, midichannel); break; case 2: maccz = fmap(az,-4.0,4.0, 0.0,127.0); MIDI.sendControlChange(2 + hand, maccz, midichannel); break; case 3: mgyrox = map(gx, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0); MIDI.sendControlChange(3 + hand, mgyrox, midichannel); break; case 4: mgyroy = map(gy, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0); MIDI.sendControlChange(4 + hand, mgyroy, midichannel); break; case 5: mgyroz = map(gz, -2000*DEG_TO_RAD, 2000*DEG_TO_RAD, 0.0, 127.0); MIDI.sendControlChange(5 + hand, mgyroz, midichannel); break; case 6: accmag = sqrt(ax*ax+ay*ay+az*az); // accmag = map(accmag, 0, 1000, 0,127); MIDI.sendControlChange(6 + hand, accmag * 100, midichannel); break; case 7: mmagx = fmap(mx, 0,60, 0,127); MIDI.sendControlChange(7 + hand, mmagx, midichannel); break; case 8: mmagy = fmap(my, 0,60, 0,127); MIDI.sendControlChange(8 + hand, mmagy, midichannel); break; case 9: mmagz = fmap(mz, 0,60, 0,127); MIDI.sendControlChange(9 + hand, mmagz, midichannel); break; case 10: roll = fusion.getRoll(); //you could also use getRollRadians() ecc mroll = fmap(roll,-180,180,0,127); MIDI.sendControlChange(10 + hand, mroll, midichannel); break; case 11: pitch = fusion.getPitch(); mpitch = fmap(pitch,-90,90,0,127); MIDI.sendControlChange(11 + hand, mpitch, midichannel); break; case 12: MIDI.sendControlChange(12 + hand, myaw, midichannel); yaw = fusion.getYaw(); myaw = fmap(yaw,0,360,0,127); break; case 13: roll = fusion.getRoll(); //you could also use getRollRadians() ecc mroll = fmap(roll,-180,180,0,127); MIDI.sendControlChange(13 + hand, lowbits(mroll), midichannel); break; case 14: pitch = fusion.getPitch(); mpitch = fmap(pitch,-90,90,0,127); MIDI.sendControlChange(14 + hand, lowbits(mpitch), midichannel); break; case 15: MIDI.sendControlChange(15 + hand, lowbits(myaw), midichannel); yaw = fusion.getYaw(); myaw = fmap(yaw,0,360,0,127); break; } //mgyrox = gyrox/2000 * 64 + 64; //mgyroy = gyroy/2000 * 64 + 64; //mgyroz = gyroz/2000 * 64 + 64; // mz = max(10, abs(z) * 1000); // maxx = max(magx, maxx); // maxy = max(magy, maxy); // maxz = max(magz, maxz); // Serial.print(mx); // Serial.print('\t'); // Serial.print(my); // Serial.print(y); // Serial.print('\t'); // Serial.println(z); // Serial.println(); // MIDI.sendNoteOn (my, mx, 1); // note 60, velocity 100 on channel 1 // Serial.print(mmagx); // Serial.print('\t'); // Serial.print(mmagy); // Serial.print('\t'); // Serial.println(mmagz); // Serial.print('\t'); // Serial.println(accmag * 100); // Serial.println("ping"); // delay(mz); // MIDI.sendNoteOff(my, mx, 1); midis=(midis+1)%16; if (midis==0) { Serial.print("MIDI sending took (ms): "); Serial.println(tm-midimillis); midimillis=tm; } } } }