pifcamp-2021/osc32final/osc32final.ino

// ESP32 Dev Module

#include "Wire.h"
#include "MPU6050_6Axis_MotionApps20.h"

#include <OSCBundle.h>
#include <OSCBoards.h>

// POSITION CALCULATION
#include <BasicLinearAlgebra.h>
#include "math.h"

using namespace BLA;

#define SERIAL_OSC
//#define WIFI_OSC
#define BT_OSC

#define OUTPUT_READABLE_WORLDACCEL

// 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

// WIFI
#ifdef WIFI_OSC
#include <WiFi.h>
  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.h>
  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 <SLIPEncodedSerial.h>
#include "BluetoothSerial.h"
#include "SLIPEncodedBluetoothSerial.h"
  
BluetoothSerial SerialBT;
SLIPEncodedBluetoothSerial SLIPBTSerial(SerialBT);
#endif

// Motion sensor object
MPU6050 mpu;

// 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
Quaternion pq;          // [w, x, y, z]         previous quaternion container
Quaternion diff;        // [w, x, y, z]         quaternion derivate container
Quaternion cq;          // [w, x, y, z]         calibration quaternion
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
uint32_t timeOn = 0;    //                      Uptime counter for movement calculation
Matrix<3> position;     // [x,y,z]              tracks position of device
Matrix<3> speed;        // [x,y,z]              tracks speed of device
Matrix<3> eulerVector;
Matrix<3> eulerDiffVector;
bool reset; // For quaternion calibration


// Sem dobimo vrednosti pospeskomerja in ziroskopa
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;

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;
}

float clamp(float value,float min,float max) {
  return fmaxf( min, fminf(max, value));
}


/* OSC MSG channels */
OSCBundle bundle;

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

#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);
  }
   
  // Position and speed tracking

  timeOn = 0;
  position.Fill(0);
  speed.Fill(0);

  // Start MPU
  mpu.initialize();

  // Set sensitivity / range
  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 (read comments for instructions)

  /* 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)
  }

#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 
    
    // 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);
    q = q.getProduct(cq);
    
    //compute the differential rotation between the previous and new orientation
    diff = q.getProduct(pq.getConjugate());
    
    // Quaternion - rotacija
    bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z

    // Euler - rotacija
    //eulerVector = eulerFromQuaternion(q);
    //bundle.add("/euler").add(eulerVector(0)).add(eulerVector(1)).add(eulerVector(2)); // X Y Z
    
    // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
    bundle.add("/quaternionDiff").add(diff.w).add(diff.y * -1).add(diff.z).add(diff.x * -1); // W X Y Z

    // Rotation diff value in euler angle
    //eulerDiffVector = eulerFromQuaternion(diff);
    //bundle.add("/eulerDiff").add(eulerDiffVector(0)).add(eulerDiffVector(1)).add(eulerDiffVector(2)); // X Y Z
    
#ifdef OUTPUT_READABLE_REALACCEL
    // 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.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

   // Calculate speed and position from accelerometer data
   /*
    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;

    bundle.add("/position/").add(position(0)).add(position(1)).add(position(2));
    bundle.add("/speed/").add(speed(0)).add(speed(1)).add(speed(2));
*/
    // Accelerometer
    bundle.add("/accel").add(AcX).add(AcY).add(AcZ); ; // X Y Z

    // Keys held down
    bundle.add("/keys"); // A B C D E
    
    // Send keys
    for(int i = 0; i < KEYLEN; i++) {
      pressed[i] = !digitalRead(keys[i]);
      bundle.getOSCMessage("/keys")->add(pressed[i]);
    }

    // Reset calibration euler?
    if (pressed[0] && pressed[1] && pressed[2] && pressed[3]) {
      if (!reset) {
        cq = q.getConjugate();
        reset = true;
        Serial.println("Quaternion calibrate");
      }
    } else {
      if (reset) {
        reset = false;
      }
    }
    
#ifdef SERIAL_OSC
    SLIPSerial.beginPacket();
    bundle.send(SLIPSerial);
    SLIPSerial.endPacket();
#endif
    
  
#ifdef WIFI_OSC
    udp.beginPacket(castIp, port);
    bundle.send(udp);
    udp.endPacket();
#endif

// Some bug below, it seems
#ifdef BT_OSC
    SLIPBTSerial.beginPacket();
    bundle.send(SLIPBTSerial);
    SLIPBTSerial.endPacket();
#endif

    bundle.empty();
  }
}