195 lines
4.4 KiB
C++
195 lines
4.4 KiB
C++
#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);
|
|
|
|
}
|
|
|