// ESP32 Dev Module #include "Wire.h" #include "MPU6050_6Axis_MotionApps20.h" #include #include #define SERIAL_OSC //#define WIFI_OSC //#define BT_OSC #define OUTPUT_READABLE_WORLDACCEL // SERIAL #ifdef BOARD_HAS_USB_SERIAL #include SLIPEncodedUSBSerial SLIPSerial( thisBoardsSerialUSB ); #else #include 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 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 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 #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).add(q.x).add(q.z); // W X Y Z // Quaterion difference - rotacijska razlika (prejsnji reading - trenutni reading) bundle.add("/quaternionDiff").add(diff.w).add(diff.y).add(diff.x).add(diff.z); // 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(); } }