#include #include #include "SensorFusion.h" //SF //#include //#include //#include #include BLEMIDI_CREATE_DEFAULT_INSTANCE() 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; //Gyro Offset....: -605622 -24028 -338386 float goffx = -605622.0 / 1000000.0; float goffy = -24028.0 / 1000000.0; float goffz = -338386.0 / 1000000.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; } // ----------------------------------------------------------------------------- // 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); Serial.println("MIDI-BLE TEST 1"); Serial.println("2022-03-02"); 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); }); // accelerometer 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"); } // ----------------------------------------------------------------------------- // // ----------------------------------------------------------------------------- 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()) { IMU.readAcceleration(ax, ay, az); /* ax *= 9.81; ay *= 9.81; az *= 9.81; */ IMU.readGyroscope(gx, gy, gz); gx -= goffx; gy -= goffy; gz -= goffz; 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 pitch = fusion.getPitch(); roll = fusion.getRoll(); //you could also use getRollRadians() ecc yaw = fusion.getYaw(); accmag = sqrt(ax*ax+ay*ay+az*az); // accmag = map(accmag, 0, 1000, 0,127); mroll = fmap(roll,-180,180,0,127); mpitch = fmap(pitch,-90,90,0,127); myaw = fmap(yaw,0,360,0,127); maccx = fmap(ax,-4.0,4.0, 0.0,127.0); maccy = fmap(ay,-4.0,4.0, 0.0,127.0); maccz = fmap(az,-4.0,4.0, 0.0,127.0); //mgyrox = gyrox/2000 * 64 + 64; //mgyroy = gyroy/2000 * 64 + 64; //mgyroz = gyroz/2000 * 64 + 64; mgyrox = map(gx, -2000,2000, 0.0, 127.0); mgyroy = map(gy, -2000,2000, 0.0, 127.0); mgyroz = map(gz, -2000,2000, 0.0, 127.0); // mz = max(10, abs(z) * 1000); mmagx = fmap(mx, 0,60, 0,127); mmagy = fmap(my, 0,60, 0,127); mmagz = fmap(mz, 0,60, 0,127); 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.read(); if (isConnected && (millis() - t0) > 10) { t0 = millis(); // MIDI.sendNoteOn (my, mx, 1); // note 60, velocity 100 on channel 1 MIDI.sendControlChange(0 + hand, maccx, midichannel); MIDI.sendControlChange(1 + hand, maccy, midichannel); MIDI.sendControlChange(2 + hand, maccz, midichannel); MIDI.sendControlChange(3 + hand, mgyrox, midichannel); MIDI.sendControlChange(4 + hand, mgyroy, midichannel); MIDI.sendControlChange(5 + hand, mgyroz, midichannel); MIDI.sendControlChange(6, accmag * 100, midichannel); MIDI.sendControlChange(7 + hand, mmagx, midichannel); MIDI.sendControlChange(8 + hand, mmagy, midichannel); MIDI.sendControlChange(9 + hand, mmagz, midichannel); MIDI.sendControlChange(10 + hand, mroll, midichannel); MIDI.sendControlChange(11 + hand, mpitch, midichannel); MIDI.sendControlChange(12 + hand, myaw, midichannel); // 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); } }