From 018887a13d57297f1e39d5e71333ee534aa71061 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Sat, 5 Mar 2022 00:49:55 +0100 Subject: [PATCH] Standradising the new final sketch --- osc32bt/osc32bt.ino | 24 +- osc32final/SLIPEncodedBluetoothSerial.cpp | 181 ++++++++++++ osc32final/SLIPEncodedBluetoothSerial.h | 62 ++++ osc32final/osc32final.ino | 336 ++++++++++++++-------- osc32wifi/osc32wifi.ino | 2 - 5 files changed, 465 insertions(+), 140 deletions(-) create mode 100644 osc32final/SLIPEncodedBluetoothSerial.cpp create mode 100644 osc32final/SLIPEncodedBluetoothSerial.h diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino index da1764d..6303ca9 100644 --- a/osc32bt/osc32bt.ino +++ b/osc32bt/osc32bt.ino @@ -62,13 +62,12 @@ unsigned long timeOn = 0; // 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 VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 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 @@ -204,27 +203,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); diff --git a/osc32final/SLIPEncodedBluetoothSerial.cpp b/osc32final/SLIPEncodedBluetoothSerial.cpp new file mode 100644 index 0000000..36e5520 --- /dev/null +++ b/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(); +} + diff --git a/osc32final/SLIPEncodedBluetoothSerial.h b/osc32final/SLIPEncodedBluetoothSerial.h new file mode 100644 index 0000000..c7404ae --- /dev/null +++ b/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 +#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/osc32final/osc32final.ino b/osc32final/osc32final.ino index 5798c6a..ec221dd 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -6,52 +6,88 @@ #include #include -/* -Make an OSC message and send it over serial - */ +//#define SERIAL_OSC +//#define WIFI_OSC +#define BT_OSC + +// 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 +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 + 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 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 +#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 +// Sem dobimo vrednosti pospeskomerja in ziroskopa 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/"); -OSCMessage emsg("/error/"); -OSCMessage kmsg("/keys/"); -OSCMessage qmsg("/quaternion/"); + +/* 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 + 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 - SLIPSerial.begin(115200); // set this as high as you can reliably run on your platform +#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)); - 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 // (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 - - -#ifdef OUTPUT_READABLE_QUATERNION - // display quaternion values in easy matrix form: w x y z + + // Store last Q value + pq = Quaternion(q.w,q.x,q.y,q.z); + + // get 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 - - -#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 + qdmsg.empty(); #ifdef OUTPUT_READABLE_REALACCEL - // display real acceleration, adjusted to remove gravity - mpu.dmpGetQuaternion(&q, fifoBuffer); - 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.dmpGetQuaternion(&q, fifoBuffer); - 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; - + // 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 - // Send over serial - msg.add(AcX); - msg.add(AcY); - msg.add(AcZ); - SLIPSerial.beginPacket(); - msg.send(SLIPSerial); - SLIPSerial.endPacket(); - msg.empty(); + amsg.add(AcX); + amsg.add(AcY); + amsg.add(AcZ); +#ifdef SERIAL_OSC + SLIPSerial.beginPacket(); + amsg.send(SLIPSerial); + SLIPSerial.endPacket(); +#endif + +#ifdef WIFI_OSC + udp.beginPacket(castIp, port); + amsg.send(udp); + udp.endPacket(); +#endif - gmsg.add(GyX); - gmsg.add(GyY); - gmsg.add(GyZ); - SLIPSerial.beginPacket(); - gmsg.send(SLIPSerial); - 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); - SLIPSerial.endPacket(); + +#ifdef SERIAL_OSC + 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(); } } diff --git a/osc32wifi/osc32wifi.ino b/osc32wifi/osc32wifi.ino index de517bb..e606d9b 100644 --- a/osc32wifi/osc32wifi.ino +++ b/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 udp;