pifcamp-2021/osc32final/osc32final.ino

361 lines
9.5 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 <OSCBoards.h>
#include <OSCMessage.h>
//#define SERIAL_OSC
//#define WIFI_OSC
#define BT_OSC
// 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
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
// 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;
/* OSC MSG channels */
// Quaternion - rotacija
OSCMessage qmsg("/quaternion/"); // W X Y Z
// Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading)
OSCMessage qdmsg("/quaternionDiff/"); // W X Y Z
// Accelerometer
OSCMessage amsg("/accel/"); // X Y Z
// Keys held down
OSCMessage kmsg("/keys/"); // A B C D E
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);
}
// Start MPU
mpu.initialize();
// Set sensitivity / range
mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
// DMP init
devStatus = mpu.dmpInitialize();
// 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);
// 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();
} 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() {
// 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);
//compute the differential rotation between the previous and new orientation
diff = q.getProduct(pq.getConjugate());
qmsg.add(q.w);
qmsg.add(q.x);
qmsg.add(q.y);
qmsg.add(q.z);
qdmsg.add(diff.w);
qdmsg.add(diff.x);
qdmsg.add(diff.y);
qdmsg.add(diff.z);
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
qdmsg.send(SLIPSerial);
SLIPSerial.endPacket();
SLIPSerial.beginPacket();
qmsg.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
qmsg.send(udp);
udp.endPacket();
udp.beginPacket(castIp, port);
qdmsg.send(udp);
udp.endPacket();
#endif
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
qmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
SLIPBTSerial.beginPacket();
qdmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
qmsg.empty();
qdmsg.empty();
#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
amsg.add(AcX);
amsg.add(AcY);
amsg.add(AcZ);
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
amsg.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
amsg.send(udp);
udp.endPacket();
#endif
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
amsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
amsg.empty();
// Send keys
for(int i = 0; i < KEYLEN; i++) {
pressed[i] = !digitalRead(keys[i]);
kmsg.add(pressed[i]);
}
#ifdef SERIAL_OSC
SLIPSerial.beginPacket();
kmsg.send(SLIPSerial);
SLIPSerial.endPacket();
#endif
#ifdef WIFI_OSC
udp.beginPacket(castIp, port);
kmsg.send(udp);
udp.endPacket();
#endif
#ifdef BT_OSC
SLIPBTSerial.beginPacket();
kmsg.send(SLIPBTSerial);
SLIPBTSerial.endPacket();
#endif
kmsg.empty();
}
}