Last updated
// -------------------------------------------
// Esp32 Code to send orientation data
// of the BNO055 sensor via UDP.
// -------------------------------------------
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <utility/imumaths.h>
#include <WiFi.h>
#include <WiFiUdp.h>
// I2C Clock Pin: 4
// I2C Data Pin: 0
// WiFi credentials
const char* ssid = "";
const char* password = "";
// UDP settings
const char* udpServerIP = "192.168.0.61"; // Replace with your computer's IP address
const int udpPort = 8888;
WiFiUDP udp;
unsigned long previousMillis = 0;
const long udpInterval = 5000;
#define BNO055_SAMPLERATE_DELAY_MS (100) /* Set the delay between fresh samples for the BNO055*/
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x28, &Wire); //(by default address is 0x29 or 0x28)
void setup(void) {
Serial.begin(115200);
Wire.begin(0,4); // Important! Define the I2C Pins manually
// wait for serial port to open
while (!Serial)
delay(10);
// Initialise BNO055 Orientation Sensor
if(!bno.begin())
{
Serial.print("BNO055 not found.");
while(1);
}
delay(1000);
bno.setExtCrystalUse(true);
// Establish WIFI Connection
WiFi.begin(ssid, password);
while (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
}
Serial.println("WiFi connected, IP address: ");
Serial.println(WiFi.localIP());
// Establish UDP
udp.begin(udpPort);
Serial.print("UDP started on port ");
Serial.println(udpPort);
}
void loop() {
/* Get a new sensor event */
sensors_event_t event;
bno.getEvent(&event);
/* Display the floating point data */
Serial.print("X: ");
Serial.print(event.orientation.x, 4);
Serial.print("\tY: ");
Serial.print(event.orientation.y, 4);
Serial.print("\tZ: ");
Serial.print(event.orientation.z, 4);
Serial.println("");
sendUDPMessage("RotX:"+String(event.orientation.x)+",RotY:"+String(event.orientation.y)+",RotZ:"+String(event.orientation.z));
unsigned long currentMillis = millis();
// Check WiFi connection
if (WiFi.status() != WL_CONNECTED) {
Serial.println("WiFi disconnected. Reconnecting...");
WiFi.begin(ssid, password);
return;
}
// Send message at an interval (currently deactivated)
if (currentMillis - previousMillis >= udpInterval) {
previousMillis = currentMillis;
sendUDPMessage("RotX:"+String(event.orientation.x)+",RotY:"+String(event.orientation.y)+",RotZ:"+String(event.orientation.z));
}
/* Wait the specified delay before looping and requesting next data */
delay(BNO055_SAMPLERATE_DELAY_MS);
}
void sendUDPMessage(String message) {
udp.beginPacket(udpServerIP, udpPort);
udp.print(message);
bool success = udp.endPacket();
if (success) {
//Serial.println("UDP message sent: " + message);
} else {
Serial.println("Failed to send UDP message");
}
}