296 lines
8.4 KiB
Plaintext
296 lines
8.4 KiB
Plaintext
// ESP32 Dev Module
|
||
|
||
#include "Wire.h"
|
||
#include "MPU6050_6Axis_MotionApps20.h"
|
||
|
||
#include <OSCBundle.h>
|
||
#include <OSCBoards.h>
|
||
|
||
#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
|
||
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 */
|
||
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);
|
||
}
|
||
|
||
// 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());
|
||
|
||
// Quaternion - rotacija
|
||
bundle.add("/quaternion").add(q.w).add(q.y * -1).add(q.z).add(q.x * -1); // W 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
|
||
|
||
#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
|
||
|
||
// 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]);
|
||
}
|
||
|
||
#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();
|
||
}
|
||
}
|