Merge branch 'master' of https://git.kompot.si/g1smo/pifcamp-2021
Martin Krauser
commit
7c93796221
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue