Compare commits
1 Commits
master
...
eulerConve
Author | SHA1 | Date |
---|---|---|
Martin | f3dfbe4958 |
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -1,277 +0,0 @@
|
|||
/**! @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 <BLEMIDI_Transport.h>
|
||||
#include "SensorFusion.h" //SF
|
||||
|
||||
#include <BLEMIDI_Transport.h>
|
||||
//#include <hardware/BLEMIDI_ESP32_NimBLE.h>
|
||||
//#include <hardware/BLEMIDI_ESP32.h>
|
||||
//#include <hardware/BLEMIDI_nRF52.h>
|
||||
|
@ -9,7 +9,6 @@
|
|||
|
||||
BLEMIDI_CREATE_DEFAULT_INSTANCE()
|
||||
|
||||
unsigned long tm = millis();
|
||||
unsigned long t0 = millis();
|
||||
bool isConnected = false;
|
||||
|
||||
|
@ -44,30 +43,18 @@ 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;
|
||||
//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;
|
||||
}
|
||||
|
||||
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.
|
||||
|
@ -77,16 +64,8 @@ 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");
|
||||
|
||||
Serial.println("MIDI-BLE TEST 1");
|
||||
Serial.println("2022-03-02");
|
||||
|
||||
MIDI.begin();
|
||||
|
||||
|
@ -114,21 +93,7 @@ void setup()
|
|||
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
|
||||
// accelerometer
|
||||
if (!IMU.begin()) {
|
||||
Serial.println("Failed to initialize IMU!");
|
||||
while (1);
|
||||
|
@ -154,12 +119,6 @@ void setup()
|
|||
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();
|
||||
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------------------
|
||||
|
@ -167,29 +126,15 @@ void setup()
|
|||
// -----------------------------------------------------------------------------
|
||||
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;
|
||||
|
@ -199,9 +144,9 @@ void loop()
|
|||
|
||||
IMU.readGyroscope(gx, gy, gz);
|
||||
|
||||
gx -= gxoff;
|
||||
gy -= gxoff;
|
||||
gz -= gxoff;
|
||||
gx -= goffx;
|
||||
gy -= goffy;
|
||||
gz -= goffz;
|
||||
|
||||
gx *= DEG_TO_RAD;
|
||||
gy *= DEG_TO_RAD;
|
||||
|
@ -211,119 +156,36 @@ void loop()
|
|||
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
|
||||
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 = 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;
|
||||
|
||||
}
|
||||
|
||||
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);
|
||||
maxx = max(magx, maxx);
|
||||
maxy = max(magy, maxy);
|
||||
maxz = max(magz, maxz);
|
||||
|
||||
// Serial.print(mx);
|
||||
// Serial.print('\t');
|
||||
|
@ -332,9 +194,30 @@ break;
|
|||
// 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);
|
||||
|
@ -345,25 +228,5 @@ break;
|
|||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
|
|
@ -42,7 +42,6 @@
|
|||
** dependencies (libraries)
|
||||
1. OSC (filter by "open sound control")
|
||||
2. MPU6050
|
||||
3. BasicLinearAlgebra
|
||||
|
||||
** calibration
|
||||
- IMU_Zero
|
||||
|
|
|
@ -63,11 +63,10 @@ uint8_t devStatus; // return status after each device operation (0 = succes
|
|||
uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
|
||||
uint16_t fifoCount; // count of all bytes currently in FIFO
|
||||
uint8_t fifoBuffer[64]; // FIFO storage buffer
|
||||
unsigned long timeOn;
|
||||
unsigned long timeOn = 0;
|
||||
|
||||
// orientation/motion vars
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
Quaternion pq; // [w, x, y, z] quaternion container
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 gy; // [x, y, z] gyro sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
|
@ -75,8 +74,6 @@ VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measure
|
|||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
Matrix<3> position; // [x,y,z] tracks position of device
|
||||
Matrix<3> speed; // [x,y,z] tracks speed of device
|
||||
|
||||
|
||||
// Sem dobimo vrednosti
|
||||
|
@ -94,9 +91,38 @@ OSCMessage emsg("/error/");
|
|||
OSCMessage kmsg("/keys/");
|
||||
OSCMessage quaternionMessage("/quaternion/");
|
||||
OSCMessage quaternionDiffMessage("/quaternionDiff/");
|
||||
OSCMessage eulerDiffMessage("/eulerDiff/");
|
||||
OSCMessage positionMessage("/position/");
|
||||
OSCMessage speedMessage("/speed/");
|
||||
OSCMessage eulerDiffMessage("eulerDiff");
|
||||
|
||||
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
|
||||
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
|
||||
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
|
||||
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
|
||||
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
|
||||
|
||||
BLA::Matrix<4,4> rotationMatrix = {
|
||||
1 - (yy + zz), xy + wz, xz - wy, 0,
|
||||
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
||||
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
||||
0, 0, 0, 1
|
||||
};
|
||||
|
||||
//TODO: test whether BLA library uses column-major matrix notation in code
|
||||
BLA::Matrix<3> eulerVector;
|
||||
eulerVector.Fill(0);
|
||||
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
|
||||
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
|
||||
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
|
||||
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
|
||||
} else {
|
||||
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
|
||||
eulerVector(2) = 0;
|
||||
}
|
||||
return eulerVector;
|
||||
}
|
||||
|
||||
float clamp(float value,float min,float max) {
|
||||
return fmaxf( min, fminf(max, value));
|
||||
}
|
||||
|
||||
void setup() {
|
||||
Wire.begin();
|
||||
|
@ -107,11 +133,6 @@ void setup() {
|
|||
pinMode(keys[i], INPUT_PULLUP);
|
||||
}
|
||||
|
||||
//Set position to origin, speed to nothing, and uptime to 0
|
||||
timeOn = 0;
|
||||
position.Fill(0);
|
||||
speed.Fill(0);
|
||||
|
||||
Serial.begin(115200); // set this as high as you can reliably run on your platform
|
||||
SerialBT.begin("wavey wind");
|
||||
|
||||
|
@ -169,50 +190,6 @@ void setup() {
|
|||
}
|
||||
}
|
||||
|
||||
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
|
||||
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
|
||||
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
|
||||
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
|
||||
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
|
||||
|
||||
BLA::Matrix<4,4> rotationMatrix = {
|
||||
1 - (yy + zz), xy + wz, xz - wy, 0,
|
||||
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
||||
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
||||
0, 0, 0, 1
|
||||
};
|
||||
|
||||
//TODO: test whether BLA library uses column-major matrix notation in code
|
||||
BLA::Matrix<3> eulerVector;
|
||||
eulerVector.Fill(0);
|
||||
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
|
||||
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
|
||||
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
|
||||
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
|
||||
} else {
|
||||
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
|
||||
eulerVector(2) = 0;
|
||||
}
|
||||
return eulerVector;
|
||||
}
|
||||
|
||||
void streamAndClearMessage(OSCMessage msg) {
|
||||
SLIPSerial.beginPacket();
|
||||
msg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
msg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
msg.empty();
|
||||
}
|
||||
|
||||
float clamp(float value,float min,float max) {
|
||||
return fmaxf( min, fminf(max, value));
|
||||
}
|
||||
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
|
@ -235,27 +212,72 @@ void loop() {
|
|||
quaternionMessage.add(q.x);
|
||||
quaternionMessage.add(q.y);
|
||||
quaternionMessage.add(q.z);
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
quaternionMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
quaternionMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
streamAndClearMessage(quaternionMessage);
|
||||
quaternionMessage.empty();
|
||||
|
||||
quaternionDiffMessage.add(diff.w);
|
||||
quaternionDiffMessage.add(diff.x);
|
||||
quaternionDiffMessage.add(diff.y);
|
||||
quaternionDiffMessage.add(diff.z);
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
quaternionDiffMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
quaternionDiffMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
streamAndClearMessage(quaternionDiffMessage);
|
||||
quaternionDiffMessage.empty();
|
||||
|
||||
Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
|
||||
|
||||
eulerDiffMessage.add(eulerDiffVector(0));
|
||||
eulerDiffMessage.add(eulerDiffVector(1));
|
||||
eulerDiffMessage.add(eulerDiffVector(2));
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
eulerDiffMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
eulerDiffMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
streamAndClearMessage(eulerDiffMessage);
|
||||
eulerDiffMessage.empty();
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
|
||||
GyX = euler[0];
|
||||
GyY = euler[1];
|
||||
GyZ = euler[2];
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
|
||||
GyX = ypr[0];
|
||||
GyY = ypr[1];
|
||||
GyZ = ypr[2];
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
|
@ -285,11 +307,6 @@ void loop() {
|
|||
int prevTime = timeOn;
|
||||
timeOn = millis();
|
||||
int elapsedTime = timeOn - prevTime;
|
||||
Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
|
||||
|
||||
//Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
|
||||
position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
|
||||
speed += speedGain;
|
||||
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
|
@ -298,27 +315,33 @@ void loop() {
|
|||
msg.add(AcY);
|
||||
msg.add(AcZ);
|
||||
msg.add(elapsedTime);
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
msg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
streamAndClearMessage(msg);
|
||||
SLIPBTSerial.beginPacket();
|
||||
msg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
msg.empty();
|
||||
|
||||
positionMessage.add(position(0));
|
||||
positionMessage.add(position(1));
|
||||
positionMessage.add(position(2));
|
||||
|
||||
streamAndClearMessage(positionMessage);
|
||||
|
||||
speedMessage.add(speed(0));
|
||||
speedMessage.add(speed(1));
|
||||
speedMessage.add(speed(2));
|
||||
|
||||
streamAndClearMessage(speedMessage);
|
||||
|
||||
|
||||
// Send keys
|
||||
for(int i = 0; i < KEYLEN; i++) {
|
||||
pressed[i] = !digitalRead(keys[i]);
|
||||
kmsg.add(pressed[i]);
|
||||
}
|
||||
streamAndClearMessage(kmsg);
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
kmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
kmsg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
kmsg.empty();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
|
|
@ -1,181 +0,0 @@
|
|||
#include "SLIPEncodedBluetoothSerial.h"
|
||||
#include "BluetoothSerial.h"
|
||||
|
||||
/*
|
||||
CONSTRUCTOR
|
||||
*/
|
||||
//instantiate with the tranmission layer
|
||||
//use BluetoothSerial
|
||||
SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
|
||||
serial = &s;
|
||||
rstate = CHAR;
|
||||
}
|
||||
|
||||
static const uint8_t eot = 0300;
|
||||
static const uint8_t slipesc = 0333;
|
||||
static const uint8_t slipescend = 0334;
|
||||
static const uint8_t slipescesc = 0335;
|
||||
/*
|
||||
SERIAL METHODS
|
||||
*/
|
||||
bool SLIPEncodedBluetoothSerial::endofPacket()
|
||||
{
|
||||
if(rstate == SECONDEOT)
|
||||
{
|
||||
rstate = CHAR;
|
||||
return true;
|
||||
}
|
||||
if (rstate==FIRSTEOT)
|
||||
{
|
||||
if(serial->available())
|
||||
{
|
||||
uint8_t c =serial->peek();
|
||||
if(c==eot)
|
||||
{
|
||||
serial->read(); // throw it on the floor
|
||||
}
|
||||
}
|
||||
rstate = CHAR;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
int SLIPEncodedBluetoothSerial::available(){
|
||||
back:
|
||||
int cnt = serial->available();
|
||||
|
||||
if(cnt==0)
|
||||
return 0;
|
||||
if(rstate==CHAR)
|
||||
{
|
||||
uint8_t c =serial->peek();
|
||||
if(c==slipesc)
|
||||
{
|
||||
rstate = SLIPESC;
|
||||
serial->read(); // throw it on the floor
|
||||
goto back;
|
||||
}
|
||||
else if( c==eot)
|
||||
{
|
||||
rstate = FIRSTEOT;
|
||||
serial->read(); // throw it on the floor
|
||||
goto back;
|
||||
}
|
||||
return 1; // we may have more but this is the only sure bet
|
||||
}
|
||||
else if(rstate==SLIPESC)
|
||||
return 1;
|
||||
else if(rstate==FIRSTEOT)
|
||||
{
|
||||
if(serial->peek()==eot)
|
||||
{
|
||||
rstate = SECONDEOT;
|
||||
serial->read(); // throw it on the floor
|
||||
return 0;
|
||||
}
|
||||
rstate = CHAR;
|
||||
}else if (rstate==SECONDEOT) {
|
||||
rstate = CHAR;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
//reads a byte from the buffer
|
||||
int SLIPEncodedBluetoothSerial::read(){
|
||||
back:
|
||||
uint8_t c = serial->read();
|
||||
if(rstate==CHAR)
|
||||
{
|
||||
if(c==slipesc)
|
||||
{
|
||||
rstate=SLIPESC;
|
||||
goto back;
|
||||
}
|
||||
else if(c==eot){
|
||||
|
||||
return -1; // xxx this is an error
|
||||
}
|
||||
return c;
|
||||
}
|
||||
else
|
||||
if(rstate==SLIPESC)
|
||||
{
|
||||
rstate=CHAR;
|
||||
if(c==slipescend)
|
||||
return eot;
|
||||
else if(c==slipescesc)
|
||||
return slipesc;
|
||||
else {
|
||||
// insert some error code here
|
||||
return -1;
|
||||
}
|
||||
|
||||
}
|
||||
else
|
||||
return -1;
|
||||
}
|
||||
|
||||
// as close as we can get to correct behavior
|
||||
int SLIPEncodedBluetoothSerial::peek(){
|
||||
uint8_t c = serial->peek();
|
||||
if(rstate==SLIPESC)
|
||||
{
|
||||
if(c==slipescend)
|
||||
return eot;
|
||||
else if(c==slipescesc)
|
||||
return slipesc;
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
//the arduino and wiring libraries have different return types for the write function
|
||||
#if defined(WIRING) || defined(BOARD_DEFS_H)
|
||||
|
||||
//encode SLIP
|
||||
void SLIPEncodedBluetoothSerial::write(uint8_t b){
|
||||
if(b == eot){
|
||||
serial->write(slipesc);
|
||||
return serial->write(slipescend);
|
||||
} else if(b==slipesc) {
|
||||
serial->write(slipesc);
|
||||
return serial->write(slipescesc);
|
||||
} else {
|
||||
return serial->write(b);
|
||||
}
|
||||
}
|
||||
void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { while(size--) write(*buffer++); }
|
||||
#else
|
||||
//encode SLIP
|
||||
size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
|
||||
if(b == eot){
|
||||
serial->write(slipesc);
|
||||
return serial->write(slipescend);
|
||||
} else if(b==slipesc) {
|
||||
serial->write(slipesc);
|
||||
return serial->write(slipescesc);
|
||||
} else {
|
||||
return serial->write(b);
|
||||
}
|
||||
}
|
||||
size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
|
||||
|
||||
#endif
|
||||
|
||||
void SLIPEncodedBluetoothSerial::begin(String name){
|
||||
serial->begin(name);
|
||||
}
|
||||
//SLIP specific method which begins a transmitted packet
|
||||
void SLIPEncodedBluetoothSerial::beginPacket() { serial->write(eot); }
|
||||
|
||||
//signify the end of the packet with an EOT
|
||||
void SLIPEncodedBluetoothSerial::endPacket(){
|
||||
serial->write(eot);
|
||||
|
||||
}
|
||||
|
||||
void SLIPEncodedBluetoothSerial::flush(){
|
||||
serial->flush();
|
||||
}
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
/*
|
||||
Extends the Serial class to encode SLIP over serial
|
||||
*/
|
||||
|
||||
#ifndef SLIPEncodedBluetoothSerial_h
|
||||
#define SLIPEncodedBluetoothSerial_h
|
||||
|
||||
|
||||
#include "Arduino.h"
|
||||
#include <Stream.h>
|
||||
#include "BluetoothSerial.h"
|
||||
|
||||
|
||||
class SLIPEncodedBluetoothSerial: public Stream{
|
||||
|
||||
private:
|
||||
enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
|
||||
|
||||
//the serial port used
|
||||
BluetoothSerial * serial;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
//the serial port used
|
||||
SLIPEncodedBluetoothSerial(BluetoothSerial & );
|
||||
|
||||
|
||||
int available();
|
||||
int read();
|
||||
int peek();
|
||||
void flush();
|
||||
|
||||
//same as Serial.begin
|
||||
void begin(String);
|
||||
|
||||
//SLIP specific method which begins a transmitted packet
|
||||
void beginPacket();
|
||||
|
||||
//SLIP specific method which ends a transmittedpacket
|
||||
void endPacket();
|
||||
// SLIP specific method which indicates that an EOT was received
|
||||
bool endofPacket();
|
||||
|
||||
|
||||
//the arduino and wiring libraries have different return types for the write function
|
||||
#if defined(WIRING) || defined(BOARD_DEFS_H)
|
||||
void write(uint8_t b);
|
||||
void write(const uint8_t *buffer, size_t size);
|
||||
|
||||
#else
|
||||
//overrides the Stream's write function to encode SLIP
|
||||
size_t write(uint8_t b);
|
||||
size_t write(const uint8_t *buffer, size_t size);
|
||||
|
||||
//using Print::write;
|
||||
#endif
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif
|
|
@ -3,98 +3,55 @@
|
|||
#include "Wire.h"
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
|
||||
#include <OSCBundle.h>
|
||||
|
||||
#include <OSCBoards.h>
|
||||
#include <OSCMessage.h>
|
||||
/*
|
||||
Make an OSC message and send it over serial
|
||||
*/
|
||||
|
||||
// POSITION CALCULATION
|
||||
#include <BasicLinearAlgebra.h>
|
||||
#include "math.h"
|
||||
|
||||
using namespace BLA;
|
||||
|
||||
#define SERIAL_OSC
|
||||
//#define WIFI_OSC
|
||||
#define BT_OSC
|
||||
|
||||
#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// SERIAL
|
||||
#ifdef BOARD_HAS_USB_SERIAL
|
||||
#include <SLIPEncodedUSBSerial.h>
|
||||
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
|
||||
#else
|
||||
#include <SLIPEncodedSerial.h>
|
||||
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
|
||||
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
|
||||
#endif
|
||||
|
||||
// WIFI
|
||||
#ifdef WIFI_OSC
|
||||
#include <WiFi.h>
|
||||
const char* ssid = "Grajski"; // your network SSID (name of wifi network)
|
||||
const char* password = "nedeladanes"; // your network password
|
||||
|
||||
// Multicast IP / port
|
||||
const IPAddress castIp = IPAddress(224,0,1,9);
|
||||
const int port = 6696;
|
||||
bool connected = false;
|
||||
|
||||
#include <WiFiUdp.h>
|
||||
WiFiUDP udp;
|
||||
|
||||
void connectToWiFi(const char * ssid, const char * pwd){
|
||||
Serial.println("Connecting to WiFi network: " + String(ssid));
|
||||
|
||||
// delete old config
|
||||
WiFi.disconnect(true);
|
||||
//register event handler
|
||||
WiFi.onEvent(WiFiEvent);
|
||||
|
||||
//Initiate connection
|
||||
WiFi.begin(ssid, pwd);
|
||||
|
||||
Serial.println("Waiting for WIFI connection...");
|
||||
}
|
||||
|
||||
//wifi event handler
|
||||
void WiFiEvent(WiFiEvent_t event){
|
||||
switch(event) {
|
||||
case ARDUINO_EVENT_WIFI_STA_GOT_IP:
|
||||
//When connected set
|
||||
Serial.print("WiFi connected! IP address: ");
|
||||
Serial.println(WiFi.localIP());
|
||||
//initializes the UDP state
|
||||
//This initializes the transfer buffer
|
||||
udp.begin(WiFi.localIP(), port);
|
||||
connected = true;
|
||||
break;
|
||||
case ARDUINO_EVENT_WIFI_STA_DISCONNECTED:
|
||||
connected = false;
|
||||
Serial.println("\n\n\n================\nLOST WIFI CONNECTION!\n\n\nTrying again soon...\n\n\n");
|
||||
delay(1000);
|
||||
connectToWiFi(ssid, password);
|
||||
break;
|
||||
default: break;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// Bluetooth
|
||||
#ifdef BT_OSC
|
||||
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
|
||||
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
|
||||
#endif
|
||||
|
||||
#include <SLIPEncodedSerial.h>
|
||||
#include "BluetoothSerial.h"
|
||||
#include "SLIPEncodedBluetoothSerial.h"
|
||||
|
||||
BluetoothSerial SerialBT;
|
||||
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
|
||||
#endif
|
||||
|
||||
// Motion sensor object
|
||||
MPU6050 mpu;
|
||||
|
||||
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
|
||||
// quaternion components in a [w, x, y, z] format (not best for parsing
|
||||
// on a remote host such as Processing or something though)
|
||||
#define OUTPUT_READABLE_QUATERNION
|
||||
|
||||
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
|
||||
// (in degrees) calculated from the quaternions coming from the FIFO.
|
||||
// Note that Euler angles suffer from gimbal lock (for more info, see
|
||||
// http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
//#define OUTPUT_READABLE_EULER
|
||||
|
||||
// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
|
||||
// pitch/roll angles (in degrees) calculated from the quaternions coming
|
||||
// from the FIFO. Note this also requires gravity vector calculations.
|
||||
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
|
||||
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
|
||||
#define OUTPUT_READABLE_YAWPITCHROLL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
|
||||
// components with gravity removed. This acceleration reference frame is
|
||||
// not compensated for orientation, so +X is always +X according to the
|
||||
// sensor, just without the effects of gravity. If you want acceleration
|
||||
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
|
||||
//#define OUTPUT_READABLE_REALACCEL
|
||||
|
||||
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
|
||||
// components with gravity removed and adjusted for the world frame of
|
||||
// reference (yaw is relative to initial orientation, since no magnetometer
|
||||
// is present in this case). Could be quite handy in some cases.
|
||||
#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
|
||||
// MPU control/status vars
|
||||
bool dmpReady = false; // set true if DMP init was successful
|
||||
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
||||
|
@ -105,9 +62,6 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
|
|||
|
||||
// orientation/motion vars
|
||||
Quaternion q; // [w, x, y, z] quaternion container
|
||||
Quaternion pq; // [w, x, y, z] previous quaternion container
|
||||
Quaternion diff; // [w, x, y, z] quaternion derivate container
|
||||
Quaternion cq; // [w, x, y, z] calibration quaternion
|
||||
VectorInt16 aa; // [x, y, z] accel sensor measurements
|
||||
VectorInt16 gy; // [x, y, z] gyro sensor measurements
|
||||
VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
|
||||
|
@ -115,15 +69,9 @@ VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measure
|
|||
VectorFloat gravity; // [x, y, z] gravity vector
|
||||
float euler[3]; // [psi, theta, phi] Euler angle container
|
||||
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
|
||||
uint32_t timeOn = 0; // Uptime counter for movement calculation
|
||||
Matrix<3> position; // [x,y,z] tracks position of device
|
||||
Matrix<3> speed; // [x,y,z] tracks speed of device
|
||||
Matrix<3> eulerVector;
|
||||
Matrix<3> eulerDiffVector;
|
||||
bool reset; // For quaternion calibration
|
||||
|
||||
|
||||
// Sem dobimo vrednosti pospeskomerja in ziroskopa
|
||||
// Sem dobimo vrednosti
|
||||
int16_t AcX,AcY,AcZ;
|
||||
float GyX, GyY, GyZ;
|
||||
|
||||
|
@ -132,69 +80,23 @@ byte keys[] = {16, 17, 5, 18};
|
|||
byte pressed[] = {0, 0, 0, 0};
|
||||
byte KEYLEN = 4;
|
||||
|
||||
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
|
||||
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
|
||||
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
|
||||
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
|
||||
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
|
||||
|
||||
BLA::Matrix<4,4> rotationMatrix = {
|
||||
1 - (yy + zz), xy + wz, xz - wy, 0,
|
||||
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
|
||||
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
|
||||
0, 0, 0, 1
|
||||
};
|
||||
|
||||
//TODO: test whether BLA library uses column-major matrix notation in code
|
||||
BLA::Matrix<3> eulerVector;
|
||||
eulerVector.Fill(0);
|
||||
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
|
||||
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
|
||||
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
|
||||
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
|
||||
} else {
|
||||
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
|
||||
eulerVector(2) = 0;
|
||||
}
|
||||
return eulerVector;
|
||||
}
|
||||
|
||||
float clamp(float value,float min,float max) {
|
||||
return fmaxf( min, fminf(max, value));
|
||||
}
|
||||
|
||||
|
||||
/* OSC MSG channels */
|
||||
OSCBundle bundle;
|
||||
OSCMessage msg("/accel/");
|
||||
OSCMessage gmsg("/gyro/");
|
||||
OSCMessage emsg("/error/");
|
||||
OSCMessage kmsg("/keys/");
|
||||
OSCMessage qmsg("/quaternion/");
|
||||
|
||||
void setup() {
|
||||
// Basic(debug) serial init
|
||||
// Serial.begin(115200); // set this as high as you can reliably run on your platform
|
||||
Serial.println("Starting up...");
|
||||
|
||||
// I2C init
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
|
||||
#ifdef SERIAL_OSC
|
||||
SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform
|
||||
#endif
|
||||
|
||||
|
||||
// Keys
|
||||
for(int i = 0; i < KEYLEN; i++) {
|
||||
pinMode(keys[i], INPUT_PULLUP);
|
||||
}
|
||||
|
||||
// Position and speed tracking
|
||||
|
||||
timeOn = 0;
|
||||
position.Fill(0);
|
||||
speed.Fill(0);
|
||||
|
||||
// Start MPU
|
||||
mpu.initialize();
|
||||
|
||||
// Set sensitivity / range
|
||||
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
|
||||
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
|
||||
|
||||
|
@ -202,24 +104,24 @@ void setup() {
|
|||
devStatus = mpu.dmpInitialize();
|
||||
|
||||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
// !!! Run Zero IMU to get readings (read comments for instructions)
|
||||
// !!! Run Zero IMU to get readings
|
||||
|
||||
/* First proto (right hand, black&blue)*/
|
||||
/* First proto (right hand, black&blue)
|
||||
mpu.setXGyroOffset(76);
|
||||
mpu.setYGyroOffset(68);
|
||||
mpu.setZGyroOffset(10);
|
||||
mpu.setXAccelOffset(-3527);
|
||||
mpu.setYAccelOffset(-913);
|
||||
mpu.setZAccelOffset(1027);
|
||||
*/
|
||||
|
||||
/* Second proto, translucent / white
|
||||
/* Second proto, translucent / white */
|
||||
mpu.setXGyroOffset(-3650);
|
||||
mpu.setYGyroOffset(-2531);
|
||||
mpu.setZGyroOffset(1131);
|
||||
mpu.setXAccelOffset(162);
|
||||
mpu.setYAccelOffset(-16);
|
||||
mpu.setZAccelOffset(-12);
|
||||
*/
|
||||
|
||||
// make sure it worked (returns 0 if so)
|
||||
if (devStatus == 0) {
|
||||
|
@ -239,65 +141,65 @@ void setup() {
|
|||
// get expected DMP packet size for later comparison
|
||||
packetSize = mpu.dmpGetFIFOPacketSize();
|
||||
} else {
|
||||
Serial.println("DMP Initialization failed (code " + String(devStatus) + ")");
|
||||
Serial.println("Error: " + String(devStatus));
|
||||
emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
|
||||
SLIPSerial.beginPacket();
|
||||
emsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
emsg.empty();
|
||||
// ERROR!
|
||||
// 1 = initial memory load failed
|
||||
// 2 = DMP configuration updates failed
|
||||
// (if it's going to break, usually the code will be 1)
|
||||
}
|
||||
|
||||
#ifdef WIFI_OSC
|
||||
// WIFI init
|
||||
Serial.print("Attempting to connect to SSID: ");
|
||||
Serial.println(ssid);
|
||||
connectToWiFi(ssid, password);
|
||||
|
||||
// attempt to connect to Wifi network:
|
||||
while (WiFi.status() != WL_CONNECTED) {
|
||||
Serial.print(".");
|
||||
// wait 1 second for re-trying
|
||||
delay(1000);
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef BT_OSC
|
||||
SerialBT.begin("wavey wind");
|
||||
#endif
|
||||
}
|
||||
|
||||
void loop() {
|
||||
// if programming failed, don't try to do anything
|
||||
if (!dmpReady) return;
|
||||
|
||||
// read a packet from FIFO
|
||||
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
|
||||
|
||||
// Store last Q value
|
||||
pq = Quaternion(q.w,q.x,q.y,q.z);
|
||||
|
||||
// get quaternion values in easy matrix form: w x y z
|
||||
|
||||
|
||||
#ifdef OUTPUT_READABLE_QUATERNION
|
||||
// display quaternion values in easy matrix form: w x y z
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
q = q.getProduct(cq);
|
||||
|
||||
//compute the differential rotation between the previous and new orientation
|
||||
diff = q.getProduct(pq.getConjugate());
|
||||
|
||||
// Quaternion - rotacija
|
||||
bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z
|
||||
qmsg.add(q.w);
|
||||
qmsg.add(q.x);
|
||||
qmsg.add(q.y);
|
||||
qmsg.add(q.z);
|
||||
SLIPSerial.beginPacket();
|
||||
qmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
qmsg.empty();
|
||||
#endif
|
||||
|
||||
// Euler - rotacija
|
||||
//eulerVector = eulerFromQuaternion(q);
|
||||
//bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
|
||||
|
||||
// Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
|
||||
bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z
|
||||
|
||||
// Rotation diff value in euler angle
|
||||
//eulerDiffVector = eulerFromQuaternion(diff);
|
||||
//bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
|
||||
|
||||
#ifdef OUTPUT_READABLE_EULER
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetEuler(euler, &q);
|
||||
|
||||
GyX = euler[0];
|
||||
GyY = euler[1];
|
||||
GyZ = euler[2];
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_YAWPITCHROLL
|
||||
// display Euler angles in degrees
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
|
||||
|
||||
GyX = ypr[0];
|
||||
GyY = ypr[1];
|
||||
GyZ = ypr[2];
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
|
@ -309,6 +211,7 @@ void loop() {
|
|||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
|
@ -316,67 +219,36 @@ void loop() {
|
|||
AcX = aaWorld.x;
|
||||
AcY = aaWorld.y;
|
||||
AcZ = aaWorld.z;
|
||||
|
||||
#endif
|
||||
|
||||
// Calculate speed and position from accelerometer data
|
||||
/*
|
||||
int prevTime = timeOn;
|
||||
timeOn = millis();
|
||||
int elapsedTime = timeOn - prevTime;
|
||||
Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
|
||||
// Send over serial
|
||||
msg.add(AcX);
|
||||
msg.add(AcY);
|
||||
msg.add(AcZ);
|
||||
SLIPSerial.beginPacket();
|
||||
msg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
msg.empty();
|
||||
|
||||
|
||||
gmsg.add(GyX);
|
||||
gmsg.add(GyY);
|
||||
gmsg.add(GyZ);
|
||||
SLIPSerial.beginPacket();
|
||||
gmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
gmsg.empty();
|
||||
|
||||
//Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
|
||||
position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
|
||||
speed += speedGain;
|
||||
|
||||
bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
|
||||
bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
|
||||
*/
|
||||
// Accelerometer
|
||||
bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
|
||||
|
||||
// Keys held down
|
||||
bundle.add("/keys"); // A B C D E
|
||||
|
||||
// Send keys
|
||||
for(int i = 0; i < KEYLEN; i++) {
|
||||
pressed[i] = !digitalRead(keys[i]);
|
||||
bundle.getOSCMessage("/keys")->add(pressed[i]);
|
||||
kmsg.add(pressed[i]);
|
||||
}
|
||||
|
||||
// Reset calibration euler?
|
||||
if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
|
||||
if (!reset) {
|
||||
cq = q.getConjugate();
|
||||
reset = true;
|
||||
Serial.println("Quaternion calibrate");
|
||||
}
|
||||
} else {
|
||||
if (reset) {
|
||||
reset = false;
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef SERIAL_OSC
|
||||
SLIPSerial.beginPacket();
|
||||
bundle.send(SLIPSerial);
|
||||
kmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef WIFI_OSC
|
||||
udp.beginPacket(castIp, port);
|
||||
bundle.send(udp);
|
||||
udp.endPacket();
|
||||
#endif
|
||||
|
||||
// Some bug below, it seems
|
||||
#ifdef BT_OSC
|
||||
SLIPBTSerial.beginPacket();
|
||||
bundle.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
#endif
|
||||
|
||||
bundle.empty();
|
||||
kmsg.empty();
|
||||
}
|
||||
}
|
||||
|
|
|
@ -28,6 +28,8 @@ const IPAddress castIp = IPAddress(224,0,1,9);
|
|||
const int port = 6696;
|
||||
bool connected = false;
|
||||
|
||||
//#include "AsyncUDP.h"
|
||||
//AsyncUDP udp;
|
||||
|
||||
#include <WiFiUdp.h>
|
||||
WiFiUDP udp;
|
||||
|
|
Loading…
Reference in New Issue