233 lines
6.5 KiB
C++
233 lines
6.5 KiB
C++
#include <Arduino_LSM9DS1.h>
|
|
#include <BLEMIDI_Transport.h>
|
|
#include "SensorFusion.h" //SF
|
|
|
|
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
|
|
//#include <hardware/BLEMIDI_ESP32.h>
|
|
//#include <hardware/BLEMIDI_nRF52.h>
|
|
#include <hardware/BLEMIDI_ArduinoBLE.h>
|
|
|
|
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);
|
|
}
|
|
}
|