Compare commits
8 Commits
a6469f57a6
...
0bedee7d4b
Author | SHA1 | Date |
---|---|---|
Martin | 0bedee7d4b | |
Martin | 2f0e7a6b7f | |
Martin | d55edf3271 | |
Martin | 63990f2242 | |
Martin | 22c3ce731b | |
Martin | 132ce51f2f | |
Martin | 21b5674b27 | |
Martin | 5d24919125 |
|
@ -2,10 +2,15 @@
|
||||||
|
|
||||||
#include "Wire.h"
|
#include "Wire.h"
|
||||||
#include "MPU6050_6Axis_MotionApps20.h"
|
#include "MPU6050_6Axis_MotionApps20.h"
|
||||||
|
#include "BasicLinearAlgebra.h"
|
||||||
#include "BluetoothSerial.h"
|
#include "BluetoothSerial.h"
|
||||||
#include <SLIPEncodedSerial.h>
|
#include <SLIPEncodedSerial.h>
|
||||||
#include "SLIPEncodedBluetoothSerial.h"
|
#include "SLIPEncodedBluetoothSerial.h"
|
||||||
|
#include <BasicLinearAlgebra.h>
|
||||||
|
#include "math.h"
|
||||||
|
|
||||||
|
using namespace BLA;
|
||||||
|
|
||||||
|
|
||||||
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
|
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
|
||||||
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
|
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
|
||||||
|
@ -58,7 +63,7 @@ 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 = 0;
|
unsigned long timeOn;
|
||||||
|
|
||||||
// orientation/motion vars
|
// orientation/motion vars
|
||||||
Quaternion q; // [w, x, y, z] quaternion container
|
Quaternion q; // [w, x, y, z] quaternion container
|
||||||
|
@ -68,6 +73,10 @@ 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
|
||||||
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
|
||||||
VectorFloat gravity; // [x, y, z] gravity vector
|
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
|
// Sem dobimo vrednosti
|
||||||
|
@ -85,6 +94,9 @@ 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 positionMessage("/position/");
|
||||||
|
OSCMessage speedMessage("/speed/");
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
Wire.begin();
|
Wire.begin();
|
||||||
|
@ -95,6 +107,11 @@ 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");
|
||||||
|
|
||||||
|
@ -152,6 +169,50 @@ 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;
|
||||||
|
@ -175,30 +236,22 @@ void loop() {
|
||||||
quaternionMessage.add(q.y);
|
quaternionMessage.add(q.y);
|
||||||
quaternionMessage.add(q.z);
|
quaternionMessage.add(q.z);
|
||||||
|
|
||||||
SLIPBTSerial.beginPacket();
|
streamAndClearMessage(quaternionMessage);
|
||||||
quaternionMessage.send(SLIPBTSerial);
|
|
||||||
SLIPBTSerial.endPacket();
|
|
||||||
|
|
||||||
SLIPSerial.beginPacket();
|
|
||||||
quaternionMessage.send(SLIPSerial);
|
|
||||||
SLIPSerial.endPacket();
|
|
||||||
|
|
||||||
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();
|
streamAndClearMessage(quaternionDiffMessage);
|
||||||
quaternionDiffMessage.send(SLIPBTSerial);
|
|
||||||
SLIPBTSerial.endPacket();
|
|
||||||
|
|
||||||
SLIPSerial.beginPacket();
|
Matrix<3> eulerDiffVector = eulerFromQuaternion(diff);
|
||||||
quaternionDiffMessage.send(SLIPSerial);
|
|
||||||
SLIPSerial.endPacket();
|
|
||||||
|
|
||||||
quaternionDiffMessage.empty();
|
eulerDiffMessage.add(eulerDiffVector(0));
|
||||||
|
eulerDiffMessage.add(eulerDiffVector(1));
|
||||||
|
eulerDiffMessage.add(eulerDiffVector(2));
|
||||||
|
|
||||||
|
streamAndClearMessage(eulerDiffMessage);
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -232,6 +285,11 @@ 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);
|
||||||
|
@ -241,31 +299,26 @@ void loop() {
|
||||||
msg.add(AcZ);
|
msg.add(AcZ);
|
||||||
msg.add(elapsedTime);
|
msg.add(elapsedTime);
|
||||||
|
|
||||||
|
streamAndClearMessage(msg);
|
||||||
|
|
||||||
SLIPSerial.beginPacket();
|
positionMessage.add(position(0));
|
||||||
msg.send(SLIPSerial);
|
positionMessage.add(position(1));
|
||||||
SLIPSerial.endPacket();
|
positionMessage.add(position(2));
|
||||||
|
|
||||||
SLIPBTSerial.beginPacket();
|
streamAndClearMessage(positionMessage);
|
||||||
msg.send(SLIPBTSerial);
|
|
||||||
SLIPBTSerial.endPacket();
|
speedMessage.add(speed(0));
|
||||||
|
speedMessage.add(speed(1));
|
||||||
|
speedMessage.add(speed(2));
|
||||||
|
|
||||||
|
streamAndClearMessage(speedMessage);
|
||||||
|
|
||||||
msg.empty();
|
|
||||||
|
|
||||||
// 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();
|
|
||||||
}
|
}
|
||||||
}
|
}
|
|
@ -6,6 +6,12 @@
|
||||||
#include <OSCBundle.h>
|
#include <OSCBundle.h>
|
||||||
#include <OSCBoards.h>
|
#include <OSCBoards.h>
|
||||||
|
|
||||||
|
// POSITION CALCULATION
|
||||||
|
#include <BasicLinearAlgebra.h>
|
||||||
|
#include "math.h"
|
||||||
|
|
||||||
|
using namespace BLA;
|
||||||
|
|
||||||
#define SERIAL_OSC
|
#define SERIAL_OSC
|
||||||
//#define WIFI_OSC
|
//#define WIFI_OSC
|
||||||
//#define BT_OSC
|
//#define BT_OSC
|
||||||
|
@ -108,6 +114,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
|
||||||
|
|
||||||
// Sem dobimo vrednosti pospeskomerja in ziroskopa
|
// Sem dobimo vrednosti pospeskomerja in ziroskopa
|
||||||
int16_t AcX,AcY,AcZ;
|
int16_t AcX,AcY,AcZ;
|
||||||
|
@ -140,6 +149,12 @@ void setup() {
|
||||||
pinMode(keys[i], INPUT_PULLUP);
|
pinMode(keys[i], INPUT_PULLUP);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Position and speed tracking
|
||||||
|
|
||||||
|
timeOn = 0;
|
||||||
|
position.Fill(0);
|
||||||
|
speed.Fill(0);
|
||||||
|
|
||||||
// Start MPU
|
// Start MPU
|
||||||
mpu.initialize();
|
mpu.initialize();
|
||||||
|
|
||||||
|
@ -258,6 +273,19 @@ void loop() {
|
||||||
AcZ = aaWorld.z;
|
AcZ = aaWorld.z;
|
||||||
#endif
|
#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};
|
||||||
|
|
||||||
|
//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
|
// Accelerometer
|
||||||
bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
|
bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue