From 3f6933a22059255fdcb2b2ff9fdae53cbce869a2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= <jurij@kompot.si>
Date: Sat, 5 Mar 2022 12:05:23 +0100
Subject: [PATCH] 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 <OSCBundle.h>
 #include <OSCBoards.h>
-#include <OSCMessage.h>
 
-//#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 <SLIPEncodedUSBSerial.h>
@@ -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();
   }
 }