coffee_cup/src/main.cpp

#include "Arduino.h"
#include "MPU9250.h"
#include "WiFi.h"
#include "WiFiUdp.h"
#include "OSCBundle.h"
#include "OneWire.h"
#include "DallasTemperature.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 "tasse"

const std::string devicesToSearch[] = {
       "sessel",
       "schrank",
       "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;

// Setup a oneWire instance to communicate with any OneWire devices
OneWire oneWire(oneWireBus);
// Pass our oneWire reference to Dallas Temperature sensor 
DallasTemperature sensors(&oneWire);




float temp;
float threshold = 1.0;


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 bgTask(void * parameter) {
    Wire.begin();
    delay(1000);
    float ypr[3];
    float acc[3];
    float yprNew[3];
    float accNew[3];

    if (!mpu.setup(0x68)) {  
        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();
    

    while(true) {
	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("/tasse/acc").add(acc[0]).add(acc[1]).add(acc[2]);
            bundle.add("/tasse/abs/acc").add(abs(acc[0])).add(abs(acc[1])).add(abs(acc[2]));
            //bundle.add("/tasse/ypr").add(ypr[0]).add(ypr[1]).add(ypr[2]);
            bundle.add("/tasse/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];
	}
	delay(100);
      }
}



void setup() {
    Serial.begin(115200);
    delay(1000);
	
    sensors.begin();
    
    // 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);
    BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
    pAdvertising->start();
    #endif

    Udp.begin(9000);

    xTaskCreatePinnedToCore(
    bgTask, 
    "background task",
    4000,
    NULL,
    1,
    NULL,
    0 );

    temp = -275;
}


void loop() {
   #if BL_ON
   BLEScan *scan = BLEDevice::getScan();
   scan->setActiveScan(true);
   BLEScanResults results = scan->start(1);
   
   for(int i = 0; i < sizeof(devicesToSearch) / sizeof(devicesToSearch[0]); i++){
      for(int j = 0; j < results.getCount(); j++) {
           if( results.getDevice(j).getName() == devicesToSearch[i]) {
	      /*
              Serial.print(devicesToSearch[i].c_str());
              Serial.print(":  ");
              Serial.println(results.getDevice(j).getRSSI());
	      */
	      std::string addr = "/tasse/signal/" + devicesToSearch[i];
	      //Serial.println(addr.c_str());
              bundle.add(addr.c_str()).add(results.getDevice(j).getRSSI());
              break;
              }
      }   
   }
   #endif

   sensors.requestTemperatures();
   float tempNew = sensors.getTempCByIndex(0);
   if(abs(temp - tempNew) > threshold){
	OSCMessage msg("/tasse/temperature");
   	//Serial.println(sensors.getTempCByIndex(0)); 
  	temp = tempNew; 
   	msg.add(tempNew);
   	Udp.beginPacket(host, port);
	msg.send(Udp);
   	Udp.endPacket();
   }
    
   delay(300);

 }