// ESP32 Dev Module #include "Wire.h" #include "MPU6050_6Axis_MotionApps20.h" #include "BasicLinearAlgebra.h" #include "BluetoothSerial.h" #include #include "SLIPEncodedBluetoothSerial.h" #include #include "math.h" using namespace BLA; #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; SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT); SLIPEncodedSerial SLIPSerial(Serial); 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 unsigned long timeOn; // 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 Matrix<3> position; // [x,y,z] tracks position of device Matrix<3> speed; // [x,y,z] tracks speed of device // 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 quaternionMessage("/quaternion/"); OSCMessage quaternionDiffMessage("/quaternionDiff/"); OSCMessage eulerDiffMessage("/eulerDiff/"); OSCMessage positionMessage("/position/"); OSCMessage speedMessage("/speed/"); void setup() { Wire.begin(); Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties // Keys for(int i = 0; i < KEYLEN; i++) { pinMode(keys[i], INPUT_PULLUP); } //Set position to origin, speed to nothing, and uptime to 0 timeOn = 0; position.Fill(0); speed.Fill(0); 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); // DMP init devStatus = mpu.dmpInitialize(); // 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) { // 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 { 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) } } BLA::Matrix<3> eulerFromQuaternion(Quaternion q) { float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z; float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2; float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2; float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2; BLA::Matrix<4,4> rotationMatrix = { 1 - (yy + zz), xy + wz, xz - wy, 0, xy - wz, 1 - ( xx + zz ), yz + wx, 0, xz + wy, yz - wx, 1 - ( xx + yy ), 0, 0, 0, 0, 1 }; //TODO: test whether BLA library uses column-major matrix notation in code BLA::Matrix<3> eulerVector; eulerVector.Fill(0); eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1)); if (fabsf(rotationMatrix(1,3)) < 0.9999999) { eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3)); eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1)); } else { eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2)); eulerVector(2) = 0; } return eulerVector; } void streamAndClearMessage(OSCMessage msg) { SLIPSerial.beginPacket(); msg.send(SLIPSerial); SLIPSerial.endPacket(); SLIPBTSerial.beginPacket(); msg.send(SLIPBTSerial); SLIPBTSerial.endPacket(); msg.empty(); } float clamp(float value,float min,float max) { return fmaxf( min, fminf(max, value)); } 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 Quaternion previousQ(q.w,q.x,q.y,q.z); mpu.dmpGetQuaternion(&q, fifoBuffer); //compute the differential rotation between the previous and new orientation Quaternion diff = q.getProduct(previousQ.getConjugate()); //oscmsg = qOSC(q.w, q.x, q.y, q.z, diff.w, diff.x, diff.y, diff,z); quaternionMessage.add(q.w); quaternionMessage.add(q.x); quaternionMessage.add(q.y); quaternionMessage.add(q.z); streamAndClearMessage(quaternionMessage); quaternionDiffMessage.add(diff.w); quaternionDiffMessage.add(diff.x); quaternionDiffMessage.add(diff.y); quaternionDiffMessage.add(diff.z); streamAndClearMessage(quaternionDiffMessage); Matrix<3> eulerDiffVector = eulerFromQuaternion(diff); eulerDiffMessage.add(eulerDiffVector(0)); eulerDiffMessage.add(eulerDiffVector(1)); eulerDiffMessage.add(eulerDiffVector(2)); streamAndClearMessage(eulerDiffMessage); #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 (accel) and time elapsed over serial int prevTime = timeOn; timeOn = millis(); int elapsedTime = timeOn - prevTime; Matrix<3> speedGain = {AcX * elapsedTime, AcY * elapsedTime, AcZ * elapsedTime}; //Assume linear acceleration over measured time window, multiply time by halfpoint between last-known and current speed position = position + (((speed + speedGain) + speed) /2 * elapsedTime); speed += speedGain; mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); AcX = aaReal.x; msg.add(AcX); msg.add(AcY); msg.add(AcZ); msg.add(elapsedTime); streamAndClearMessage(msg); positionMessage.add(position(0)); positionMessage.add(position(1)); positionMessage.add(position(2)); streamAndClearMessage(positionMessage); speedMessage.add(speed(0)); speedMessage.add(speed(1)); speedMessage.add(speed(2)); streamAndClearMessage(speedMessage); // Send keys for(int i = 0; i < KEYLEN; i++) { pressed[i] = !digitalRead(keys[i]); kmsg.add(pressed[i]); } streamAndClearMessage(kmsg); } }