g1smo
/
pifcamp-2021
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Manjša sprememba, quaternioni

eulerConversion
Jurij Podgoršek 2022-02-16 17:00:06 +01:00
parent ffc5422558
commit a7c0eee66b
2 changed files with 296 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

284
osc32bt/osc32bt.ino 100644
View File

@ -0,0 +1,284 @@
// ESP32 Dev Module
#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"
#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();
}
}

29
osc32final/osc32final.ino
View File

@ -1,7 +1,8 @@
// ESP32 Dev Module
#include "Wire.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <OSCBoards.h>
#include <OSCMessage.h>
@ -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();