merge conflict in osc32bt resolved

osc32bt/osc32bt.ino

@@ -67,6 +67,7 @@ unsigned long timeOn;
 
 // orientation/motion vars
 Quaternion q;           // [w, x, y, z]         quaternion container
+Quaternion pq;          // [w, x, y, z]         quaternion container
 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
 VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
@@ -255,27 +256,6 @@ void loop() {
 #endif
 
 
-#ifdef OUTPUT_READABLE_EULER
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetEuler(euler, &q);
-
-    GyX = euler[0];
-    GyY = euler[1];
-    GyZ = euler[2];
-#endif
-
-#ifdef OUTPUT_READABLE_YAWPITCHROLL
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetGravity(&gravity, &q);
-    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-
-    GyX = ypr[0];
-    GyY = ypr[1];
-    GyZ = ypr[2];
-#endif
-
 #ifdef OUTPUT_READABLE_REALACCEL
     // display real acceleration, adjusted to remove gravity
     mpu.dmpGetQuaternion(&q, fifoBuffer);

osc32final/SLIPEncodedBluetoothSerial.cpp

@@ -0,0 +1,181 @@
+#include "SLIPEncodedBluetoothSerial.h"
+#include "BluetoothSerial.h"
+
+/*
+ CONSTRUCTOR
+ */
+//instantiate with the tranmission layer
+//use BluetoothSerial
+SLIPEncodedBluetoothSerial::SLIPEncodedBluetoothSerial(BluetoothSerial &s){
+	serial = &s;
+	rstate = CHAR;
+}
+
+static const uint8_t eot = 0300;
+static const uint8_t slipesc = 0333;
+static const uint8_t slipescend = 0334;
+static const uint8_t slipescesc = 0335;
+/*
+ SERIAL METHODS
+ */
+bool SLIPEncodedBluetoothSerial::endofPacket()
+{
+	if(rstate == SECONDEOT)
+	{
+		rstate = CHAR; 
+		return true;
+	}
+	if (rstate==FIRSTEOT)
+	{
+        if(serial->available())
+        {
+            uint8_t c =serial->peek();
+            if(c==eot)
+            {
+                serial->read(); // throw it on the floor
+            }
+        }
+		rstate = CHAR;
+		return true;
+	}
+	return false;
+}
+int SLIPEncodedBluetoothSerial::available(){
+back:
+	int cnt = serial->available();
+	
+	if(cnt==0)
+		return 0;
+	if(rstate==CHAR)
+	{
+		uint8_t c =serial->peek();
+		if(c==slipesc)
+		{
+			rstate = SLIPESC;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		else if( c==eot)
+		{
+			rstate = FIRSTEOT;
+			serial->read(); // throw it on the floor
+			goto back;
+		}
+		return 1; // we may have more but this is the only sure bet
+	}
+	else if(rstate==SLIPESC)	
+		return 1;
+	else if(rstate==FIRSTEOT)
+	{
+		if(serial->peek()==eot)
+		{
+			rstate = SECONDEOT;
+			serial->read(); // throw it on the floor
+			return 0;
+		}		
+		rstate = CHAR;
+	}else if (rstate==SECONDEOT) {
+		rstate = CHAR;
+	}
+	
+	return 0;
+		
+}
+
+//reads a byte from the buffer
+int SLIPEncodedBluetoothSerial::read(){
+back:
+	uint8_t c = serial->read();
+	if(rstate==CHAR)
+	{
+		if(c==slipesc)
+		{
+			rstate=SLIPESC;
+			goto back;
+		}	
+		else if(c==eot){
+		
+			return -1; // xxx this is an error
+		}
+		return c;
+	}
+	else
+	if(rstate==SLIPESC)
+	{
+		rstate=CHAR;
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+			else {
+				// insert some error code here
+				return -1;
+			}
+
+	}
+	else
+		return -1;
+}
+
+// as close as we can get to correct behavior
+int SLIPEncodedBluetoothSerial::peek(){
+	uint8_t c = serial->peek();
+	if(rstate==SLIPESC)
+	{
+		if(c==slipescend)
+			return eot;
+		else if(c==slipescesc)
+			return slipesc;
+	}
+	return c; 
+}
+
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+
+//encode SLIP
+ void SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+void SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) {  while(size--) write(*buffer++); }
+#else
+//encode SLIP
+size_t SLIPEncodedBluetoothSerial::write(uint8_t b){
+	if(b == eot){ 
+		serial->write(slipesc);
+		return serial->write(slipescend); 
+	} else if(b==slipesc) {  
+		serial->write(slipesc);
+		return serial->write(slipescesc); 
+	} else {
+		return serial->write(b);
+	}	
+}
+size_t SLIPEncodedBluetoothSerial::write(const uint8_t *buffer, size_t size) { size_t result=0; while(size--) result = write(*buffer++); return result; }
+
+#endif
+
+void SLIPEncodedBluetoothSerial::begin(String name){
+	serial->begin(name);
+}
+//SLIP specific method which begins a transmitted packet
+void SLIPEncodedBluetoothSerial::beginPacket() { 	serial->write(eot); }
+
+//signify the end of the packet with an EOT
+void SLIPEncodedBluetoothSerial::endPacket(){
+	serial->write(eot);
+
+}
+
+void SLIPEncodedBluetoothSerial::flush(){
+	serial->flush();
+}
+

osc32final/SLIPEncodedBluetoothSerial.h

@@ -0,0 +1,62 @@
+/*
+Extends the Serial class to encode SLIP over serial
+*/
+
+#ifndef SLIPEncodedBluetoothSerial_h
+#define SLIPEncodedBluetoothSerial_h
+
+
+#include "Arduino.h"
+#include <Stream.h>
+#include "BluetoothSerial.h"
+
+
+class SLIPEncodedBluetoothSerial: public Stream{
+	
+private:
+	enum erstate {CHAR, FIRSTEOT, SECONDEOT, SLIPESC } rstate;
+	
+	//the serial port used
+	BluetoothSerial * serial;
+
+	
+public:
+	
+	//the serial port used
+	SLIPEncodedBluetoothSerial(BluetoothSerial & );
+
+	
+	int available();
+	int read();
+	int peek();
+	void flush();
+	
+	//same as Serial.begin
+	void begin(String);
+    
+    //SLIP specific method which begins a transmitted packet
+	void beginPacket();
+	
+	//SLIP specific method which ends a transmittedpacket
+	void endPacket();
+	// SLIP specific method which indicates that an EOT was received 
+	bool endofPacket();
+	
+	
+//the arduino and wiring libraries have different return types for the write function
+#if defined(WIRING) || defined(BOARD_DEFS_H)
+	void write(uint8_t b);
+    void write(const uint8_t *buffer, size_t size);
+
+#else
+	//overrides the Stream's write function to encode SLIP
+	size_t write(uint8_t b);
+    size_t write(const uint8_t *buffer, size_t size);
+
+	//using Print::write;
+#endif
+
+};
+
+
+#endif

osc32final/osc32final.ino

@@ -6,52 +6,88 @@
 
 #include <OSCBoards.h>
 #include <OSCMessage.h>
-/*
-Make an OSC message and send it over serial
- */
 
+//#define SERIAL_OSC
+//#define WIFI_OSC BRIŠI
+#define BT_OSC
+
+// SERIAL
 #ifdef BOARD_HAS_USB_SERIAL
 #include <SLIPEncodedUSBSerial.h>
 SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB );
 #else
 #include <SLIPEncodedSerial.h>
- SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
+SLIPEncodedSerial SLIPSerial(Serial); // Change to Serial1 or Serial2 etc. for boards with multiple serial ports that don’t have Serial
 #endif
 
+// WIFI
+#ifdef WIFI_OSC
+#include <WiFi.h>
+  const char* ssid     = "Grajski";     // your network SSID (name of wifi network)
+  const char* password = "nedeladanes"; // your network password
+  
+  // Multicast IP / port
+  const IPAddress castIp = IPAddress(224,0,1,9);
+  const int port = 6696;
+  bool connected = false;
+
+#include <WiFiUdp.h>
+  WiFiUDP udp;
+
+  void connectToWiFi(const char * ssid, const char * pwd){
+    Serial.println("Connecting to WiFi network: " + String(ssid));
+  
+    // delete old config
+    WiFi.disconnect(true);
+    //register event handler
+    WiFi.onEvent(WiFiEvent);
+    
+    //Initiate connection
+    WiFi.begin(ssid, pwd);
+  
+    Serial.println("Waiting for WIFI connection...");
+  }
+  
+  //wifi event handler
+  void WiFiEvent(WiFiEvent_t event){
+    switch(event) {
+      case ARDUINO_EVENT_WIFI_STA_GOT_IP:
+        //When connected set 
+        Serial.print("WiFi connected! IP address: ");
+        Serial.println(WiFi.localIP());  
+        //initializes the UDP state
+        //This initializes the transfer buffer
+        udp.begin(WiFi.localIP(), port);
+        connected = true;
+        break;
+      case ARDUINO_EVENT_WIFI_STA_DISCONNECTED:
+        connected = false;
+        Serial.println("\n\n\n================\nLOST WIFI CONNECTION!\n\n\nTrying again soon...\n\n\n");
+        delay(1000);
+        connectToWiFi(ssid, password);
+        break;
+      default: break;
+    }
+  }
+#endif
+
+// Bluetooth
+#ifdef BT_OSC
+#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
+#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
+#endif
+  
+#include <SLIPEncodedSerial.h>
+#include "BluetoothSerial.h"
+#include "SLIPEncodedBluetoothSerial.h"
+  
+BluetoothSerial SerialBT;
+SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
+#endif
+
+// Motion sensor object
 MPU6050 mpu;
 
-// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
-// quaternion components in a [w, x, y, z] format (not best for parsing
-// on a remote host such as Processing or something though)
-#define OUTPUT_READABLE_QUATERNION
-
-// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
-// (in degrees) calculated from the quaternions coming from the FIFO.
-// Note that Euler angles suffer from gimbal lock (for more info, see
-// http://en.wikipedia.org/wiki/Gimbal_lock)
-//#define OUTPUT_READABLE_EULER
-
-// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
-// pitch/roll angles (in degrees) calculated from the quaternions coming
-// from the FIFO. Note this also requires gravity vector calculations.
-// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
-// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
-#define OUTPUT_READABLE_YAWPITCHROLL
-
-// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
-// components with gravity removed. This acceleration reference frame is
-// not compensated for orientation, so +X is always +X according to the
-// sensor, just without the effects of gravity. If you want acceleration
-// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
-//#define OUTPUT_READABLE_REALACCEL
-
-// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
-// components with gravity removed and adjusted for the world frame of
-// reference (yaw is relative to initial orientation, since no magnetometer
-// is present in this case). Could be quite handy in some cases.
-#define OUTPUT_READABLE_WORLDACCEL
-
-
 // MPU control/status vars
 bool dmpReady = false;  // set true if DMP init was successful
 uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
@@ -62,6 +98,8 @@ uint8_t fifoBuffer[64]; // FIFO storage buffer
 
 // orientation/motion vars
 Quaternion q;           // [w, x, y, z]         quaternion container
+Quaternion pq;          // [w, x, y, z]         previous quaternion container
+Quaternion diff;        // [w, x, y, z]         quaternion derivate container
 VectorInt16 aa;         // [x, y, z]            accel sensor measurements
 VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
@@ -70,8 +108,7 @@ 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
 
-
+// Sem dobimo vrednosti pospeskomerja in ziroskopa
-// Sem dobimo vrednosti
 int16_t AcX,AcY,AcZ;
 float GyX, GyY, GyZ;
 
@@ -80,23 +117,44 @@ byte keys[] = {16, 17, 5, 18};
 byte pressed[] = {0, 0, 0, 0};
 byte KEYLEN = 4;
 
