integrated speed and position into bundled messaging
Martin
commit
0bedee7d4b
|
|
@ -3,9 +3,8 @@
|
|||
#include "Wire.h"
|
||||
#include "MPU6050_6Axis_MotionApps20.h"
|
||||
|
||||
|
||||
#include <OSCBundle.h>
|
||||
#include <OSCBoards.h>
|
||||
#include <OSCMessage.h>
|
||||
|
||||
// POSITION CALCULATION
|
||||
#include <BasicLinearAlgebra.h>
|
||||
|
|
@ -13,9 +12,11 @@
|
|||
|
||||
using namespace BLA;
|
||||
|
||||
//#define SERIAL_OSC
|
||||
//#define WIFI_OSC BRIŠI
|
||||
#define BT_OSC
|
||||
#define SERIAL_OSC
|
||||
//#define WIFI_OSC
|
||||
//#define BT_OSC
|
||||
|
||||
#define OUTPUT_READABLE_WORLDACCEL
|
||||
|
||||
// SERIAL
|
||||
#ifdef BOARD_HAS_USB_SERIAL
|
||||
|
|
@ -128,28 +129,13 @@ byte KEYLEN = 4;
|
|||
|
||||
|
||||
/* OSC MSG channels */
|
||||
|
||||
// Quaternion - rotacija
|
||||
OSCMessage qmsg("/quaternion/"); // W X Y Z
|
||||
|
||||
// Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
|
||||
OSCMessage qdmsg("/quaternionDiff/"); // W X Y Z
|
||||
|
||||
// Accelerometer
|
||||
OSCMessage amsg("/accel/"); // X Y Z
|
||||
|
||||
// Keys held down
|
||||
OSCMessage kmsg("/keys/"); // A B C D E
|
||||
|
||||
// Position and speed
|
||||
OSCMessage positionMessage("/position/");
|
||||
OSCMessage speedMessage("/speed/");
|
||||
OSCBundle bundle;
|
||||
|
||||
void setup() {
|
||||
// Basic(debug) serial init
|
||||
// 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
|
||||
Serial.println("Starting up...");
|
||||
|
||||
|
||||
// I2C init
|
||||
Wire.begin();
|
||||
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
||||
|
|
@ -182,22 +168,22 @@ void setup() {
|
|||
// supply your own gyro offsets here, scaled for min sensitivity
|
||||
// !!! Run Zero IMU to get readings (read comments for instructions)
|
||||
|
||||
/* 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) {
|
||||
|
|
@ -225,21 +211,21 @@ void setup() {
|
|||
}
|
||||
|
||||
#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);
|
||||
}
|
||||
// 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");
|
||||
SerialBT.begin("wavey wind");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -260,95 +246,34 @@ void loop() {
|
|||
//compute the differential rotation between the previous and new orientation
|
||||
diff = q.getProduct(pq.getConjugate());
|
||||
|
||||
qmsg.add(q.w);
|
||||
qmsg.add(q.x);
|
||||
qmsg.add(q.y);
|
||||
qmsg.add(q.z);
|
||||
// Quaternion - rotacija
|
||||
bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W 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
|
||||
|
||||
qdmsg.add(diff.w);
|
||||
qdmsg.add(diff.x);
|
||||
qdmsg.add(diff.y);
|
||||
qdmsg.add(diff.z);
|
||||
|
||||
#ifdef SERIAL_OSC
|
||||
SLIPSerial.beginPacket();
|
||||
qdmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
qmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
#endif
|
||||
|
||||
#ifdef WIFI_OSC
|
||||
udp.beginPacket(castIp, port);
|
||||
qmsg.send(udp);
|
||||
udp.endPacket();
|
||||
|
||||
udp.beginPacket(castIp, port);
|
||||
qdmsg.send(udp);
|
||||
udp.endPacket();
|
||||
#endif
|
||||
|
||||
#ifdef BT_OSC
|
||||
SLIPBTSerial.beginPacket();
|
||||
qmsg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
qdmsg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
#endif
|
||||
|
||||
qmsg.empty();
|
||||
qdmsg.empty();
|
||||
|
||||
#ifdef OUTPUT_READABLE_REALACCEL
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
AcX = aaReal.x;
|
||||
AcY = aaReal.y;
|
||||
AcZ = aaReal.z;
|
||||
// display real acceleration, adjusted to remove gravity
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
AcX = aaReal.x;
|
||||
AcY = aaReal.y;
|
||||
AcZ = aaReal.z;
|
||||
#endif
|
||||
|
||||
#ifdef OUTPUT_READABLE_WORLDACCEL
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
AcX = aaWorld.x;
|
||||
AcY = aaWorld.y;
|
||||
AcZ = aaWorld.z;
|
||||
#endif
|
||||
amsg.add(AcX);
|
||||
amsg.add(AcY);
|
||||
amsg.add(AcZ);
|
||||
|
||||
#ifdef SERIAL_OSC
|
||||
SLIPSerial.beginPacket();
|
||||
amsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
// display initial world-frame acceleration, adjusted to remove gravity
|
||||
// and rotated based on known orientation from quaternion
|
||||
mpu.dmpGetAccel(&aa, fifoBuffer);
|
||||
mpu.dmpGetGravity(&gravity, &q);
|
||||
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
|
||||
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
|
||||
AcX = aaWorld.x;
|
||||
AcY = aaWorld.y;
|
||||
AcZ = aaWorld.z;
|
||||
#endif
|
||||
|
||||
#ifdef WIFI_OSC
|
||||
udp.beginPacket(castIp, port);
|
||||
amsg.send(udp);
|
||||
udp.endPacket();
|
||||
#endif
|
||||
|
||||
#ifdef BT_OSC
|
||||
SLIPBTSerial.beginPacket();
|
||||
amsg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
#endif
|
||||
|
||||
amsg.empty();
|
||||
|
||||
// Calculate speed and position from accelerometer data
|
||||
// Calculate speed and position from accelerometer data
|
||||
int prevTime = timeOn;
|
||||
timeOn = millis();
|
||||
int elapsedTime = timeOn - prevTime;
|
||||
|
|
@ -358,64 +283,41 @@ void loop() {
|
|||
position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
|
||||
speed += speedGain;
|
||||
|
||||
positionMessage.add(position(0));
|
||||
positionMessage.add(position(1));
|
||||
positionMessage.add(position(2));
|
||||
bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
|
||||
bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
|
||||
|
||||
speedMessage.add(speed(0));
|
||||
speedMessage.add(speed(1));
|
||||
speedMessage.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]);
|
||||
kmsg.add(pressed[i]);
|
||||
bundle.getOSCMessage("/keys")->add(pressed[i]);
|
||||
}
|
||||
|
||||
|
||||
#ifdef SERIAL_OSC
|
||||
SLIPSerial.beginPacket();
|
||||
kmsg.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
positionMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
|
||||
SLIPSerial.beginPacket();
|
||||
speedMessage.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
SLIPSerial.beginPacket();
|
||||
bundle.send(SLIPSerial);
|
||||
SLIPSerial.endPacket();
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef WIFI_OSC
|
||||
udp.beginPacket(castIp, port);
|
||||
kmsg.send(udp);
|
||||
udp.endPacket();
|
||||
|
||||
udp.beginPacket(castIp, port);
|
||||
positionMessage.send(udp);
|
||||
udp.endPacket();
|
||||
|
||||
udp.beginPacket(castIp, port);
|
||||
speedMessage.send(udp);
|
||||
udp.endPacket();
|
||||
udp.beginPacket(castIp, port);
|
||||
bundle.send(udp);
|
||||
udp.endPacket();
|
||||
#endif
|
||||
|
||||
|
||||
// Some bug below, it seems
|
||||
#ifdef BT_OSC
|
||||
SLIPBTSerial.beginPacket();
|
||||
kmsg.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
SLIPBTSerial.beginPacket();
|
||||
bundle.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
#endif
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
positionMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
|
||||
SLIPBTSerial.beginPacket();
|
||||
speedMessage.send(SLIPBTSerial);
|
||||
SLIPBTSerial.endPacket();
|
||||
#endif
|
||||
kmsg.empty();
|
||||
positionMessage.empty();
|
||||
speedMessage.empty();
|
||||
bundle.empty();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue