This repository has been archived by the owner on Sep 24, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
stroke-profiler.ino
99 lines (82 loc) · 3.29 KB
/
stroke-profiler.ino
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
/*!
* This file is part of the 'Stroke Profiler' software library.
* Copyright (C) 2019 Tom Udding. All rights reserved.
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 3 of the License, or (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
* You should have received a copy of the GNU Lesser General Public License
* along with this program; if not, write to the Free Software Foundation,
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
*/
#include <ArduinoBLE.h>
#include <Arduino_LSM9DS1.h>
#include <vector>
// define local device as peripheral
BLEService profilerService("12566370-6212-4161-A625-566743024140");
BLEFloatCharacteristic imuCharacteristic("12566370-6212-4161-B625-566743024140", BLERead | BLENotify);
void setup() {
Serial.begin(115200);
pinMode(LED_BUILTIN, OUTPUT);
// begin BLE initialisation
if (!BLE.begin()) {
Serial.println("Failed to initialise BLE!");
while(1);
}
// begin IMU initialisation
if (!IMU.begin()) {
Serial.println("Failed to initialise IMU!");
while(1);
}
// set advertised local name and service UUID, add characteristic
// to the service, attach the service, and start advertising
BLE.setLocalName("Stroke Profiler");
BLE.setAdvertisedService(profilerService);
profilerService.addCharacteristic(imuCharacteristic);
BLE.addService(profilerService);
imuCharacteristic.writeValue(0);
BLE.advertise();
// Arduino is ready for use
Serial.println("Stroke Profiler is now active...");
}
void loop() {
// listen for BLE centrals to connect
BLEDevice central = BLE.central();
if (central) {
Serial.print("Connected to central: "); Serial.println(central.address());
digitalWrite(LED_BUILTIN, HIGH);
std::vector<float> data_vector(2);
// as long as connected to central do
while (central.connected()) {
getIMUVector(data_vector);
for (auto &i : data_vector) {
imuCharacteristic.writeValue(i);
}
}
// disconnected from central
digitalWrite(LED_BUILTIN, LOW);
Serial.print("Disconnected from central: "); Serial.println(central.address());
}
}
// generate vector from IMU data
void getIMUVector(std::vector<float> &data_vector) {
float gyro_x, gyro_y, gyro_z, accl_x, accl_y, accl_z;
if (IMU.gyroscopeAvailable()) {
IMU.readGyroscope(gyro_x, gyro_y, gyro_z);
data_vector[0] = map_float(gyro_x, -2000, 2000, -1, 1);
}
if (IMU.accelerationAvailable()) {
IMU.readAcceleration(accl_x, accl_y, accl_z);
data_vector[1] = map_float(accl_z, -4, 4, -1, 1);
}
}
// fixed version of Arduino's map() function to allow for floats
float map_float(float x, float min_in, float max_in, float min_out, float max_out) {
return (x - min_in) * (max_out - min_out) / (max_in - min_in) + min_out;
}