143 lines
2.9 KiB
C++
143 lines
2.9 KiB
C++
#include <Arduino_LSM6DS3.h>
|
|
#include <WiFiNINA.h>
|
|
#include "data.h"
|
|
|
|
#define SAMPLES_PER_MINUTE 256
|
|
#define SENSITIVITY 5
|
|
#define THRESHOLD 0.04
|
|
#define ID 1
|
|
#define DEBUG false
|
|
|
|
float accelX, accelY, accelZ;
|
|
|
|
float calX, calY, calZ;
|
|
unsigned char calibration;
|
|
|
|
float oneMinSamples[SAMPLES_PER_MINUTE];
|
|
unsigned char oneMinSampleIndex;
|
|
float oneMinMax;
|
|
int startTime, deltaTime;
|
|
|
|
void setup()
|
|
{
|
|
// Serial only used in debug mode
|
|
if (DEBUG)
|
|
{
|
|
Serial.begin(9600);
|
|
while (!Serial);
|
|
}
|
|
|
|
if (!IMU.begin())
|
|
{
|
|
if (DEBUG)
|
|
{
|
|
Serial.println("Failed to initialize IMU");
|
|
}
|
|
|
|
while(1);
|
|
}
|
|
|
|
// Setup LED
|
|
pinMode(LEDR, OUTPUT);
|
|
pinMode(LEDG, OUTPUT);
|
|
pinMode(LEDB, OUTPUT);
|
|
|
|
oneMinSampleIndex = 0;
|
|
calibration = 0;
|
|
calX = 0.0;
|
|
calY = 0.0;
|
|
calZ = 0.0;
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
startTime = micros();
|
|
// Calculate and update the square of the magnitude of acceleration
|
|
if (IMU.accelerationAvailable())
|
|
{
|
|
IMU.readAcceleration(accelX, accelY, accelZ);
|
|
accelX *= SENSITIVITY;
|
|
accelY *= SENSITIVITY;
|
|
accelZ *= SENSITIVITY;
|
|
}
|
|
|
|
|
|
// Calibration Stage
|
|
if (calibration < 0xFF)
|
|
{
|
|
calibration++;
|
|
if (DEBUG)
|
|
{
|
|
Serial.print("Calibrating (");
|
|
Serial.print(calibration/2.55);
|
|
Serial.print("%)\x0d");
|
|
}
|
|
digitalWrite(LEDR, LOW);
|
|
digitalWrite(LEDG, LOW);
|
|
digitalWrite(LEDB, HIGH);
|
|
|
|
// Sum all accelerations over the calibration period
|
|
calX += accelX;
|
|
calY += accelY;
|
|
calZ += accelZ;
|
|
|
|
// Set the calibration values to the mean over the calibration period
|
|
if (calibration == 0xFF)
|
|
{
|
|
calX /= calibration;
|
|
calY /= calibration;
|
|
calZ /= calibration;
|
|
}
|
|
}
|
|
// Post-Calibration Stage
|
|
else
|
|
{
|
|
// Adjust the result by the calibrated offset
|
|
accelX -= calX;
|
|
accelY -= calY;
|
|
accelZ -= calZ;
|
|
|
|
// Add the sample to the sample history
|
|
oneMinSamples[oneMinSampleIndex] = sqrt(accelX*accelX + accelY*accelY + accelZ*accelZ);
|
|
|
|
// Continuously calculate the maximum square of acceleration per minute
|
|
oneMinMax = arrMaxf(oneMinSamples, SAMPLES_PER_MINUTE);
|
|
if (DEBUG)
|
|
{
|
|
Serial.print("id:");
|
|
Serial.print(ID);
|
|
Serial.print(", equipmentStatus:");
|
|
Serial.print((oneMinMax >= THRESHOLD) ? "Taken":"Free");
|
|
Serial.print(", oneMinMax:");
|
|
Serial.print(oneMinMax);
|
|
Serial.print(", accelerationX:");
|
|
Serial.print(accelX);
|
|
Serial.print(", accelerationY:");
|
|
Serial.print(accelY);
|
|
Serial.print(", accelerationZ:");
|
|
Serial.print(accelZ);
|
|
Serial.print(", accelerationMag:");
|
|
Serial.println(oneMinSamples[oneMinSampleIndex]);
|
|
}
|
|
if (oneMinMax >= THRESHOLD)
|
|
{
|
|
digitalWrite(LEDR, HIGH);
|
|
digitalWrite(LEDG, LOW);
|
|
digitalWrite(LEDB, LOW);
|
|
}
|
|
else
|
|
{
|
|
digitalWrite(LEDR, LOW);
|
|
digitalWrite(LEDG, HIGH);
|
|
digitalWrite(LEDB, LOW);
|
|
}
|
|
}
|
|
|
|
// Calculate the time in microseconds the function took
|
|
deltaTime = micros() - startTime;
|
|
|
|
// 256 samples per minute
|
|
delayMicroseconds(234375 - deltaTime);
|
|
oneMinSampleIndex++;
|
|
}
|