gtr/osc32final/osc32final.ino

444 lines
13 KiB
Arduino
Raw Normal View History

// 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() {
2022-09-05 10:02:57 +02:00
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() {
2022-09-05 10:02:57 +02:00
// 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));
2022-09-05 10:02:57 +02:00
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
2022-09-05 10:02:57 +02:00
for(int i = 0; i < KEYLEN; i++) {
pressed[i] = touchRead(keys[i]);
bundle.getOSCMessage("/keys")->add(pressed[i]);
}
2022-09-05 10:02:57 +02:00
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();
}
}