From a7c0eee66be4b6d193275494cc394740ca0c1ebd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jurij=20Podgor=C5=A1ek?= Date: Wed, 16 Feb 2022 17:00:06 +0100 Subject: [PATCH] =?UTF-8?q?Manj=C5=A1a=20sprememba,=20quaternioni?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- osc32bt/osc32bt.ino | 284 ++++++++++++++++++++++++++++++++++++++ osc32final/osc32final.ino | 29 ++-- 2 files changed, 296 insertions(+), 17 deletions(-) create mode 100644 osc32bt/osc32bt.ino diff --git a/osc32bt/osc32bt.ino b/osc32bt/osc32bt.ino new file mode 100644 index 0000000..1b61a16 --- /dev/null +++ b/osc32bt/osc32bt.ino @@ -0,0 +1,284 @@ +// ESP32 Dev Module + +#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" + +#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) +#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it +#endif + +BluetoothSerial SerialBT; +SLIPEncodedSerial SLIPSerial(SerialBT); + + + +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 +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [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 +int16_t AcX,AcY,AcZ; +float GyX, GyY, GyZ; + +// Keys +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/"); + +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); + } + + mpu.initialize(); + mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); + mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + + // DMP init + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + // !!! Run Zero IMU to get readings + mpu.setXGyroOffset(76); + mpu.setYGyroOffset(68); + mpu.setZGyroOffset(10); + mpu.setXAccelOffset(-3527); + mpu.setYAccelOffset(-913); + mpu.setZAccelOffset(1027); + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + //Serial.println(); + //mpu.PrintActiveOffsets(); + // turn on the DMP, now that it's ready + //Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + //Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // 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(); + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + } +} + +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 + mpu.dmpGetQuaternion(&q, fifoBuffer); + + qmsg.add(q.w); + qmsg.add(q.x); + qmsg.add(q.y); + qmsg.add(q.z); + SLIPSerial.beginPacket(); + qmsg.send(SLIPSerial); + SLIPSerial.endPacket(); + qmsg.send(SerialBT); + 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 + +#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; +#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; + +#endif + + // Send 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(); + */ + + // 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); + kmsg.empty(); + } +} diff --git a/osc32final/osc32final.ino b/osc32final/osc32final.ino index 88d0968..40e1208 100644 --- a/osc32final/osc32final.ino +++ b/osc32final/osc32final.ino @@ -1,7 +1,8 @@ // ESP32 Dev Module #include "Wire.h" -#include "MPU6050_6Axis_MotionApps_V6_12.h" +#include "MPU6050_6Axis_MotionApps20.h" + #include #include @@ -22,20 +23,20 @@ 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 +#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 +//#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 +#define OUTPUT_READABLE_YAWPITCHROLL // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration // components with gravity removed. This acceleration reference frame is @@ -97,8 +98,8 @@ void setup() { } mpu.initialize(); -// setFullScaleGyroRange(MPU6050_GYRO_FS_250); -// setFullScaleAccelRange(MPU6050_ACCEL_FS_2); + mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250); + mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // DMP init devStatus = mpu.dmpInitialize(); @@ -118,7 +119,7 @@ void setup() { mpu.CalibrateAccel(6); mpu.CalibrateGyro(6); //Serial.println(); - mpu.PrintActiveOffsets(); + //mpu.PrintActiveOffsets(); // turn on the DMP, now that it's ready //Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); @@ -131,14 +132,10 @@ void setup() { packetSize = mpu.dmpGetFIFOPacketSize(); } else { emsg.add("DMP Initialization failed (code " + String(devStatus) + ")"); - - - SLIPSerial.beginPacket(); emsg.send(SLIPSerial); SLIPSerial.endPacket(); emsg.empty(); - //sendSerial(emsg); // ERROR! // 1 = initial memory load failed // 2 = DMP configuration updates failed @@ -161,7 +158,6 @@ void loop() { qmsg.add(q.x); qmsg.add(q.y); qmsg.add(q.z); - //sendSerial(msg); SLIPSerial.beginPacket(); qmsg.send(SLIPSerial); SLIPSerial.endPacket(); @@ -219,27 +215,26 @@ void loop() { msg.add(AcX); msg.add(AcY); msg.add(AcZ); - //sendSerial(msg); SLIPSerial.beginPacket(); msg.send(SLIPSerial); SLIPSerial.endPacket(); msg.empty(); - + + gmsg.add(GyX); gmsg.add(GyY); gmsg.add(GyZ); - //sendSerial(gmsg); 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]); } - //sendSerial(kmsg); SLIPSerial.beginPacket(); kmsg.send(SLIPSerial); SLIPSerial.endPacket();