gtr/osc32final/osc32final.ino

444 lines
13 KiB
C++
Raw Blame History

This file contains ambiguous Unicode characters!

This file contains ambiguous Unicode characters that may be confused with others in your current locale. If your use case is intentional and legitimate, you can safely ignore this warning. Use the Escape button to highlight these characters.

// ESP32 Dev Module
#include "Wire.h"
#include "MPU6050_6Axis_MotionApps20.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 dont 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
MPU6050 mpu;
// 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
Quaternion pq; // [w, x, y, z] previous quaternion container
Quaternion diff; // [w, x, y, z] quaternion derivate container
Quaternion cq; // [w, x, y, z] calibration quaternion
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
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 keys[] = {4, 0, 2, 15, 13, 14, 12, 27}; // TOUCH0 - TOUCH7
byte pressed[] = {0, 0, 0, 0, 0, 0, 0, 0};
byte KEYLEN = 8;
BLA::Matrix<3> eulerFromQuaternion(Quaternion q) {
float x2 = q.x + q.x; float y2 = q.y + q.y; float z2 = q.z + q.z;
float xx = q.x * x2; float xy = q.x * y2; float xz = q.x * z2;
float yy = q.y * y2; float yz = q.y * z2; float zz = q.z * z2;
float wx = q.w * x2; float wy = q.w * y2; float wz = q.w * z2;
BLA::Matrix<4,4> rotationMatrix = {
1 - (yy + zz), xy + wz, xz - wy, 0,
xy - wz, 1 - ( xx + zz ), yz + wx, 0,
xz + wy, yz - wx, 1 - ( xx + yy ), 0,
0, 0, 0, 1
};
//TODO: test whether BLA library uses column-major matrix notation in code
BLA::Matrix<3> eulerVector;
eulerVector.Fill(0);
eulerVector(1) = asin(clamp(rotationMatrix(1,3),-1,1));
if (fabsf(rotationMatrix(1,3)) < 0.9999999) {
eulerVector(0) = atan2f(-rotationMatrix(2,3), rotationMatrix(3,3));
eulerVector(2) = atan2f( -rotationMatrix(1,2), rotationMatrix(1,1));
} else {
eulerVector(0) = atan2f(rotationMatrix(3,2), rotationMatrix(2,2));
eulerVector(2) = 0;
}
return eulerVector;
}
float clamp(float value,float min,float max) {
return fmaxf( min, fminf(max, value));
}
/* OSC MSG channels */
OSCBundle bundle;
// ----------------------------- capacative switching
// set pin numbers
const int touchPin = 4;
//const int ledPin = 16;
// change with your threshold value
const int threshold = 20;
// variable for storing the touch pin value
int touchValue;
void setup() {
pinMode(32, OUTPUT);
// Basic(debug) serial init
Serial.begin(115200); // set this as high as you can reliably run on your platform
Serial.println("Starting up...");
// CAPACITIVE SWITCHING + LEDS
// initialize the LED pin as an output:
//pinMode (ledPin, OUTPUT);
// 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);
Serial.println("MPU will init...");
// Start MPU
mpu.initialize();
Serial.println("MPU did init... 1");
// Set sensitivity / range
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
Serial.println("Ranges set... 2");
// DMP init
devStatus = mpu.dmpInitialize();
Serial.println("DMP did init... 3");
// supply your own gyro offsets here, scaled for min sensitivity
// !!! Run Zero IMU to get readings (read comments for instructions)
/* 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);
*/
Serial.println("DMP Init all good?");
// 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();
Serial.println("DMP Initialization good!");
} else {
Serial.println("DMP Initialization failed (code " + String(devStatus) + ")");
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 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");
#endif
}
void loop() {
// LED ON
// capacitive ------------------
// read the state of the pushbutton value:
touchValue = touchRead(touchPin);
//Serial.print(touchValue);
// check if the touchValue is below the threshold
// if it is, set ledPin to HIGH
if(touchValue < threshold){
// turn LED on
//digitalWrite(ledPin, HIGH);
//Serial.println(" - LED on");
}
else{
// turn LED off
//digitalWrite(ledPin, LOW);
//Serial.println(" - LED off");
}
// ==============================
// 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
// Store last Q value
pq = Quaternion(q.w,q.x,q.y,q.z);
// get quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
q = q.getProduct(cq);
//compute the differential rotation between the previous and new orientation
diff = q.getProduct(pq.getConjugate());
// Quaternion - rotacija
bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W X Y Z
// 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
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.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
// 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("3/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] = touchRead(keys[i]);
bundle.getOSCMessage("/keys")->add(pressed[i]);
}
if (pressed[0] != 0 ){ digitalWrite(32, HIGH);}
else { digitalWrite(32, LOW); };
// 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();
}
}