pifcamp-2021/osc32wifi/osc32wifi.ino

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

// WIFI
#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;

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 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(),6000);
          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;
    }
}

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 

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

  /* 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("Error: " + String(devStatus));
    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)
  }

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

  Serial.print("Connected to ");
  Serial.println(ssid);
  
  //Serial.print("UDP connection: ");
  //Serial.println(udp.connect(castIp, port));
}

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();

    udp.beginPacket(castIp, port);
    qmsg.send(udp);
    udp.endPacket();
    
    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();

    udp.beginPacket(castIp, port);
    msg.send(udp);
    udp.endPacket();
    
    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();

    
    udp.beginPacket(castIp, port);
    kmsg.send(udp);
    udp.endPacket();
    
    kmsg.empty();
  }
}