Smikkelbakje/J1B4-Fitbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch '51-als-gebruiker-wil-ik-dat-pepper-mee-beweegt-tijdens-oefeningen-zodat-ik-meer-motivatie-heb-om'

Browse Source
This commit is contained in:
Sam Hos
2024-05-21 13:50:50 +02:00
parent bad3346475
commit 8e92cbee7d
6 changed files with 84 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
code/arduino/Movement-sensor-code/Movement-sensor-code.ino
View File

@@ -1,6 +1,6 @@
#include "headerFile.h"
SensorManager::Rotation offset;
// SensorManager::Rotation offset;
void setup() {
Serial.begin(9600);
@@ -10,51 +10,49 @@ void setup() {
sensorManager.sensorSetup();
//ws server address, port and URL
webSocket.begin("145.3.245.22", 8001, "");
webSocket.begin("145.28.160.108", 8001, "");
// try every 500 again if connection has failed
webSocket.setReconnectInterval(500);
}
void loop() {
SensorManager::Rotation rotation = sensorManager.readLoop();
SensorManager::eulerAngles eulerRotation = sensorManager.getEulerAngles();
// Subtract offset
rotation.i -= offset.i;
rotation.j -= offset.j;
rotation.k -= offset.k;
rotation.w -= offset.w;
// rotation.i -= offset.i;
// rotation.j -= offset.j;
// rotation.k -= offset.k;
// rotation.w -= offset.w;
// Convert quaternion to Euler angles in radians
float roll = atan2(2.0f * (rotation.w * rotation.i + rotation.j * rotation.k), 1.0f - 2.0f * (rotation.i * rotation.i + rotation.j * rotation.j));
float pitch = asin(2.0f * (rotation.w * rotation.j - rotation.k * rotation.i));
float yaw = atan2(2.0f * (rotation.w * rotation.k + rotation.i * rotation.j), 1.0f - 2.0f * (rotation.j * rotation.j + rotation.k * rotation.k));
// Convert to degrees
float rollDegrees = roll * 180.0f / PI;
float pitchDegrees = pitch * 180.0f / PI;
float yawDegrees = yaw * 180.0f / PI;
// float rollDegrees = roll * 180.0f / PI;
// float pitchDegrees = pitch * 180.0f / PI;
// float yawDegrees = yaw * 180.0f / PI;
Serial.print(roll);
Serial.print(eulerRotation.roll);
Serial.print(" ");
Serial.print(pitch);
Serial.print(eulerRotation.pitch);
Serial.print(" ");
Serial.print(yaw);
sendData(roll, pitch, yaw);
Serial.print(eulerRotation.yaw);
sendData(eulerRotation.roll, eulerRotation.pitch, eulerRotation.yaw);
Serial.println();
webSocket.loop();
if (Serial.available()) {
String command = Serial.readStringUntil('\n');
command.trim(); // remove any trailing whitespace
if (command == "setZeroPoint") {
setZeroPoint();
}
}
}
void setZeroPoint() {
offset = sensorManager.readLoop();
}
// if (Serial.available()) {
// String command = Serial.readStringUntil('\n');
// command.trim(); // remove any trailing whitespace
// if (command == "setZeroPoint") {
// setZeroPoint();
// }
// }
// }
// void setZeroPoint() {
// offset = sensorManager.readLoop();
// }
void sendData(float roll, float pitch, float yaw){
String message = "{\"Sensor\": 1, \"roll\":\"" + String(roll) + "\",\"pitch\":\"" + String(pitch) + "\",\"yaw\":\"" + String(yaw) + "\"}";