Added position and speed tracking and messaging

osc32final/osc32final.ino

@@ -7,6 +7,12 @@
 #include <OSCBoards.h>
 #include <OSCMessage.h>
 
+// POSITION CALCULATION
+#include <BasicLinearAlgebra.h>
+#include "math.h"
+
+using namespace BLA;
+
 //#define SERIAL_OSC
 //#define WIFI_OSC BRIŠI
 #define BT_OSC
@@ -108,7 +114,8 @@ VectorFloat gravity;    // [x, y, z]            gravity vector
 float euler[3];         // [psi, theta, phi]    Euler angle container
 float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 uint32_t timeOn = 0;    //                      Uptime counter for movement calculation
-
+Matrix<3> position;     // [x,y,z]              tracks position of device
+Matrix<3> speed;        // [x,y,z]              tracks speed of device
 
 // Sem dobimo vrednosti pospeskomerja in ziroskopa
 int16_t AcX,AcY,AcZ;
@@ -134,6 +141,9 @@ 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/");
 
 void setup() {
   // Basic(debug) serial init
@@ -152,6 +162,12 @@ void setup() {
   for(int i = 0; i < KEYLEN; i++) {
     pinMode(keys[i], INPUT_PULLUP);
   }
+   
+  // Position and speed tracking
+
+  timeOn = 0;
+  position.Fill(0);
+  speed.Fill(0);
 
   // Start MPU
   mpu.initialize();
@@ -308,7 +324,6 @@ void loop() {
       AcY = aaWorld.y;
       AcZ = aaWorld.z;
 #endif
-
     amsg.add(AcX);
     amsg.add(AcY);
     amsg.add(AcZ);
@@ -332,6 +347,25 @@ void loop() {
 #endif
     
     amsg.empty();
+
+    // Calculate speed and position from accelerometer data
+    int prevTime = timeOn;
+    timeOn = millis();
+    int elapsedTime = timeOn - prevTime;
+    Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime};
+    
+    //Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed
+    position = position + (((speed + speedGain) + speed) /2 * elapsedTime);
+    speed += speedGain;
+
+    positionMessage.add(position(0));
+    positionMessage.add(position(1));
+    positionMessage.add(position(2));
+
+    speedMessage.add(speed(0));
+    speedMessage.add(speed(1));
+    speedMessage.add(speed(2));
+
     
     // Send keys
     for(int i = 0; i < KEYLEN; i++) {
@@ -343,20 +377,45 @@ void loop() {
       SLIPSerial.beginPacket();
       kmsg.send(SLIPSerial);
       SLIPSerial.endPacket();
+      
+      SLIPSerial.beginPacket();
+      positionMessage.send(SLIPSerial);
+      SLIPSerial.endPacket();
+      
+      SLIPSerial.beginPacket();
+      speedMessage.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();
 #endif
     
 #ifdef BT_OSC
       SLIPBTSerial.beginPacket();
       kmsg.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();
   }
 }