Bluetooth funcitonality, OSC extension, new calibration for proto

esp32s2example/esp32s2example.ino

@@ -0,0 +1,53 @@
+/*
+ *  Here a simple sketch to test the upload of ESP32 S2 Saola 1MI.
+ *  The addressable RGB LED (WS2812), driven by GPIO18 need
+ *  Adafruit NeoPixel library
+ *  by Mischianti Renzo <https://www.mischianti.org>
+ *
+ *  https://www.mischianti.org/
+ *
+ */
+ 
+#include <Adafruit_NeoPixel.h>
+#define PIN 18
+#define NUMPIXELS 1
+Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
+ 
+enum {NONE, RED, GREEN, BLUE};
+int ledColor = NONE;
+ 
+void setup()
+{
+  pixels.begin();
+}
+ 
+void loop()
+{
+  switch (ledColor) {
+    case NONE:
+      pixels.setPixelColor(0, pixels.Color(0, 0, 0));
+      pixels.show();
+      break;
+    case RED:
+      pixels.setPixelColor(0, pixels.Color(20, 0, 0));
+      pixels.show();
+      break;
+    case GREEN:
+      pixels.setPixelColor(0, pixels.Color(0, 20, 0));
+      pixels.show();
+      break;
+    case BLUE:
+      pixels.setPixelColor(0, pixels.Color(0, 0, 20));
+      pixels.show();
+      break;
+    default:
+      break;
+  }
+ 
+  ledColor++;
+  if (ledColor == 4) {
+    ledColor = NONE;
+  }
+ 
+  delay(1000);
+}

osc32bt/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();
+}
+

osc32bt/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

osc32bt/osc32bt.ino

@@ -3,33 +3,19 @@
 #include "Wire.h"
 #include "MPU6050_6Axis_MotionApps20.h"
 
-
-#include <OSCBoards.h>
-#include <OSCMessage.h>
-/*
-Make an OSC message and send it over 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
-#endif
-
-
-
 #include "BluetoothSerial.h"
+#include <SLIPEncodedSerial.h>
+#include "SLIPEncodedBluetoothSerial.h"
 
 #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
 #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
 #endif
 
+#include <OSCMessage.h>
+
 BluetoothSerial SerialBT;
-SLIPEncodedSerial SLIPSerial(SerialBT);
+SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
-
+SLIPEncodedSerial SLIPSerial(Serial);
-
 
 MPU6050 mpu;
 
@@ -94,7 +80,7 @@ byte pressed[] = {0, 0, 0, 0};
 byte KEYLEN = 4;
 
 OSCMessage msg("/accel/");
-// OSCMessage gmsg("/gyro/");
+OSCMessage gmsg("/gyro/");
 OSCMessage emsg("/error/");
 OSCMessage kmsg("/keys/");
 OSCMessage qmsg("/quaternion/");
@@ -102,37 +88,16 @@ OSCMessage qmsg("/quaternion/");
 void setup() {
   Wire.begin();
   Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
-  SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
 
-  /*
-  // Bluetooth serial
-  BLEDevice::init("Long name works now");
-  BLEServer *pServer = BLEDevice::createServer();
-  BLEService *pService = pServer->createService(SERVICE_UUID);
-  BLECharacteristic *pCharacteristic = pService->createCharacteristic(
-                                         CHARACTERISTIC_UUID,
-                                         BLECharacteristic::PROPERTY_READ |
-                                         BLECharacteristic::PROPERTY_WRITE
-                                       );
-
-  pCharacteristic->setValue("Hello World says Neil");
-  pService->start();
-  // BLEAdvertising *pAdvertising = pServer->getAdvertising();  // this still is working for backward compatibility
-  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
-  pAdvertising->addServiceUUID(SERVICE_UUID);
-  pAdvertising->setScanResponse(true);
-  pAdvertising->setMinPreferred(0x06);  // functions that help with iPhone connections issue
-  pAdvertising->setMinPreferred(0x12);
-  BLEDevice::startAdvertising();
-  */
-  SerialBT.begin("wavey wind");
-
-  
   // Keys
   for(int i = 0; i < KEYLEN; i++) {
     pinMode(keys[i], INPUT_PULLUP);
   }
-    
+
+  Serial.begin(115200);   // set this as high as you can reliably run on your platform
+  SerialBT.begin("wavey wind");
+
+  // Motion processor init
   mpu.initialize();
   mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
   mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@@ -142,12 +107,23 @@ void setup() {
 
   // supply your own gyro offsets here, scaled for min sensitivity
   // !!! Run Zero IMU to get readings
+
+  /* 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 */
+  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) {
@@ -167,11 +143,7 @@ void setup() {
     // get expected DMP packet size for later comparison
     packetSize = mpu.dmpGetFIFOPacketSize();
   } else {
-    emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
+    Serial.println("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
@@ -190,14 +162,21 @@ void loop() {
     // display quaternion values in easy matrix form: w x y z
     mpu.dmpGetQuaternion(&q, fifoBuffer);
     
+    //oscmsg = qOSC(q.w, q.x, q.y, q.z);
+    
     qmsg.add(q.w);
     qmsg.add(q.x);
     qmsg.add(q.y);
     qmsg.add(q.z);
+
+    SLIPBTSerial.beginPacket();
+    qmsg.send(SLIPBTSerial);
+    SLIPBTSerial.endPacket();
+    
     SLIPSerial.beginPacket();
     qmsg.send(SLIPSerial);
     SLIPSerial.endPacket();
-    qmsg.send(SerialBT);
+    
     qmsg.empty();
 #endif
 
@@ -248,37 +227,35 @@ void loop() {
 
 #endif
 
-    // Send over serial
+    // Send (accel) over serial
     msg.add(AcX);
     msg.add(AcY);
     msg.add(AcZ);
+    
     SLIPSerial.beginPacket();
     msg.send(SLIPSerial);
     SLIPSerial.endPacket();
-    msg.send(SerialBT);
-    msg.empty();
-
     
-
+    SLIPBTSerial.beginPacket();
-    /*
+    msg.send(SLIPBTSerial);
-    gmsg.add(GyX);
+    SLIPBTSerial.endPacket();
-    gmsg.add(GyY);
+    
-    gmsg.add(GyZ);
+    msg.empty();
-    SLIPSerial.beginPacket();
-    gmsg.send(SLIPSerial);
-    SLIPSerial.endPacket();
-    gmsg.empty();
-    */
     
     // Send keys
     for(int i = 0; i < KEYLEN; i++) {
       pressed[i] = !digitalRead(keys[i]);
       kmsg.add(pressed[i]);
     }
+    
     SLIPSerial.beginPacket();
     kmsg.send(SLIPSerial);
     SLIPSerial.endPacket();
-    kmsg.send(SerialBT);
+    
+    SLIPBTSerial.beginPacket();
+    kmsg.send(SLIPBTSerial);
+    SLIPBTSerial.endPacket();
+    
     kmsg.empty();
   }
 }

osc32final/osc32final.ino

@@ -92,11 +92,10 @@ void setup() {
   SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
 
   // Keys
-  
   for(int i = 0; i < KEYLEN; i++) {
     pinMode(keys[i], INPUT_PULLUP);
   }
-    
+
   mpu.initialize();
   mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
   mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@@ -106,12 +105,23 @@ void setup() {
 
   // supply your own gyro offsets here, scaled for min sensitivity
   // !!! Run Zero IMU to get readings
+
+  /* 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 */
+  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) {