esp32, lcd, mpu6050, ble bluetooth, source code 종합 소스코드
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#define SERVICE_UUID "0000FFE0-0000-1000-8000-00805F9B34FB"
#define CHARACTERISTIC_UUID "0000FFE1-0000-1000-8000-00805F9B34FB"
#define DEVICE_NAME "Motion0002"
//상기 3가지 값이 BLE 디바이스를 찾고 연결하는데 사용됩니다. 유니티에서도 동일하게 입력되어야 함
BLEServer* pServer = NULL;
BLECharacteristic* pCharacteristic = NULL;
bool deviceConnected = false;
bool oldDeviceConnected = false;
void setup() {
pinMode(D1, INPUT);
pinMode(D2, INPUT);
Serial.begin(115200);
ble_bluetooth_init();
mpu_setup();
}
unsigned int buff_size = 20; //BLE bluetooth 패킷은 최대 20바이트
String data_str = "";
char buf[20] = "";
uint8_t values[20];
uint8_t sw1;
uint8_t sw2;
void loop(){
sw1 = digitalRead(D1);
sw2 = digitalRead(D2);
mpu_loop();
connect_check();
set_data();
send_data();
}
void connect_check(){
// disconnecting
if (!deviceConnected && oldDeviceConnected) {
//delay(500); // give the bluetooth stack the chance to get things ready
pServer->startAdvertising(); // restart advertising
oldDeviceConnected = deviceConnected;
}
// connecting
if (deviceConnected && !oldDeviceConnected) {
// do stuff here on connecting
oldDeviceConnected = deviceConnected;
}
}
Quaternion q; // [w, x, y, z] quaternion container
void set_data(){
data_str = "";
int q_x = abs(int(trunc((q.x+2)*1000)));
int q_y = abs(int(trunc((q.y+2)*1000)));
int q_z = abs(int(trunc((q.z+2)*1000)));
int q_w = abs(int(trunc((q.w+2)*1000)));
String s_x = String(q_x);
String s_y = String(q_y);
String s_z = String(q_z);
String s_w = String(q_w);
data_str += sw1;
data_str += sw2;
data_str += s_x;
data_str += s_y;
data_str += s_z;
data_str += s_w;
Serial.println(data_str);
data_str.toCharArray(buf, buff_size);
for(int i=0;i<buff_size;i++)
{
values[i] = buf[i];
}
}
void send_data(){
if (deviceConnected) {
pCharacteristic->setValue(values, buff_size);
pCharacteristic->notify();
//delay(10); // bluetooth stack will go into congestion, if too many packets are sent, in 6 hours test i was able to go as low as 3ms
}
}
class ServerCallbacks: public BLEServerCallbacks {
void onConnect(BLEServer* pServer) {
deviceConnected = true;
BLEDevice::startAdvertising();
};
void onDisconnect(BLEServer* pServer) {
deviceConnected = false;
}
};
void ble_bluetooth_init()
{
// Create the BLE Device
BLEDevice::init(DEVICE_NAME);
// Create the BLE Server
pServer = BLEDevice::createServer();
pServer->setCallbacks(new ServerCallbacks());
// Create the BLE Service
BLEService *pService = pServer->createService(SERVICE_UUID);
// Create a BLE Characteristic
pCharacteristic = pService->createCharacteristic(CHARACTERISTIC_UUID, BLECharacteristic::PROPERTY_NOTIFY);
// https://www.bluetooth.com/specifications/gatt/viewer?attributeXmlFile=org.bluetooth.descriptor.gatt.client_characteristic_configuration.xml
// Create a BLE Descriptor
pCharacteristic->addDescriptor(new BLE2902());
// Start the service
pService->start();
// Start advertising
BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
pAdvertising->addServiceUUID(SERVICE_UUID);
pAdvertising->setScanResponse(false);
pAdvertising->setMinPreferred(0x0); // set value to 0x00 to not advertise this parameter
BLEDevice::startAdvertising();
Serial.println("Waiting a client connection to notify...");
}
/////////////////////////////////////////////////////////////////////////////////////////////
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_QUATERNION
#define SDA D4
#define SCL D5
#define INTERRUPT_PIN 5 //이경우는 인터럽트핀 번호가 5입니다.
// 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 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
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void mpu_setup() {
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
Serial.println("I2CDEV_ARDUINO_WIRE");
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
Serial.println("I2CDEV_BUILTIN_FASTWIRE");
#endif
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
//Sensor readings with offsets: -2 -13 16390 0 -5 -2
//Your offsets: -481 1422 1807 -75 56 40
//acelX acelY acelZ giroX giroY giroZ
mpu.setXAccelOffset(-481);
mpu.setYAccelOffset(1422);
mpu.setZAccelOffset(1807);
mpu.setXGyroOffset(-75);
mpu.setYGyroOffset(56);
mpu.setZGyroOffset(40);
//set offset from calculator
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// 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 {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
//pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===
// ================================================================
void mpu_loop() {
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
// if you are really paranoid you can frequently test in between other
// stuff to see if mpuInterrupt is true, and if so, "break;" from the
// while() loop to immediately process the MPU data
}
// reset interrupt flag and get INT_STATUS byte
mpuInterrupt = false;
mpuIntStatus = mpu.getIntStatus();
// get current FIFO count
fifoCount = mpu.getFIFOCount();
// check for overflow (this should never happen unless our code is too inefficient)
if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
//Serial.println(F("FIFO overflow!"));
// otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
// wait for correct available data length, should be a VERY short wait
while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
// read a packet from FIFO
mpu.getFIFOBytes(fifoBuffer, packetSize);
// track FIFO count here in case there is > 1 packet available
// (this lets us immediately read more without waiting for an interrupt)
fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
/*
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
*/
#endif
}
}