225 lines
7.2 KiB
Arduino
225 lines
7.2 KiB
Arduino
|
// ESP32 Dev Module
|
|||
|
|
|||
|
#include "Wire.h"
|
|||
|
#include "MPU6050_6Axis_MotionApps_V6_12.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
|
|||
|
|
|||
|
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;
|
|||
|
|
|||
|
|
|||
|
OSCMessage msg("/accel/");
|
|||
|
OSCMessage gmsg("/gyro/");
|
|||
|
OSCMessage emsg("/error/");
|
|||
|
|
|||
|
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
|
|||
|
|
|||
|
mpu.initialize();
|
|||
|
// setFullScaleGyroRange(MPU6050_GYRO_FS_250);
|
|||
|
// 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 {
|
|||
|
|
|||
|
OSCMessage emsg("/error/");
|
|||
|
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)
|
|||
|
}
|
|||
|
/*
|
|||
|
SerialBT.begin("MotionGlove");
|
|||
|
*/
|
|||
|
}
|
|||
|
|
|||
|
void sendSerial(OSCMessage msg) {
|
|||
|
SLIPSerial.beginPacket();
|
|||
|
msg.send(SLIPSerial);
|
|||
|
SLIPSerial.endPacket();
|
|||
|
msg.empty();
|
|||
|
}
|
|||
|
|
|||
|
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);
|
|||
|
Serial.print("quat\t");
|
|||
|
Serial.print(q.w);
|
|||
|
Serial.print("\t");
|
|||
|
Serial.print(q.x);
|
|||
|
Serial.print("\t");
|
|||
|
Serial.print(q.y);
|
|||
|
Serial.print("\t");
|
|||
|
Serial.println(q.z);
|
|||
|
#endif
|
|||
|
*/
|
|||
|
|
|||
|
#ifdef OUTPUT_READABLE_EULER
|
|||
|
// display Euler angles in degrees
|
|||
|
mpu.dmpGetQuaternion(&q, fifoBuffer);
|
|||
|
mpu.dmpGetEuler(euler, &q);
|
|||
|
|
|||
|
GyX = euler[0]/* * 180 / M_PI*/;
|
|||
|
GyY = euler[1]/* * 180 / M_PI*/;
|
|||
|
GyZ = euler[2]/* * 180 / M_PI*/;
|
|||
|
#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]/* * 180 / M_PI*/;
|
|||
|
GyY = ypr[1]/* * 180 / M_PI*/;
|
|||
|
GyZ = ypr[2]/* * 180 / M_PI*/;
|
|||
|
#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);
|
|||
|
sendSerial(msg);
|
|||
|
|
|||
|
gmsg.add(GyX);
|
|||
|
gmsg.add(GyY);
|
|||
|
gmsg.add(GyZ);
|
|||
|
sendSerial(gmsg);
|
|||
|
}
|
|||
|
}
|