Merge branch 'main' of ssh://gitlab.fdmci.hva.nl/propedeuse-hbo-ict/onderwijs/2023-2024/out-a-se-ti/blok-4/muupooviixee66 into 55-als-gebruiker-wil-ik-oefeningen-te-zien-krijgen-met-een-instructie-video-zodat-ik-weet-wat-ik

This commit is contained in:
Niels Gras
2024-05-21 14:21:05 +02:00
7 changed files with 86 additions and 53 deletions

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) + "\"}";

View File

@@ -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;
}

View File

@@ -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

View File

@@ -7,7 +7,7 @@ android {
defaultConfig {
applicationId "com.example.fitbot"
minSdk 29
minSdk 23
targetSdk 29
versionCode 1
versionName "1.0"

View File

@@ -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 {

View File

@@ -1,15 +1,19 @@
package com.example.fitbot.ui.activities;
import android.net.Uri;
import static com.example.fitbot.sports.Animations.Animate;
import android.os.Bundle;
import android.widget.MediaController;
import android.widget.VideoView;
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.EndScreenActivity;
import com.example.fitbot.R;
import com.example.fitbot.sports.Animations;
public class FitnessActivity extends RobotActivity implements RobotLifecycleCallbacks {
@@ -29,10 +33,15 @@ public class FitnessActivity extends RobotActivity implements RobotLifecycleCall
}
QiSDK.register(this, this);
}
@Override
public void onRobotFocusGained(QiContext qiContext) {
// Implement your logic when the robot focus is gained
Animate("bicepcurl", qiContext);
}
@Override
@@ -44,5 +53,12 @@ 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();
}
}

View File

@@ -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;