From e4e7f3b86764c44d429f033d43fb8d4d72bdd2e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Sat, 5 Mar 2022 02:10:27 +0100 Subject: [PATCH 1/3] Cleanup --- osc32final/osc32final.ino | 126 +++++++++++++++++++------------------- 1 file changed, 64 insertions(+), 62 deletions(-) diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index ec221dd..19a73e1 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -8,8 +8,8 @@ #include //#define SERIAL_OSC -//#define WIFI_OSC -#define BT_OSC +#define WIFI_OSC +//#define BT_OSC // SERIAL #ifdef BOARD_HAS_USB_SERIAL @@ -135,7 +135,7 @@ OSCMessage kmsg("/keys/"); // A B C D E 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 @@ -207,21 +207,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 } @@ -243,24 +243,25 @@ void loop() { diff = q.getProduct(pq.getConjugate()); qmsg.add(q.w); - qmsg.add(q.x); qmsg.add(q.y); + qmsg.add(q.x); qmsg.add(q.z); - + qdmsg.add(diff.w); - qdmsg.add(diff.x); qdmsg.add(diff.y); + qdmsg.add(diff.x); qdmsg.add(diff.z); #ifdef SERIAL_OSC SLIPSerial.beginPacket(); - qdmsg.send(SLIPSerial); + qmsg.send(SLIPSerial); SLIPSerial.endPacket(); SLIPSerial.beginPacket(); - qmsg.send(SLIPSerial); + qdmsg.send(SLIPSerial); SLIPSerial.endPacket(); #endif + #ifdef WIFI_OSC udp.beginPacket(castIp, port); @@ -271,40 +272,41 @@ void loop() { 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(); + 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; + // 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); @@ -312,21 +314,21 @@ void loop() { amsg.add(AcZ); #ifdef SERIAL_OSC - SLIPSerial.beginPacket(); - amsg.send(SLIPSerial); - SLIPSerial.endPacket(); + SLIPSerial.beginPacket(); + amsg.send(SLIPSerial); + SLIPSerial.endPacket(); #endif #ifdef WIFI_OSC - udp.beginPacket(castIp, port); - amsg.send(udp); - udp.endPacket(); + udp.beginPacket(castIp, port); + amsg.send(udp); + udp.endPacket(); #endif #ifdef BT_OSC - SLIPBTSerial.beginPacket(); - amsg.send(SLIPBTSerial); - SLIPBTSerial.endPacket(); + SLIPBTSerial.beginPacket(); + amsg.send(SLIPBTSerial); + SLIPBTSerial.endPacket(); #endif amsg.empty(); @@ -338,21 +340,21 @@ void loop() { } #ifdef SERIAL_OSC - SLIPSerial.beginPacket(); - kmsg.send(SLIPSerial); - SLIPSerial.endPacket(); + SLIPSerial.beginPacket(); + kmsg.send(SLIPSerial); + SLIPSerial.endPacket(); #endif #ifdef WIFI_OSC - udp.beginPacket(castIp, port); - kmsg.send(udp); - udp.endPacket(); + udp.beginPacket(castIp, port); + kmsg.send(udp); + udp.endPacket(); #endif #ifdef BT_OSC - SLIPBTSerial.beginPacket(); - kmsg.send(SLIPBTSerial); - SLIPBTSerial.endPacket(); + SLIPBTSerial.beginPacket(); + kmsg.send(SLIPBTSerial); + SLIPBTSerial.endPacket(); #endif kmsg.empty(); From 3f6933a22059255fdcb2b2ff9fdae53cbce869a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Sat, 5 Mar 2022 12:05:23 +0100 Subject: [PATCH 2/3] Use bundle, cleanup, enable accel --- osc32final/osc32final.ino | 123 +++++++++----------------------------- 1 file changed, 28 insertions(+), 95 deletions(-) diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index 19a73e1..0ef3202 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -3,14 +3,15 @@ #include "Wire.h" #include "MPU6050_6Axis_MotionApps20.h" - +#include #include -#include -//#define SERIAL_OSC -#define WIFI_OSC +#define SERIAL_OSC +//#define WIFI_OSC //#define BT_OSC +#define OUTPUT_READABLE_WORLDACCEL + // SERIAL #ifdef BOARD_HAS_USB_SERIAL #include @@ -119,25 +120,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 - +OSCBundle bundle; 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 @@ -164,22 +153,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) { @@ -242,51 +231,11 @@ void loop() { //compute the differential rotation between the previous and new orientation diff = q.getProduct(pq.getConjugate()); - qmsg.add(q.w); - qmsg.add(q.y); - qmsg.add(q.x); - qmsg.add(q.z); - - qdmsg.add(diff.w); - qdmsg.add(diff.y); - qdmsg.add(diff.x); - qdmsg.add(diff.z); + // Quaternion - rotacija + bundle.add("/quaternion").add(q.w).add(q.y).add(q.x).add(q.z); // W X Y Z + // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading) + bundle.add("/quaternionDiff").add(diff.w).add(diff.y).add(diff.x).add(diff.z); // W X Y 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 // display real acceleration, adjusted to remove gravity mpu.dmpGetAccel(&aa, fifoBuffer); @@ -309,54 +258,38 @@ void loop() { AcZ = aaWorld.z; #endif - amsg.add(AcX); - amsg.add(AcY); - amsg.add(AcZ); + // Accelerometer + bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z -#ifdef SERIAL_OSC - SLIPSerial.beginPacket(); - 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(); + // 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); + bundle.send(SLIPSerial); SLIPSerial.endPacket(); #endif + #ifdef WIFI_OSC udp.beginPacket(castIp, port); - kmsg.send(udp); + bundle.send(udp); udp.endPacket(); #endif - + +// Some bug below, it seems #ifdef BT_OSC SLIPBTSerial.beginPacket(); - kmsg.send(SLIPBTSerial); + bundle.send(SLIPBTSerial); SLIPBTSerial.endPacket(); #endif - - kmsg.empty(); + + bundle.empty(); } } From a6469f57a6f1b5386d7e708535d72c59a407af53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Sat, 5 Mar 2022 12:20:49 +0100 Subject: [PATCH 3/3] Fix the quaternion parameters --- osc32final/osc32final.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index 0ef3202..83bdf85 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -232,9 +232,9 @@ void loop() { diff = q.getProduct(pq.getConjugate()); // Quaternion - rotacija - bundle.add("/quaternion").add(q.w).add(q.y).add(q.x).add(q.z); // W X Y Z + 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).add(diff.x).add(diff.z); // W X Y Z + bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity