180 lines
4.5 KiB
C++
180 lines
4.5 KiB
C++
// ESP32 Dev Module
|
|
|
|
#include <Wire.h>
|
|
|
|
// ID kegla mora bit unikaten za vsakega! (se poslje poleg parametrov)
|
|
#define KEGEL_ID 1
|
|
|
|
// IMU libraries
|
|
#include "I2Cdev.h"
|
|
#include "RTIMUSettings.h"
|
|
#include "RTIMU.h"
|
|
#include "RTFusionRTQF.h"
|
|
#include "CalLib.h"
|
|
#include <EEPROM.h>
|
|
|
|
//#include "RTMath.h"
|
|
|
|
|
|
#include <esp_now.h>
|
|
#include <WiFi.h>
|
|
|
|
uint8_t sprejemnikMac[] = {0x08, 0x3A, 0xF2, 0x50, 0xEF, 0x6C };
|
|
|
|
typedef struct sensor_msg {
|
|
int id;
|
|
RTFLOAT aX;
|
|
RTFLOAT aY;
|
|
RTFLOAT aZ;
|
|
RTFLOAT qX;
|
|
RTFLOAT qY;
|
|
RTFLOAT qZ;
|
|
RTFLOAT qW;
|
|
}sensor_msg;
|
|
|
|
sensor_msg odcitek;
|
|
esp_now_peer_info_t peerInfo;
|
|
|
|
#define DISPLAY_INTERVAL 5 // interval between pose displays
|
|
|
|
// Motion sensor objects
|
|
RTIMU *imu; // the IMU object
|
|
RTFusionRTQF fusion; // the fusion object
|
|
RTIMUSettings settings; // the settings object
|
|
|
|
unsigned long lastDisplay;
|
|
unsigned long lastRate;
|
|
int sampleCount;
|
|
RTQuaternion gravity;
|
|
|
|
bool reset; // For quaternion calibration
|
|
|
|
void setup() {
|
|
int errcode;
|
|
|
|
// Basic(debug) serial init
|
|
Serial.begin(115200);
|
|
//Serial.begin(115200); // set this as high as you can reliably run on your platform
|
|
Serial.println("Starting up...");
|
|
|
|
// Init EEPROM based on magnet calibration size requirement
|
|
EEPROM.begin(512);
|
|
|
|
// I2C init
|
|
Wire.begin();
|
|
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
|
|
|
|
// create the imu object
|
|
imu = RTIMU::createIMU(&settings);
|
|
|
|
Serial.print("ArduinoIMU starting using device "); Serial.println(imu->IMUName());
|
|
if ((errcode = imu->IMUInit()) < 0) {
|
|
Serial.print("Failed to init IMU: "); Serial.println(errcode);
|
|
}
|
|
|
|
|
|
if (imu->getCalibrationValid())
|
|
Serial.println("Using compass calibration");
|
|
else
|
|
Serial.println("No valid compass calibration data");
|
|
|
|
// Gravity obj
|
|
gravity.setScalar(0);
|
|
gravity.setX(0);
|
|
gravity.setY(0);
|
|
gravity.setZ(1);
|
|
/*
|
|
fusion.setSlerpPower(0.02);
|
|
fusion.setGyroEnable(true);
|
|
fusion.setAccelEnable(true);
|
|
fusion.setCompassEnable(true);
|
|
*/
|
|
|
|
lastDisplay = lastRate = millis();
|
|
sampleCount = 0;
|
|
|
|
// WIFI init
|
|
WiFi.mode(WIFI_STA);
|
|
if (esp_now_init() != ESP_OK) {
|
|
Serial.println("Error initializing ESP-NOW");
|
|
return;
|
|
}
|
|
|
|
memcpy(peerInfo.peer_addr, sprejemnikMac, 6);
|
|
peerInfo.channel = 0;
|
|
peerInfo.encrypt = false;
|
|
|
|
if (esp_now_add_peer(&peerInfo) != ESP_OK){
|
|
Serial.println("WIFI registracija ni uspela");
|
|
return;
|
|
}
|
|
}
|
|
|
|
void loop() {
|
|
unsigned long now = millis();
|
|
unsigned long delta;
|
|
RTVector3 realAccel;
|
|
RTQuaternion rotatedGravity;
|
|
RTQuaternion fusedConjugate;
|
|
RTQuaternion qTemp;
|
|
int loopCount = 0;
|
|
|
|
// get the latest data if ready yet
|
|
while (imu->IMURead()) {
|
|
// this flushes remaining data in case we are falling behind
|
|
if (++loopCount >= 10)
|
|
continue;
|
|
|
|
fusion.newIMUData(imu->getGyro(), imu->getAccel(), imu->getCompass(), imu->getTimestamp());
|
|
|
|
// do gravity rotation and subtraction
|
|
|
|
// create the conjugate of the pose
|
|
fusedConjugate = fusion.getFusionQPose().conjugate();
|
|
|
|
// now do the rotation - takes two steps with qTemp as the intermediate variable
|
|
qTemp = gravity * fusion.getFusionQPose();
|
|
rotatedGravity = fusedConjugate * qTemp;
|
|
|
|
// now adjust the measured accel and change the signs to make sense
|
|
realAccel.setX(-(imu->getAccel().x() - rotatedGravity.x()));
|
|
realAccel.setY(-(imu->getAccel().y() - rotatedGravity.y()));
|
|
realAccel.setZ(-(imu->getAccel().z() - rotatedGravity.z()));
|
|
|
|
sampleCount++;
|
|
if ((delta = now - lastRate) >= 1000) {
|
|
|
|
//Serial.print("Sample rate: "); Serial.print(sampleCount);
|
|
if (!imu->IMUGyroBiasValid()) {
|
|
// Serial.println(", calculating gyro bias");
|
|
} else {
|
|
// Serial.println();
|
|
}
|
|
|
|
sampleCount = 0;
|
|
lastRate = now;
|
|
}
|
|
|
|
if ((now - lastDisplay) >= DISPLAY_INTERVAL) {
|
|
lastDisplay = now;
|
|
|
|
odcitek.id = KEGEL_ID;
|
|
odcitek.aX = realAccel.x();
|
|
odcitek.aY = realAccel.y();
|
|
odcitek.aZ = realAccel.z();
|
|
odcitek.qX = fusion.getFusionQPose().x();
|
|
odcitek.qY = fusion.getFusionQPose().y();
|
|
odcitek.qZ = fusion.getFusionQPose().z();
|
|
odcitek.qW = fusion.getFusionQPose().scalar();
|
|
|
|
esp_err_t result = esp_now_send(sprejemnikMac, (uint8_t *) &odcitek, sizeof(odcitek));
|
|
|
|
if (result == ESP_OK) {
|
|
Serial.println("Uspesno poslano");
|
|
} else {
|
|
Serial.println("Napaka pri posiljanju");
|
|
}
|
|
}
|
|
}
|
|
}
|