Compare commits
1 Commits
Author | SHA1 | Date |
---|---|---|
|
1ef26458d1 |
Binary file not shown.
|
@ -0,0 +1,277 @@
|
||||||
|
/**! @brief Class for managing storing "Adafruit" sensor calibration
|
||||||
|
in internal Flash memory with KVStore
|
||||||
|
for Arduino Nano 33 BLE
|
||||||
|
* **/
|
||||||
|
|
||||||
|
//https://os.mbed.com/docs/mbed-os/v6.12/apis/kvstore.html
|
||||||
|
//KVStore. TDBStore - Default implementation of the KVStore API. It provides static wear-leveling and quick access for when you have a small number of KV pairs.
|
||||||
|
//https://os.mbed.com/docs/mbed-os/v6.12/apis/data-architecture.html
|
||||||
|
|
||||||
|
#include "KVStore.h"
|
||||||
|
#include "kvstore_global_api.h"
|
||||||
|
|
||||||
|
/**! XYZ vector of offsets for zero-g, in m/s^2 */
|
||||||
|
float accel_zerog[3] = {0, 0, 0};
|
||||||
|
|
||||||
|
/**! XYZ vector of offsets for zero-rate, in rad/s */
|
||||||
|
float gyro_zerorate[3] = {0, 0, 0};
|
||||||
|
|
||||||
|
/**! XYZ vector of offsets for hard iron calibration (in uT) */
|
||||||
|
float mag_hardiron[3] = {0, 0, 0};
|
||||||
|
|
||||||
|
/**! The 3x3 matrix for soft-iron calibration (unitless) */
|
||||||
|
float mag_softiron[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
||||||
|
|
||||||
|
/**! The magnetic field magnitude in uTesla */
|
||||||
|
float mag_field = 50;
|
||||||
|
|
||||||
|
const char* const ASC_KV_Key0 = "store_flag";
|
||||||
|
const char* const ASC_KV_Key1 = "store_offsets";
|
||||||
|
|
||||||
|
void setGyroCalibration(float x, float y, float z) {
|
||||||
|
gyro_zerorate[0]=x;
|
||||||
|
gyro_zerorate[1]=y;
|
||||||
|
gyro_zerorate[2]=z;
|
||||||
|
}
|
||||||
|
|
||||||
|
float getGyroXCal() {
|
||||||
|
return gyro_zerorate[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
float getGyroYCal() {
|
||||||
|
return gyro_zerorate[1];
|
||||||
|
}
|
||||||
|
|
||||||
|
float getGyroZCal() {
|
||||||
|
return gyro_zerorate[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
bool saveCalibration(void) {
|
||||||
|
Serial.println("Save Cal");
|
||||||
|
|
||||||
|
kv_reset("/kv/");
|
||||||
|
|
||||||
|
uint8_t flag=42;
|
||||||
|
|
||||||
|
kv_set(ASC_KV_Key0,&flag,sizeof(flag),0);
|
||||||
|
|
||||||
|
float offsets[16];
|
||||||
|
memcpy(offsets, accel_zerog, 12); // 3 x 4-byte floats
|
||||||
|
memcpy(offsets + 3, gyro_zerorate, 12); // 3 x 4-byte floats
|
||||||
|
memcpy(offsets + 6, mag_hardiron, 12); // 3 x 4-byte floats
|
||||||
|
|
||||||
|
offsets[9] = mag_field;
|
||||||
|
|
||||||
|
offsets[10] = mag_softiron[0];
|
||||||
|
offsets[11] = mag_softiron[4];
|
||||||
|
offsets[12] = mag_softiron[8];
|
||||||
|
offsets[13] = mag_softiron[1];
|
||||||
|
offsets[14] = mag_softiron[2];
|
||||||
|
offsets[15] = mag_softiron[5];
|
||||||
|
|
||||||
|
kv_set(ASC_KV_Key1, (uint8_t*)&offsets, sizeof(offsets), 0);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool loadCalibration(void) {
|
||||||
|
|
||||||
|
uint8_t flag;
|
||||||
|
|
||||||
|
kv_get(ASC_KV_Key0,&flag,sizeof(flag),0);
|
||||||
|
|
||||||
|
if (flag=!42) {
|
||||||
|
Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
float offsets[16];
|
||||||
|
|
||||||
|
kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
|
||||||
|
|
||||||
|
accel_zerog[0] = offsets[0];
|
||||||
|
accel_zerog[1] = offsets[1];
|
||||||
|
accel_zerog[2] = offsets[2];
|
||||||
|
|
||||||
|
gyro_zerorate[0] = offsets[3];
|
||||||
|
gyro_zerorate[1] = offsets[4];
|
||||||
|
gyro_zerorate[2] = offsets[5];
|
||||||
|
|
||||||
|
mag_hardiron[0] = offsets[6];
|
||||||
|
mag_hardiron[1] = offsets[7];
|
||||||
|
mag_hardiron[2] = offsets[8];
|
||||||
|
|
||||||
|
mag_field = offsets[9];
|
||||||
|
|
||||||
|
mag_softiron[0] = offsets[10];
|
||||||
|
mag_softiron[1] = offsets[13];
|
||||||
|
mag_softiron[2] = offsets[14];
|
||||||
|
mag_softiron[3] = offsets[13];
|
||||||
|
mag_softiron[4] = offsets[11];
|
||||||
|
mag_softiron[5] = offsets[15];
|
||||||
|
mag_softiron[6] = offsets[14];
|
||||||
|
mag_softiron[7] = offsets[15];
|
||||||
|
mag_softiron[8] = offsets[12];
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool printCalibration(void) {
|
||||||
|
Serial.println(F("------------"));
|
||||||
|
|
||||||
|
Serial.print("Accelerometer: ");
|
||||||
|
Serial.print(accel_zerog[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(accel_zerog[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(accel_zerog[2]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
Serial.print("Gyroscope: ");
|
||||||
|
Serial.print(gyro_zerorate[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(gyro_zerorate[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(gyro_zerorate[2]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
Serial.print("Magnetometer Hard Iron: ");
|
||||||
|
Serial.print(mag_hardiron[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_hardiron[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_hardiron[2]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
Serial.print("Magnetic Field: ");
|
||||||
|
Serial.print(mag_field);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
Serial.print("Magnetometer Soft Iron: ");
|
||||||
|
Serial.print(mag_softiron[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[2]);
|
||||||
|
Serial.println();
|
||||||
|
Serial.print(mag_softiron[3]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[4]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[5]);
|
||||||
|
Serial.println();
|
||||||
|
Serial.print(mag_softiron[6]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[7]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[8]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
Serial.println(F("\n------------"));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool printSavedCalibration(void) {
|
||||||
|
Serial.println(F("------------"));
|
||||||
|
|
||||||
|
uint8_t flag;
|
||||||
|
|
||||||
|
kv_get(ASC_KV_Key0,&flag,sizeof(flag),0);
|
||||||
|
|
||||||
|
if (flag=!42) {
|
||||||
|
Serial.print("FLAG NOT SET, CALIBRATION NOT FOUND");
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
float offsets[16];
|
||||||
|
|
||||||
|
kv_get(ASC_KV_Key1,(byte*)&offsets,sizeof(offsets),0);
|
||||||
|
|
||||||
|
accel_zerog[0] = offsets[0];
|
||||||
|
accel_zerog[1] = offsets[1];
|
||||||
|
accel_zerog[2] = offsets[2];
|
||||||
|
|
||||||
|
Serial.print("Accelerometer: ");
|
||||||
|
Serial.print(accel_zerog[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(accel_zerog[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(accel_zerog[2]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
gyro_zerorate[0] = offsets[3];
|
||||||
|
gyro_zerorate[1] = offsets[4];
|
||||||
|
gyro_zerorate[2] = offsets[5];
|
||||||
|
|
||||||
|
Serial.print("Gyroscope: ");
|
||||||
|
Serial.print(gyro_zerorate[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(gyro_zerorate[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(gyro_zerorate[2]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
mag_hardiron[0] = offsets[6];
|
||||||
|
mag_hardiron[1] = offsets[7];
|
||||||
|
mag_hardiron[2] = offsets[8];
|
||||||
|
|
||||||
|
Serial.println("Magnetometer Hard Iron: ");
|
||||||
|
Serial.print(mag_hardiron[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_hardiron[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_hardiron[2]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
mag_field = offsets[9];
|
||||||
|
|
||||||
|
Serial.print("Magnetic Field: ");
|
||||||
|
Serial.print(mag_field);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
mag_softiron[0] = offsets[10];
|
||||||
|
mag_softiron[1] = offsets[13];
|
||||||
|
mag_softiron[2] = offsets[14];
|
||||||
|
mag_softiron[3] = offsets[13];
|
||||||
|
mag_softiron[4] = offsets[11];
|
||||||
|
mag_softiron[5] = offsets[15];
|
||||||
|
mag_softiron[6] = offsets[14];
|
||||||
|
mag_softiron[7] = offsets[15];
|
||||||
|
mag_softiron[8] = offsets[12];
|
||||||
|
|
||||||
|
Serial.print("Magnetometer Soft Iron: ");
|
||||||
|
Serial.print(mag_softiron[0]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[1]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[2]);
|
||||||
|
Serial.println();
|
||||||
|
Serial.print(mag_softiron[3]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[4]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[5]);
|
||||||
|
Serial.println();
|
||||||
|
Serial.print(mag_softiron[6]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[7]);
|
||||||
|
Serial.print(", ");
|
||||||
|
Serial.print(mag_softiron[8]);
|
||||||
|
Serial.println();
|
||||||
|
|
||||||
|
/* for (uint16_t a = ee_addr; a < ee_addr + KVStore_CAL_SIZE; a++) {
|
||||||
|
uint8_t c = KVStore.read(a);
|
||||||
|
Serial.print("0x");
|
||||||
|
if (c < 0x10)
|
||||||
|
Serial.print('0');
|
||||||
|
Serial.print(c, HEX);
|
||||||
|
Serial.print(", ");
|
||||||
|
if ((a - ee_addr) % 16 == 15) {
|
||||||
|
Serial.println();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
Serial.println(F("\n------------"));
|
||||||
|
return true;
|
||||||
|
}
|
|
@ -1,7 +1,7 @@
|
||||||
#include <Arduino_LSM9DS1.h>
|
#include <Arduino_LSM9DS1.h>
|
||||||
#include <BLEMIDI_Transport.h>
|
|
||||||
#include "SensorFusion.h" //SF
|
#include "SensorFusion.h" //SF
|
||||||
|
|
||||||
|
#include <BLEMIDI_Transport.h>
|
||||||
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
|
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
|
||||||
//#include <hardware/BLEMIDI_ESP32.h>
|
//#include <hardware/BLEMIDI_ESP32.h>
|
||||||
//#include <hardware/BLEMIDI_nRF52.h>
|
//#include <hardware/BLEMIDI_nRF52.h>
|
||||||
|
@ -9,6 +9,7 @@
|
||||||
|
|
||||||
BLEMIDI_CREATE_DEFAULT_INSTANCE()
|
BLEMIDI_CREATE_DEFAULT_INSTANCE()
|
||||||
|
|
||||||
|
unsigned long tm = millis();
|
||||||
unsigned long t0 = millis();
|
unsigned long t0 = millis();
|
||||||
bool isConnected = false;
|
bool isConnected = false;
|
||||||
|
|
||||||
|
@ -43,18 +44,30 @@ float gx, gy, gz, ax, ay, az, mx, my, mz;
|
||||||
float pitch, roll, yaw;
|
float pitch, roll, yaw;
|
||||||
float deltat;
|
float deltat;
|
||||||
|
|
||||||
//Gyro Offset....: -605622 -24028 -338386
|
float gxoff = 0;
|
||||||
|
float gyoff = 0;
|
||||||
float goffx = -605622.0 / 1000000.0;
|
float gzoff = 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)
|
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;
|
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 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.
|
// When receiving a NoteOn, LED will go out, on NoteOff, light comes back on.
|
||||||
|
@ -64,8 +77,16 @@ void setup()
|
||||||
{
|
{
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
|
|
||||||
Serial.println("MIDI-BLE TEST 1");
|
// while (!Serial) {
|
||||||
Serial.println("2022-03-02");
|
; // 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();
|
MIDI.begin();
|
||||||
|
|
||||||
|
@ -93,7 +114,21 @@ void setup()
|
||||||
digitalWrite(LED_BUILTIN, HIGH);
|
digitalWrite(LED_BUILTIN, HIGH);
|
||||||
});
|
});
|
||||||
|
|
||||||
// accelerometer
|
|
||||||
|
// 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()) {
|
if (!IMU.begin()) {
|
||||||
Serial.println("Failed to initialize IMU!");
|
Serial.println("Failed to initialize IMU!");
|
||||||
while (1);
|
while (1);
|
||||||
|
@ -119,6 +154,12 @@ void setup()
|
||||||
Serial.println();
|
Serial.println();
|
||||||
Serial.println("Magnetic Field in uT");
|
Serial.println("Magnetic Field in uT");
|
||||||
Serial.println("X\tY\tZ");
|
Serial.println("X\tY\tZ");
|
||||||
|
|
||||||
|
Serial.print("Setup took (ms): ");
|
||||||
|
Serial.println(millis()-loopmillis);
|
||||||
|
loopmillis = millis();
|
||||||
|
midimillis = millis();
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// -----------------------------------------------------------------------------
|
// -----------------------------------------------------------------------------
|
||||||
|
@ -126,15 +167,29 @@ void setup()
|
||||||
// -----------------------------------------------------------------------------
|
// -----------------------------------------------------------------------------
|
||||||
void loop()
|
void loop()
|
||||||
{
|
{
|
||||||
|
|
||||||
// now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
|
// now you should read the gyroscope, accelerometer (and magnetometer if you have it also)
|
||||||
// NOTE: the gyroscope data have to be in radians
|
// 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 you have them in degree convert them with: DEG_TO_RAD example: gx * DEG_TO_RAD
|
||||||
|
|
||||||
|
|
||||||
if (IMU.magneticFieldAvailable()) {
|
if (IMU.magneticFieldAvailable()) {
|
||||||
IMU.readMagneticField(mx, my, mz);
|
IMU.readMagneticField(mx, my, mz);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (IMU.accelerationAvailable()&&IMU.gyroscopeAvailable()) {
|
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);
|
IMU.readAcceleration(ax, ay, az);
|
||||||
/*
|
/*
|
||||||
ax *= 9.81;
|
ax *= 9.81;
|
||||||
|
@ -144,9 +199,9 @@ void loop()
|
||||||
|
|
||||||
IMU.readGyroscope(gx, gy, gz);
|
IMU.readGyroscope(gx, gy, gz);
|
||||||
|
|
||||||
gx -= goffx;
|
gx -= gxoff;
|
||||||
gy -= goffy;
|
gy -= gxoff;
|
||||||
gz -= goffz;
|
gz -= gxoff;
|
||||||
|
|
||||||
gx *= DEG_TO_RAD;
|
gx *= DEG_TO_RAD;
|
||||||
gy *= DEG_TO_RAD;
|
gy *= DEG_TO_RAD;
|
||||||
|
@ -156,36 +211,119 @@ void loop()
|
||||||
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
|
deltat = fusion.deltatUpdate(); //this have to be done before calling the fusion update
|
||||||
//choose only one of these two:
|
//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.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
|
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();
|
|
||||||
|
|
||||||
|
|
||||||
|
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 = sqrt(ax*ax+ay*ay+az*az);
|
||||||
// accmag = map(accmag, 0, 1000, 0,127);
|
// accmag = map(accmag, 0, 1000, 0,127);
|
||||||
|
|
||||||
mroll = fmap(roll,-180,180,0,127);
|
MIDI.sendControlChange(6 + hand, accmag * 100, midichannel);
|
||||||
mpitch = fmap(pitch,-90,90,0,127);
|
break;
|
||||||
myaw = fmap(yaw,0,360,0,127);
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
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;
|
//mgyrox = gyrox/2000 * 64 + 64;
|
||||||
//mgyroy = gyroy/2000 * 64 + 64;
|
//mgyroy = gyroy/2000 * 64 + 64;
|
||||||
//mgyroz = gyroz/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);
|
// 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);
|
// maxx = max(magx, maxx);
|
||||||
maxy = max(magy, maxy);
|
// maxy = max(magy, maxy);
|
||||||
maxz = max(magz, maxz);
|
// maxz = max(magz, maxz);
|
||||||
|
|
||||||
// Serial.print(mx);
|
// Serial.print(mx);
|
||||||
// Serial.print('\t');
|
// Serial.print('\t');
|
||||||
|
@ -194,30 +332,9 @@ void loop()
|
||||||
// Serial.print('\t');
|
// Serial.print('\t');
|
||||||
// Serial.println(z);
|
// Serial.println(z);
|
||||||
// Serial.println();
|
// Serial.println();
|
||||||
}
|
|
||||||
|
|
||||||
MIDI.read();
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (isConnected && (millis() - t0) > 10)
|
|
||||||
{
|
|
||||||
t0 = millis();
|
|
||||||
|
|
||||||
// MIDI.sendNoteOn (my, mx, 1); // note 60, velocity 100 on channel 1
|
// 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(mmagx);
|
||||||
// Serial.print('\t');
|
// Serial.print('\t');
|
||||||
// Serial.print(mmagy);
|
// Serial.print(mmagy);
|
||||||
|
@ -228,5 +345,25 @@ void loop()
|
||||||
// Serial.println("ping");
|
// Serial.println("ping");
|
||||||
// delay(mz);
|
// delay(mz);
|
||||||
// MIDI.sendNoteOff(my, mx, 1);
|
// MIDI.sendNoteOff(my, mx, 1);
|
||||||
|
|
||||||
|
midis=(midis+1)%16;
|
||||||
|
|
||||||
|
if (midis==0) {
|
||||||
|
|
||||||
|
Serial.print("MIDI sending took (ms): ");
|
||||||
|
Serial.println(tm-midimillis);
|
||||||
|
midimillis=tm;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue