Compare commits

..

1 Commits

Author SHA1 Message Date
Martin f3dfbe4958 now it compiles 2022-03-04 22:05:04 +01:00
13 changed files with 267 additions and 1028 deletions

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -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;
}

View File

@ -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,7 +9,6 @@
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;
@ -44,30 +43,18 @@ float gx, gy, gz, ax, ay, az, mx, my, mz;
float pitch, roll, yaw; float pitch, roll, yaw;
float deltat; float deltat;
float gxoff = 0; //Gyro Offset....: -605622 -24028 -338386
float gyoff = 0;
float gzoff = 0; 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) 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.
@ -77,16 +64,8 @@ void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
// while (!Serial) { Serial.println("MIDI-BLE TEST 1");
; // wait for serial port to connect. Needed for native USB Serial.println("2022-03-02");
// }
loopmillis = millis();
midimillis = millis();
Serial.println("MIDI-BLE TEST 3");
Serial.println("2022-06-16");
MIDI.begin(); MIDI.begin();
@ -114,21 +93,7 @@ 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);
@ -154,12 +119,6 @@ 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();
} }
// ----------------------------------------------------------------------------- // -----------------------------------------------------------------------------
@ -167,29 +126,15 @@ 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;
@ -199,9 +144,9 @@ void loop()
IMU.readGyroscope(gx, gy, gz); IMU.readGyroscope(gx, gy, gz);
gx -= gxoff; gx -= goffx;
gy -= gxoff; gy -= goffy;
gz -= gxoff; gz -= goffz;
gx *= DEG_TO_RAD; gx *= DEG_TO_RAD;
gy *= 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 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);
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); 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); 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); 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');
@ -332,9 +194,30 @@ break;
// 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);
@ -345,25 +228,5 @@ break;
// 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;
}
} }
}
} }

View File

@ -42,7 +42,6 @@
** dependencies (libraries) ** dependencies (libraries)
1. OSC (filter by "open sound control") 1. OSC (filter by "open sound control")
2. MPU6050 2. MPU6050
3. BasicLinearAlgebra
** calibration ** calibration
- IMU_Zero - IMU_Zero

View File