-OSCMessage msg("/accel/");
+
-OSCMessage gmsg("/gyro/");
+/* OSC MSG channels */
-OSCMessage emsg("/error/");
+
-OSCMessage kmsg("/keys/");
+// Quaternion - rotacija
-OSCMessage qmsg("/quaternion/");
+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() {
+  // 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
+
+#ifdef SERIAL_OSC
   SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
+#endif
   
   // Keys
   for(int i = 0; i < KEYLEN; i++) {
     pinMode(keys[i], INPUT_PULLUP);
   }
 
+  // Start MPU
   mpu.initialize();
+
+  // Set sensitivity / range
   mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
   mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
 
@@ -104,7 +162,7 @@ void setup() {
   devStatus = mpu.dmpInitialize();
 
   // supply your own gyro offsets here, scaled for min sensitivity
-  // !!! Run Zero IMU to get readings
+  // !!! Run Zero IMU to get readings (read comments for instructions)
 
   /* First proto (right hand, black&blue)
   mpu.setXGyroOffset(76);
@@ -141,114 +199,162 @@ void setup() {
     // get expected DMP packet size for later comparison
     packetSize = mpu.dmpGetFIFOPacketSize();
   } else {
-    Serial.println("Error: " + String(devStatus));
+    Serial.println("DMP Initialization failed (code " + String(devStatus) + ")");
-    emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
-    SLIPSerial.beginPacket();
-    emsg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    emsg.empty();
     // ERROR!
     // 1 = initial memory load failed
     // 2 = DMP configuration updates failed
     // (if it's going to break, usually the code will be 1)
   }
+
+#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);
+    }
+#endif
+
+#ifdef BT_OSC
+    SerialBT.begin("wavey wind");
+#endif
 }
 
 void loop() {
    // if programming failed, don't try to do anything
   if (!dmpReady) return;
+  
   // read a packet from FIFO
   if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 
     
+    // Store last Q value
+    pq = Quaternion(q.w,q.x,q.y,q.z);
     
-#ifdef OUTPUT_READABLE_QUATERNION
+    // get quaternion values in easy matrix form: w x y z
-    // display quaternion values in easy matrix form: w x y z
     mpu.dmpGetQuaternion(&q, fifoBuffer);
   
+    
+    //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);
+  
+    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();
-#endif
+    qdmsg.empty();
-
-
-#ifdef OUTPUT_READABLE_EULER
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetEuler(euler, &q);
-
-    GyX = euler[0];
-    GyY = euler[1];
-    GyZ = euler[2];
-#endif
-
-#ifdef OUTPUT_READABLE_YAWPITCHROLL
-    // display Euler angles in degrees
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
-    mpu.dmpGetGravity(&gravity, &q);
-    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-
-    GyX = ypr[0];
-    GyY = ypr[1];
-    GyZ = ypr[2];
-#endif
 
 #ifdef OUTPUT_READABLE_REALACCEL
-    // display real acceleration, adjusted to remove gravity
+      // display real acceleration, adjusted to remove gravity
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
+      mpu.dmpGetAccel(&aa, fifoBuffer);
-    mpu.dmpGetAccel(&aa, fifoBuffer);
+      mpu.dmpGetGravity(&gravity, &q);
-    mpu.dmpGetGravity(&gravity, &q);
+      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
-    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+      AcX = aaReal.x;
-    AcX = aaReal.x;
+      AcY = aaReal.y;
-    AcY = aaReal.y;
+      AcZ = aaReal.z;
-    AcZ = aaReal.z;
 #endif
 
 #ifdef OUTPUT_READABLE_WORLDACCEL
-    // display initial world-frame acceleration, adjusted to remove gravity
+      // display initial world-frame acceleration, adjusted to remove gravity
-    // and rotated based on known orientation from quaternion
+      // and rotated based on known orientation from quaternion
-    mpu.dmpGetQuaternion(&q, fifoBuffer);
+      mpu.dmpGetAccel(&aa, fifoBuffer);
-    mpu.dmpGetAccel(&aa, fifoBuffer);
+      mpu.dmpGetGravity(&gravity, &q);
-    mpu.dmpGetGravity(&gravity, &q);
+      mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
-    mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+      mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
-    mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+      AcX = aaWorld.x;
-    AcX = aaWorld.x;
+      AcY = aaWorld.y;
-    AcY = aaWorld.y;
+      AcZ = aaWorld.z;
-    AcZ = aaWorld.z;
-
 #endif
 
-    // Send over serial
+    amsg.add(AcX);
-    msg.add(AcX);
+    amsg.add(AcY);
-    msg.add(AcY);
+    amsg.add(AcZ);
-    msg.add(AcZ);
-    SLIPSerial.beginPacket();
-    msg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    msg.empty();
 
+#ifdef SERIAL_OSC
+      SLIPSerial.beginPacket();
+      amsg.send(SLIPSerial);
+      SLIPSerial.endPacket();
+#endif
 
-    gmsg.add(GyX);
+#ifdef WIFI_OSC
-    gmsg.add(GyY);
+      udp.beginPacket(castIp, port);
-    gmsg.add(GyZ);
+      amsg.send(udp);
-    SLIPSerial.beginPacket();
+      udp.endPacket();
-    gmsg.send(SLIPSerial);
+#endif
-    SLIPSerial.endPacket();
-    gmsg.empty();
     
+#ifdef BT_OSC
+      SLIPBTSerial.beginPacket();
+      amsg.send(SLIPBTSerial);
+      SLIPBTSerial.endPacket();
+#endif
+    
+    amsg.empty();
     
     // Send keys
     for(int i = 0; i < KEYLEN; i++) {
       pressed[i] = !digitalRead(keys[i]);
       kmsg.add(pressed[i]);
     }
-    SLIPSerial.beginPacket();
+
-    kmsg.send(SLIPSerial);
+#ifdef SERIAL_OSC
-    SLIPSerial.endPacket();
+      SLIPSerial.beginPacket();
+      kmsg.send(SLIPSerial);
+      SLIPSerial.endPacket();
+#endif
+    
+#ifdef WIFI_OSC
+      udp.beginPacket(castIp, port);
+      kmsg.send(udp);
+      udp.endPacket();
+#endif
+    
+#ifdef BT_OSC
+      SLIPBTSerial.beginPacket();
+      kmsg.send(SLIPBTSerial);
+      SLIPBTSerial.endPacket();
+#endif
+    
     kmsg.empty();
   }
 }

osc32wifi/osc32wifi.ino

@@ -28,8 +28,6 @@ const IPAddress castIp = IPAddress(224,0,1,9);
 const int port = 6696;
 bool connected = false;
 
-//#include "AsyncUDP.h"
-//AsyncUDP udp;
 
 #include <WiFiUdp.h>
 WiFiUDP udp;