Merge branch 'master' of https://git.kompot.si/g1smo/pifcamp-2021

i2cscanner/i2cscanner.ino

@@ -0,0 +1,83 @@
+ // --------------------------------------
+// i2c_scanner
+//
+// Version 1
+//    This program (or code that looks like it)
+//    can be found in many places.
+//    For example on the Arduino.cc forum.
+//    The original author is not know.
+// Version 2, Juni 2012, Using Arduino 1.0.1
+//     Adapted to be as simple as possible by Arduino.cc user Krodal
+// Version 3, Feb 26  2013
+//    V3 by louarnold
+// Version 4, March 3, 2013, Using Arduino 1.0.3
+//    by Arduino.cc user Krodal.
+//    Changes by louarnold removed.
+//    Scanning addresses changed from 0...127 to 1...119,
+//    according to the i2c scanner by Nick Gammon
+//    https://www.gammon.com.au/forum/?id=10896
+// Version 5, March 28, 2013
+//    As version 4, but address scans now to 127.
+//    A sensor seems to use address 120.
+// Version 6, November 27, 2015.
+//    Added waiting for the Leonardo serial communication.
+// 
+//
+// This sketch tests the standard 7-bit addresses
+// Devices with higher bit address might not be seen properly.
+//
+
+#include <Wire.h>
+
+
+void setup()
+{
+  Wire.begin();
+
+  Serial.begin(9600);
+  while (!Serial);             // Leonardo: wait for serial monitor
+  Serial.println("\nI2C Scanner");
+}
+
+
+void loop()
+{
+  byte error, address;
+  int nDevices;
+
+  Serial.println("Scanning...");
+
+  nDevices = 0;
+  for(address = 1; address < 127; address++ ) 
+  {
+    // The i2c_scanner uses the return value of
+    // the Write.endTransmisstion to see if
+    // a device did acknowledge to the address.
+    Wire.beginTransmission(address);
+    error = Wire.endTransmission();
+
+    if (error == 0)
+    {
+      Serial.print("I2C device found at address 0x");
+      if (address<16) 
+        Serial.print("0");
+      Serial.print(address,HEX);
+      Serial.println("  !");
+
+      nDevices++;
+    }
+    else if (error==4) 
+    {
+      Serial.print("Unknown error at address 0x");
+      if (address<16) 
+        Serial.print("0");
+      Serial.println(address,HEX);
+    }    
+  }
+  if (nDevices == 0)
+    Serial.println("No I2C devices found\n");
+  else
+    Serial.println("done\n");
+
+  delay(5000);           // wait 5 seconds for next scan
+}

osc32final/osc32final.ino

