teddy/src/main.cpp

#include "Arduino.h"
#include "MPU9250.h"
#include "WiFi.h"
#include "WiFiUdp.h"
#include "OSCBundle.h"

// SCL Pin is 22
// SDA PIN is 21

#define BL_ON 0

#if BL_ON
#include "BLEDevice.h"
#include "BLEScan.h"

// bluetooth name
#define blName "Teddy"
#endif

// WiFi stuff
#define ssid ""
#define pwd ""

// OSC
#define host "192.168.21.232"
#define port  3030

#define oneWireBus 18

WiFiUDP Udp;
MPU9250 mpu;




float ypr[3];
float acc[3];
int pr;
const float threshold = 0.7;
const int thresholdP = 50;

void set_calibration() {
      mpu.setAccBias(232.00, 89.60, -581.30);
      mpu.setGyroBias(14.80, 95.80, 23.00);
      mpu.setMagBias(284.65, -333.85, 91.40);
      mpu.setMagScale(0.59, 1.39, 1.68);
}

void get_calibration() {

      Serial.println();
      Serial.println("Accel Gyro calibration will start in 5sec.");
      Serial.println("Please leave the device still on the flat plane.");
      mpu.verbose(true);
      delay(5000);
      mpu.calibrateAccelGyro();
      Serial.println();
  
      Serial.println("Mag calibration will start in 5sec.");
      Serial.println("Please Wave device in a figure eight until done.");
      delay(5000);
      mpu.calibrateMag();
      Serial.println();
  
      Serial.println("< calibration parameters >");
      Serial.println("accel bias [g]: ");
      Serial.print(mpu.getAccBiasX());
      Serial.print(", ");
      Serial.print(mpu.getAccBiasY());
      Serial.print(", ");
      Serial.print(mpu.getAccBiasZ());
      Serial.println();
      Serial.println("gyro bias [deg/s]: ");
      Serial.print(mpu.getGyroBiasX());
      Serial.print(", ");
      Serial.print(mpu.getGyroBiasY());
      Serial.print(", ");
      Serial.print(mpu.getGyroBiasZ());
      Serial.println();
      Serial.println("mag bias [mG]: ");
      Serial.print(mpu.getMagBiasX());
      Serial.print(", ");
      Serial.print(mpu.getMagBiasY());
      Serial.print(", ");
      Serial.print(mpu.getMagBiasZ());
      Serial.println();
      Serial.println("mag scale []: ");
      Serial.print(mpu.getMagScaleX());
      Serial.print(", ");
      Serial.print(mpu.getMagScaleY());
      Serial.print(", ");
      Serial.print(mpu.getMagScaleZ());
      Serial.println();
      Serial.println();
      mpu.verbose(false);
      
}



void setup() {
    Serial.begin(115200);
    delay(500);
    Wire.begin();
    delay(500);

    if (!mpu.setup(0x68)) {  // change to your own address
        while (1) {
            Serial.println("MPU failed");
            delay(5000);
        }
    }

    set_calibration();
    //get_calibration(); 
    ypr[0] = mpu.getYaw();
    ypr[1] = mpu.getPitch();
    ypr[2] = mpu.getRoll();
        
    acc[0] = mpu.getAccX();
    acc[1] = mpu.getAccY();
    acc[2] = mpu.getAccZ();

    pr = 0;	
    
    // connecting to a WiFi network
    Serial.println();
    Serial.print("Connecting to ");
    Serial.println(ssid);
    
    WiFi.begin(ssid, pwd);
    while (WiFi.status() != WL_CONNECTED) {
        Serial.print(".");
        delay(500);
    }
    Serial.print("WiFi connected, IP = ");
    Serial.println(WiFi.localIP());
	
    #if BL_ON
    BLEDevice::init(blName);
    #endif

    Udp.begin(9000);
}


void loop() {
   float yprNew[3];
   float accNew[3];
   bool send = false;

   if (mpu.update()) {
	yprNew[0] = mpu.getYaw();
        yprNew[1] = mpu.getPitch();
        yprNew[2] = mpu.getRoll();
        accNew[0] = mpu.getAccX();
        accNew[1] = mpu.getAccY();
        accNew[2] = mpu.getAccZ();
    }
    for(int i = 0; i < 3; i++){
	    if (abs(ypr[i] - yprNew[i]) > threshold){ send = true;} 
	    if (abs(acc[i] - accNew[i]) > threshold){ send = true;} 
	}
	if(send == true){
      	    OSCBundle bundle;
            bundle.add("/teddy/acc").add(acc[0]).add(acc[1]).add(acc[2]);
            bundle.add("/teddy/abs/acc").add(abs(acc[0])).add(abs(acc[1])).add(abs(acc[2]));
            //bundle.add("/teddy/ypr").add(ypr[0]).add(ypr[1]).add(ypr[2]);
            bundle.add("/teddy/abs/ypr").add(abs(ypr[0])).add(abs(ypr[1])).add(abs(ypr[2]));
            
            Udp.beginPacket(host, port);
            bundle.send(Udp);
            Udp.endPacket();
            bundle.empty();
	}
	for(int i = 0; i < 3; i++){
		ypr[i] = yprNew[i];
		acc[i] = accNew[i];
	}

    int prNew = analogRead(39);
    if (abs(pr-prNew) > thresholdP){
	    OSCMessage msg("/teddy/druck");
	    msg.add(prNew);
            Udp.beginPacket(host, port);
  	    msg.send(Udp);
  	    Udp.endPacket();
	    pr = prNew;
    }

  delay(100);

}