diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index 060f3a4..3a53a0c 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -3,9 +3,8 @@ #include "Wire.h" #include "MPU6050_6Axis_MotionApps20.h" - +#include #include -#include // POSITION CALCULATION #include @@ -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(); } }