311 lines
8.3 KiB
Arduino
311 lines
8.3 KiB
Arduino
|
// ESP32 Dev Module
|
|||
|
|
|
|||
|
|
#include "Wire.h"
|
|||
|
|
#include "mpu9250.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
|
|||
|
|
bfs::Mpu9250 mpu(&Wire, bfs::Mpu9250::I2C_ADDR_PRIM);
|
|||
|
|
|
|||
|
|
// MPU control/status vars
|
|||
|
|
bool dmpReady = false; // set true if DMP init was successful
|
|||
|
|
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
|
|||
|
|
bool 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
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
|
|||
|
|
|
|||
|
|
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
|
|||
|
|
if (!mpu.Begin()) {
|
|||
|
|
Serial.println("IMU initialization failed");
|
|||
|
|
while(1) {}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/* Set the sample rate divider */
|
|||
|
|
if (!mpu.ConfigSrd(19)) {
|
|||
|
|
Serial.println("Error configured SRD");
|
|||
|
|
while(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");
|
|||
|
|
SerialBT.begin("wavey wind 2");
|
|||
|
|
#endif
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
void loop() {
|
|||
|
|
if (mpu.Read()) {
|
|||
|
|
Serial.print(mpu.new_imu_data());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.new_mag_data());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.accel_x_mps2());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.accel_y_mps2());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.accel_z_mps2());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.gyro_x_radps());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.gyro_y_radps());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.gyro_z_radps());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.mag_x_ut());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.mag_y_ut());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.mag_z_ut());
|
|||
|
|
Serial.print("\t");
|
|||
|
|
Serial.print(mpu.die_temp_c());
|
|||
|
|
Serial.print("\n");
|
|||
|
|
}
|
|||
|
|
return;
|
|||
|
|
|
|||
|
|
// 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
|
|||
|
|
|
|||
|
|
//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
|
|||
|
|
|
|||
|
|
//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();
|
|||
|
|
|
|||
|
|
}
|