From 8e92cbee7d0ac6889a2230df45d13328c7e898d6 Mon Sep 17 00:00:00 2001 From: Sam Hos Date: Tue, 21 May 2024 13:50:50 +0200 Subject: [PATCH] Merge branch '51-als-gebruiker-wil-ik-dat-pepper-mee-beweegt-tijdens-oefeningen-zodat-ik-meer-motivatie-heb-om' --- .../Movement-sensor-code.ino | 54 +++++++++---------- .../Movement-sensor-code/SensorManager.cpp | 30 +++++++---- .../Movement-sensor-code/SensorManager.h | 32 ++++++----- .../fitbot/{ => sports}/Animations.java | 3 +- .../fitbot/ui/activities/FitnessActivity.java | 15 ++++++ .../fitbot/ui/activities/MainActivity.java | 2 + 6 files changed, 84 insertions(+), 52 deletions(-) rename code/src/Fitbot/app/src/main/java/com/example/fitbot/{ => sports}/Animations.java (91%) diff --git a/code/arduino/Movement-sensor-code/Movement-sensor-code.ino b/code/arduino/Movement-sensor-code/Movement-sensor-code.ino index 932d32d..3c0ab5e 100644 --- a/code/arduino/Movement-sensor-code/Movement-sensor-code.ino +++ b/code/arduino/Movement-sensor-code/Movement-sensor-code.ino @@ -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) + "\"}"; diff --git a/code/arduino/Movement-sensor-code/SensorManager.cpp b/code/arduino/Movement-sensor-code/SensorManager.cpp index 063f0da..50a0052 100644 --- a/code/arduino/Movement-sensor-code/SensorManager.cpp +++ b/code/arduino/Movement-sensor-code/SensorManager.cpp @@ -15,12 +15,12 @@ void SensorManager::sensorSetup() { //start sensorfunction and start autocalibration //once calibration is enabled it attempts to every 5 min - Wire.setClock(400000); //Increase I2C data rate to 400kHz - myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag - myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms - myIMU.enableMagnetometer(100); //Send data update every 100ms - myIMU.saveCalibration(); //Saves the current dynamic calibration data (DCD) to memory - myIMU.requestCalibrationStatus(); //Sends command to get the latest calibration status + Wire.setClock(400000); //Increase I2C data rate to 400kHz + myIMU.calibrateAll(); //Turn on cal for Accel, Gyro, and Mag + myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms + myIMU.enableMagnetometer(100); //Send data update every 100ms + myIMU.saveCalibration(); //Saves the current dynamic calibration data (DCD) to memory + myIMU.requestCalibrationStatus(); //Sends command to get the latest calibration status if (myIMU.calibrationComplete() == true) { Serial.println("Calibration data successfully stored"); @@ -29,23 +29,31 @@ void SensorManager::sensorSetup() { Serial.println(F("magnetometer rotation enabled")); } -SensorManager::Rotation SensorManager::readLoop() { +SensorManager::RotationQuintillions SensorManager::getQuintillions() { if (myIMU.dataAvailable() == true) { float i = myIMU.getQuatI(); float j = myIMU.getQuatJ(); float k = myIMU.getQuatK(); float w = myIMU.getQuatReal(); - Rotation rotation = { i, j, k, w }; + RotationQuintillions rotation = { i, j, k, w }; return rotation; - } - else { + } else { float i = myIMU.getQuatI(); float j = myIMU.getQuatJ(); float k = myIMU.getQuatK(); float w = myIMU.getQuatReal(); - Rotation rotation = { i, j, k, w }; + RotationQuintillions rotation = { i, j, k, w }; return rotation; } +} + +SensorManager::eulerAngles SensorManager::getEulerAngles() { + SensorManager::RotationQuintillions rotation = getQuintillions(); + 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)); + eulerAngles EulerAngles = { roll, pitch, yaw }; + return EulerAngles; } \ No newline at end of file diff --git a/code/arduino/Movement-sensor-code/SensorManager.h b/code/arduino/Movement-sensor-code/SensorManager.h index 3c4b231..048ed7f 100644 --- a/code/arduino/Movement-sensor-code/SensorManager.h +++ b/code/arduino/Movement-sensor-code/SensorManager.h @@ -5,18 +5,26 @@ #include "SparkFun_BNO080_Arduino_Library.h" class SensorManager { - public: - SensorManager(); - void sensorSetup(); - struct Rotation { - float i; - float j; - float k; - float w; - }; - Rotation readLoop(); - private: - BNO080 myIMU; +public: + SensorManager(); + void sensorSetup(); + struct eulerAngles { + float roll; + float pitch; + float yaw; + }; + eulerAngles getEulerAngles(); + + +private: + struct RotationQuintillions { + float i; + float j; + float k; + float w; + }; + RotationQuintillions getQuintillions(); + BNO080 myIMU; }; #endif \ No newline at end of file diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/Animations.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/sports/Animations.java similarity index 91% rename from code/src/Fitbot/app/src/main/java/com/example/fitbot/Animations.java rename to code/src/Fitbot/app/src/main/java/com/example/fitbot/sports/Animations.java index 3bfff8a..72ab436 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/Animations.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/sports/Animations.java @@ -1,4 +1,4 @@ -package com.example.fitbot; +package com.example.fitbot.sports; import android.support.v7.app.AppCompatActivity; @@ -7,6 +7,7 @@ import com.aldebaran.qi.sdk.builder.AnimateBuilder; import com.aldebaran.qi.sdk.builder.AnimationBuilder; import com.aldebaran.qi.sdk.object.actuation.Animate; import com.aldebaran.qi.sdk.object.actuation.Animation; +import com.example.fitbot.ui.activities.MainActivity; public class Animations extends AppCompatActivity { diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/FitnessActivity.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/FitnessActivity.java index 196a65b..dba7cfd 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/FitnessActivity.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/FitnessActivity.java @@ -1,20 +1,28 @@ package com.example.fitbot.ui.activities; +import static com.example.fitbot.sports.Animations.Animate; + import android.os.Bundle; import com.aldebaran.qi.sdk.QiContext; +import com.aldebaran.qi.sdk.QiSDK; import com.aldebaran.qi.sdk.RobotLifecycleCallbacks; import com.aldebaran.qi.sdk.design.activity.RobotActivity; +import com.example.fitbot.sports.Animations; public class FitnessActivity extends RobotActivity implements RobotLifecycleCallbacks { @Override protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); + QiSDK.register(this, this); + } @Override public void onRobotFocusGained(QiContext qiContext) { // Implement your logic when the robot focus is gained + Animate("bicepcurl", qiContext); + } @Override @@ -26,4 +34,11 @@ public class FitnessActivity extends RobotActivity implements RobotLifecycleCall public void onRobotFocusRefused(String reason) { // Implement your logic when the robot focus is refused } + + @Override + protected void onDestroy() { + QiSDK.unregister(this, this); + + super.onDestroy(); + } } diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/MainActivity.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/MainActivity.java index fe8ea23..ca9fd5c 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/MainActivity.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/MainActivity.java @@ -1,5 +1,7 @@ package com.example.fitbot.ui.activities; +import static com.example.fitbot.sports.Animations.Animate; + import android.annotation.SuppressLint; import android.content.Intent; import android.bluetooth.BluetoothAdapter;