@ -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 packetSize; // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount; // count of all bytes currently in FIFO uint16_t fifoCount; // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer uint8_t fifoBuffer[64]; // FIFO storage buffer
unsigned long timeOn; unsigned long timeOn = 0;
// orientation/motion vars // orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container 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 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel 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 VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 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 // Sem dobimo vrednosti
@ -94,9 +91,38 @@ OSCMessage emsg("/error/");
OSCMessage kmsg("/keys/"); OSCMessage kmsg("/keys/");
OSCMessage quaternionMessage("/quaternion/"); OSCMessage quaternionMessage("/quaternion/");
OSCMessage quaternionDiffMessage("/quaternionDiff/"); OSCMessage quaternionDiffMessage("/quaternionDiff/");
OSCMessage eulerDiffMessage("/eulerDiff/"); OSCMessage eulerDiffMessage("eulerDiff");
OSCMessage positionMessage("/position/");
OSCMessage speedMessage("/speed/"); 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() { void setup() {
Wire.begin(); Wire.begin();
@ -107,11 +133,6 @@ void setup() {
pinMode(keys[i], INPUT_PULLUP); 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 Serial.begin(115200); // set this as high as you can reliably run on your platform
SerialBT.begin("wavey wind"); 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() { void loop() {
// if programming failed, don't try to do anything // if programming failed, don't try to do anything
if (!dmpReady) return; if (!dmpReady) return;
@ -235,27 +212,72 @@ void loop() {
quaternionMessage.add(q.x); quaternionMessage.add(q.x);
quaternionMessage.add(q.y); quaternionMessage.add(q.y);
quaternionMessage.add(q.z); 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.w);
quaternionDiffMessage.add(diff.x); quaternionDiffMessage.add(diff.x);
quaternionDiffMessage.add(diff.y); quaternionDiffMessage.add(diff.y);
quaternionDiffMessage.add(diff.z); 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); Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
eulerDiffMessage.add(eulerDiffVector(0)); eulerDiffMessage.add(eulerDiffVector(0));
eulerDiffMessage.add(eulerDiffVector(1)); eulerDiffMessage.add(eulerDiffVector(1));
eulerDiffMessage.add(eulerDiffVector(2)); eulerDiffMessage.add(eulerDiffVector(2));
SLIPBTSerial.beginPacket();
eulerDiffMessage.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
SLIPSerial.beginPacket();
eulerDiffMessage.send(SLIPSerial);
SLIPSerial.endPacket();
streamAndClearMessage(eulerDiffMessage); eulerDiffMessage.empty();
#endif #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 #ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity // display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetQuaternion(&q, fifoBuffer);
@ -285,11 +307,6 @@ void loop() {
int prevTime = timeOn; int prevTime = timeOn;
timeOn = millis(); timeOn = millis();
int elapsedTime = timeOn - prevTime; 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.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
@ -298,27 +315,33 @@ void loop() {
msg.add(AcY); msg.add(AcY);
msg.add(AcZ); msg.add(AcZ);
msg.add(elapsedTime); 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 // Send keys
for(int i = 0; i < KEYLEN; i++) { for(int i = 0; i < KEYLEN; i++) {
pressed[i] = !digitalRead(keys[i]); pressed[i] = !digitalRead(keys[i]);
kmsg.add(pressed[i]); kmsg.add(pressed[i]);
} }
streamAndClearMessage(kmsg);
SLIPSerial.beginPacket();
kmsg.send(SLIPSerial);
SLIPSerial.endPacket();
SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
kmsg.empty();
} }
} }

View File

@ -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();
}

View File

@ -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

View File

@ -3,98 +3,55 @@
#include "Wire.h" #include "Wire.h"
#include "MPU6050_6Axis_MotionApps20.h" #include "MPU6050_6Axis_MotionApps20.h"
#include <OSCBundle.h>
#include <OSCBoards.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 #ifdef BOARD_HAS_USB_SERIAL
#include <SLIPEncodedUSBSerial.h> #include <SLIPEncodedUSBSerial.h>
SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
#else #else
#include <SLIPEncodedSerial.h> #include <SLIPEncodedSerial.h>
SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that dont have Serial SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that dont have Serial
#endif #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; 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 // MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful bool dmpReady = false; // set true if DMP init was successful
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
@ -105,9 +62,6 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
// orientation/motion vars // orientation/motion vars
Quaternion q; // [w, x, y, z] quaternion container 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 aa; // [x, y, z] accel sensor measurements
VectorInt16 gy; // [x, y, z] gyro sensor measurements VectorInt16 gy; // [x, y, z] gyro sensor measurements
VectorInt16 aaReal; // [x, y, z] gravity-free accel 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 VectorFloat gravity; // [x, y, z] gravity vector
float euler[3]; // [psi, theta, phi] Euler angle container float euler[3]; // [psi, theta, phi] Euler angle container
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 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; int16_t AcX,AcY,AcZ;
float GyX, GyY, GyZ; float GyX, GyY, GyZ;
@ -132,69 +80,23 @@ byte keys[] = {16, 17, 5, 18};
byte pressed[] = {0, 0, 0, 0}; byte pressed[] = {0, 0, 0, 0};
byte KEYLEN = 4; byte KEYLEN = 4;
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) { OSCMessage msg("/accel/");
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z; OSCMessage gmsg("/gyro/");
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2; OSCMessage emsg("/error/");
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2; OSCMessage kmsg("/keys/");
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2; OSCMessage qmsg("/quaternion/");
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;
void setup() { 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.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties 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 SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform
#endif
// Keys // Keys
for(int i = 0; i < KEYLEN; i++) { for(int i = 0; i < KEYLEN; i++) {
pinMode(keys[i], INPUT_PULLUP); pinMode(keys[i], INPUT_PULLUP);
} }
// Position and speed tracking
timeOn = 0;
position.Fill(0);
speed.Fill(0);
// Start MPU
mpu.initialize(); mpu.initialize();
// Set sensitivity / range
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@ -202,24 +104,24 @@ void setup() {
devStatus = mpu.dmpInitialize(); devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity // 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.setXGyroOffset(76);
mpu.setYGyroOffset(68); mpu.setYGyroOffset(68);
mpu.setZGyroOffset(10); mpu.setZGyroOffset(10);
mpu.setXAccelOffset(-3527); mpu.setXAccelOffset(-3527);
mpu.setYAccelOffset(-913); mpu.setYAccelOffset(-913);
mpu.setZAccelOffset(1027); mpu.setZAccelOffset(1027);
*/
/* Second proto, translucent / white /* Second proto, translucent / white */
mpu.setXGyroOffset(-3650); mpu.setXGyroOffset(-3650);
mpu.setYGyroOffset(-2531); mpu.setYGyroOffset(-2531);
mpu.setZGyroOffset(1131); mpu.setZGyroOffset(1131);
mpu.setXAccelOffset(162); mpu.setXAccelOffset(162);
mpu.setYAccelOffset(-16); mpu.setYAccelOffset(-16);
mpu.setZAccelOffset(-12); mpu.setZAccelOffset(-12);
*/
// make sure it worked (returns 0 if so) // make sure it worked (returns 0 if so)
if (devStatus == 0) { if (devStatus == 0) {
@ -239,65 +141,65 @@ void setup() {
// get expected DMP packet size for later comparison // get expected DMP packet size for later comparison
packetSize = mpu.dmpGetFIFOPacketSize(); packetSize = mpu.dmpGetFIFOPacketSize();
} else { } 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! // ERROR!
// 1 = initial memory load failed // 1 = initial memory load failed
// 2 = DMP configuration updates failed // 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1) // (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() { void loop() {
// if programming failed, don't try to do anything // if programming failed, don't try to do anything
if (!dmpReady) return; if (!dmpReady) return;
// read a packet from FIFO // read a packet from FIFO
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet
// Store last Q value
pq = Quaternion(q.w,q.x,q.y,q.z); #ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
// get quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetQuaternion(&q, fifoBuffer);
q = q.getProduct(cq);
//compute the differential rotation between the previous and new orientation qmsg.add(q.w);
diff = q.getProduct(pq.getConjugate()); qmsg.add(q.x);
qmsg.add(q.y);
// Quaternion - rotacija qmsg.add(q.z);
bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y 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 #ifdef OUTPUT_READABLE_EULER
//eulerDiffVector = eulerFromQuaternion(diff); // display Euler angles in degrees
//bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z 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 #ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity // display real acceleration, adjusted to remove gravity
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
@ -309,6 +211,7 @@ void loop() {
#ifdef OUTPUT_READABLE_WORLDACCEL #ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity // display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion // and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
@ -316,67 +219,36 @@ void loop() {
AcX = aaWorld.x; AcX = aaWorld.x;
AcY = aaWorld.y; AcY = aaWorld.y;
AcZ = aaWorld.z; AcZ = aaWorld.z;
#endif #endif
// Calculate speed and position from accelerometer data // Send over serial
/* msg.add(AcX);
int prevTime = timeOn; msg.add(AcY);
timeOn = millis(); msg.add(AcZ);
int elapsedTime = timeOn - prevTime; SLIPSerial.beginPacket();
Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime}; 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 // Send keys
for(int i = 0; i < KEYLEN; i++) { for(int i = 0; i < KEYLEN; i++) {
pressed[i] = !digitalRead(keys[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(); SLIPSerial.beginPacket();
bundle.send(SLIPSerial); kmsg.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();
#endif kmsg.empty();
#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();
} }
} }

View File

@ -28,6 +28,8 @@ const IPAddress castIp = IPAddress(224,0,1,9);
const int port = 6696; const int port = 6696;
bool connected = false; bool connected = false;
//#include "AsyncUDP.h"
//AsyncUDP udp;
#include <WiFiUdp.h> #include <WiFiUdp.h>
WiFiUDP udp; WiFiUDP udp;