@@ -141,6 +141,7 @@ void setup() {
     // get expected DMP packet size for later comparison
     packetSize = mpu.dmpGetFIFOPacketSize();
   } else {
+    Serial.println("Error: " + String(devStatus));
     emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
     SLIPSerial.beginPacket();
     emsg.send(SLIPSerial);

osc32wifi/osc32wifi.ino

@@ -0,0 +1,342 @@
+// ESP32 Dev Module
+
+#include "Wire.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+
+
+#include <OSCBoards.h>
+#include <OSCMessage.h>
+/*
+Make an OSC message and send it over 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
+#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 "AsyncUDP.h"
+//AsyncUDP udp;
+
+#include <WiFiUdp.h>
+WiFiUDP udp;
+
+MPU6050 mpu;
+
+// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
+// quaternion components in a [w, x, y, z] format (not best for parsing
+// on a remote host such as Processing or something though)
+#define OUTPUT_READABLE_QUATERNION
+
+// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
+// (in degrees) calculated from the quaternions coming from the FIFO.
+// Note that Euler angles suffer from gimbal lock (for more info, see
+// http://en.wikipedia.org/wiki/Gimbal_lock)
+//#define OUTPUT_READABLE_EULER
+
+// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
+// pitch/roll angles (in degrees) calculated from the quaternions coming
+// from the FIFO. Note this also requires gravity vector calculations.
+// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
+// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
+#define OUTPUT_READABLE_YAWPITCHROLL
+
+// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
+// components with gravity removed. This acceleration reference frame is
+// not compensated for orientation, so +X is always +X according to the
+// sensor, just without the effects of gravity. If you want acceleration
+// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
+//#define OUTPUT_READABLE_REALACCEL
+
+// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
+// components with gravity removed and adjusted for the world frame of
+// reference (yaw is relative to initial orientation, since no magnetometer
+// is present in this case). Could be quite handy in some cases.
+#define OUTPUT_READABLE_WORLDACCEL
+
+
+// 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
+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
+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;
+
+OSCMessage msg("/accel/");
+OSCMessage gmsg("/gyro/");
+OSCMessage emsg("/error/");
+OSCMessage kmsg("/keys/");
+OSCMessage qmsg("/quaternion/");
+
+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(),6000);
+          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;
+    }
+}
+
+void setup() {
+  Wire.begin();
+  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
+  SLIPSerial.begin(115200);   // set this as high as you can reliably run on your platform 
+
+  // Keys
+  for(int i = 0; i < KEYLEN; i++) {
+    pinMode(keys[i], INPUT_PULLUP);
+  }
+
+  mpu.initialize();
+  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
+
+  /* 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("Error: " + String(devStatus));
+    emsg.add("DMP Initialization failed (code " + String(devStatus) + ")");
+    SLIPSerial.beginPacket();
+    emsg.send(SLIPSerial);
+    SLIPSerial.endPacket();
+    emsg.empty();
+    // ERROR!
+    // 1 = initial memory load failed
+    // 2 = DMP configuration updates failed
+    // (if it's going to break, usually the code will be 1)
+  }
+
+  // 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);
+  }
+
+  Serial.print("Connected to ");
+  Serial.println(ssid);
+  
+  //Serial.print("UDP connection: ");
+  //Serial.println(udp.connect(castIp, port));
+}
+
+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 
+
+
+#ifdef OUTPUT_READABLE_QUATERNION
+    // display quaternion values in easy matrix form: w x y z
+    mpu.dmpGetQuaternion(&q, fifoBuffer);
+    
+    qmsg.add(q.w);
+    qmsg.add(q.x);
+    qmsg.add(q.y);
+    qmsg.add(q.z);
+    
+    SLIPSerial.beginPacket();
+    qmsg.send(SLIPSerial);
+    SLIPSerial.endPacket();
+
+    udp.beginPacket(castIp, port);
+    qmsg.send(udp);
+    udp.endPacket();
+    
+    qmsg.empty();
+#endif
+
+
+#ifdef OUTPUT_READABLE_EULER
+    // display Euler angles in degrees
+    mpu.dmpGetQuaternion(&q, fifoBuffer);
+    mpu.dmpGetEuler(euler, &q);
+
+    GyX = euler[0];
+    GyY = euler[1];
+    GyZ = euler[2];
+#endif
+
+#ifdef OUTPUT_READABLE_YAWPITCHROLL
+    // display Euler angles in degrees
+    mpu.dmpGetQuaternion(&q, fifoBuffer);
+    mpu.dmpGetGravity(&gravity, &q);
+    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+
+    GyX = ypr[0];
+    GyY = ypr[1];
+    GyZ = ypr[2];
+#endif
+
+#ifdef OUTPUT_READABLE_REALACCEL
+    // display real acceleration, adjusted to remove gravity
+    mpu.dmpGetQuaternion(&q, fifoBuffer);
+    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.dmpGetQuaternion(&q, fifoBuffer);
+    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
+
+    // Send over serial
+    msg.add(AcX);
+    msg.add(AcY);
+    msg.add(AcZ);
+    SLIPSerial.beginPacket();
+    msg.send(SLIPSerial);
+    SLIPSerial.endPacket();
+
+    udp.beginPacket(castIp, port);
+    msg.send(udp);
+    udp.endPacket();
+    
+    msg.empty();
+
+    /*
+    gmsg.add(GyX);
+    gmsg.add(GyY);
+    gmsg.add(GyZ);
+    SLIPSerial.beginPacket();
+    gmsg.send(SLIPSerial);
+    SLIPSerial.endPacket();
+    gmsg.empty();
+    */
+    
+    // Send keys
+    for(int i = 0; i < KEYLEN; i++) {
+      pressed[i] = !digitalRead(keys[i]);
+      kmsg.add(pressed[i]);
+    }
+    SLIPSerial.beginPacket();
+    kmsg.send(SLIPSerial);
+    SLIPSerial.endPacket();
+
+    
+    udp.beginPacket(castIp, port);
+    kmsg.send(udp);
+    udp.endPacket();
+    
+    kmsg.empty();
+  }
+}