From 53d957ea0f758d7b61017de0c2aaf2dbdb9ed09b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Thu, 3 Mar 2022 22:37:49 +0100 Subject: [PATCH] Bluetooth funcitonality, OSC extension, new calibration for proto --- esp32s2example/esp32s2example.ino | 53 ++++++++ osc32bt/SLIPEncodedBluetoothSerial.cpp | 181 +++++++++++++++++++++++++ osc32bt/SLIPEncodedBluetoothSerial.h | 62 +++++++++ osc32bt/osc32bt.ino | 113 ++++++--------- osc32final/osc32final.ino | 14 +- 5 files changed, 353 insertions(+), 70 deletions(-) create mode 100644 esp32s2example/esp32s2example.ino create mode 100644 osc32bt/SLIPEncodedBluetoothSerial.cpp create mode 100644 osc32bt/SLIPEncodedBluetoothSerial.h diff --git a/esp32s2example/esp32s2example.ino b/esp32s2example/esp32s2example.ino new file mode 100644 index 0000000..7fee9c3 --- /dev/null +++ b/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/ + * + */ + +#include +#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); +} diff --git a/osc32bt/SLIPEncodedBluetoothSerial.cpp b/osc32bt/SLIPEncodedBluetoothSerial.cpp new file mode 100644 index 0000000..36e5520 --- /dev/null +++ b/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(); +} + diff --git a/osc32bt/SLIPEncodedBluetoothSerial.h b/osc32bt/SLIPEncodedBluetoothSerial.h new file mode 100644 index 0000000..c7404ae --- /dev/null +++ b/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 +#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 diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino index 1b61a16..f54c507 100644 --- a/osc32bt/osc32bt.ino +++ b/osc32bt/osc32bt.ino @@ -3,33 +3,19 @@ #include "Wire.h" #include "MPU6050_6Axis_MotionApps20.h" - -#include -#include -/* -Make an OSC message and send it over serial - */ - -#ifdef BOARD_HAS_USB_SERIAL -#include -SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); -#else -#include - 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 +#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 + 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) + ")"); - SLIPSerial.beginPacket(); - emsg.send(SLIPSerial); - SLIPSerial.endPacket(); - emsg.empty(); + Serial.println("DMP Initialization failed (code " + String(devStatus) + ")"); // 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(); - - - /* - gmsg.add(GyX); - gmsg.add(GyY); - gmsg.add(GyZ); - SLIPSerial.beginPacket(); - gmsg.send(SLIPSerial); - SLIPSerial.endPacket(); - gmsg.empty(); - */ + SLIPBTSerial.beginPacket(); + msg.send(SLIPBTSerial); + SLIPBTSerial.endPacket(); + + msg.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(); } } diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index 40e1208..c028e5a 100644 --- a/osc32final/osc32final.ino +++ b/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) {