Use bundle, cleanup, enable accel

master
Jurij Podgoršek 2022-03-05 12:05:23 +01:00
parent e4e7f3b867
commit 3f6933a220
1 changed files with 28 additions and 95 deletions

View File

@ -3,14 +3,15 @@
#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>
//#define SERIAL_OSC #define SERIAL_OSC
#define WIFI_OSC //#define WIFI_OSC
//#define BT_OSC //#define BT_OSC
#define OUTPUT_READABLE_WORLDACCEL
// SERIAL // SERIAL
#ifdef BOARD_HAS_USB_SERIAL #ifdef BOARD_HAS_USB_SERIAL
#include <SLIPEncodedUSBSerial.h> #include <SLIPEncodedUSBSerial.h>
@ -119,19 +120,7 @@ byte KEYLEN = 4;
/* OSC MSG channels */ /* OSC MSG channels */
OSCBundle bundle;
// 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
void setup() { void setup() {
// Basic(debug) serial init // Basic(debug) serial init
@ -164,22 +153,22 @@ void setup() {
// 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 (read comments for instructions)
/* 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) {
@ -242,50 +231,10 @@ void loop() {
//compute the differential rotation between the previous and new orientation //compute the differential rotation between the previous and new orientation
diff = q.getProduct(pq.getConjugate()); diff = q.getProduct(pq.getConjugate());
qmsg.add(q.w); // Quaternion - rotacija
qmsg.add(q.y); bundle.add("/quaternion").add(q.w).add(q.y).add(q.x).add(q.z); // W X Y Z
qmsg.add(q.x); // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
qmsg.add(q.z); bundle.add("/quaternionDiff").add(diff.w).add(diff.y).add(diff.x).add(diff.z); // W X Y Z
qdmsg.add(diff.w);
qdmsg.add(diff.y);
qdmsg.add(diff.x);
qdmsg.add(diff.z);
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
qmsg.send(SLIPSerial);
SLIPSerial.endPacket();
SLIPSerial.beginPacket();
qdmsg.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
// Some bug below, it seems
#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 #ifdef OUTPUT_READABLE_REALACCEL
// display real acceleration, adjusted to remove gravity // display real acceleration, adjusted to remove gravity
@ -309,54 +258,38 @@ void loop() {
AcZ = aaWorld.z; AcZ = aaWorld.z;
#endif #endif
amsg.add(AcX); // Accelerometer
amsg.add(AcY); bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z
amsg.add(AcZ);
#ifdef SERIAL_OSC // Keys held down
SLIPSerial.beginPacket(); bundle.add("/keys"); // A B C D E
amsg.send(SLIPSerial);
SLIPSerial.endPacket();
#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();
// 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]); bundle.getOSCMessage("/keys")->add(pressed[i]);
} }
#ifdef SERIAL_OSC #ifdef SERIAL_OSC
SLIPSerial.beginPacket(); SLIPSerial.beginPacket();
kmsg.send(SLIPSerial); bundle.send(SLIPSerial);
SLIPSerial.endPacket(); SLIPSerial.endPacket();
#endif #endif
#ifdef WIFI_OSC #ifdef WIFI_OSC
udp.beginPacket(castIp, port); udp.beginPacket(castIp, port);
kmsg.send(udp); bundle.send(udp);
udp.endPacket(); udp.endPacket();
#endif #endif
// Some bug below, it seems
#ifdef BT_OSC #ifdef BT_OSC
SLIPBTSerial.beginPacket(); SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial); bundle.send(SLIPBTSerial);
SLIPBTSerial.endPacket(); SLIPBTSerial.endPacket();
#endif #endif
kmsg.empty(); bundle.empty();
} }
} }