diff --git a/code/arduino/Movement-sensor-code/Connectivity.cpp b/code/arduino/Movement-sensor-code/Connectivity.cpp index a1c30c4..367e114 100644 --- a/code/arduino/Movement-sensor-code/Connectivity.cpp +++ b/code/arduino/Movement-sensor-code/Connectivity.cpp @@ -4,7 +4,7 @@ void Connectivity::connectWiFi(char* ssid, char* pass){ WiFi.mode(WIFI_STA); WiFi.begin(ssid, pass); while (WiFi.status() != WL_CONNECTED) { - Serial.println("connecting to wifi"); + // Serial.println("connecting to wifi"); delay(1000); } Serial.println(WiFi.localIP()); diff --git a/code/arduino/Movement-sensor-code/Movement-sensor-code.ino b/code/arduino/Movement-sensor-code/Movement-sensor-code.ino index be47405..35b30bb 100644 --- a/code/arduino/Movement-sensor-code/Movement-sensor-code.ino +++ b/code/arduino/Movement-sensor-code/Movement-sensor-code.ino @@ -3,35 +3,38 @@ // SensorManager::Rotation offset; void setup() { - Serial.begin(9600); - Serial.println("startup"); + // Serial.begin(9600); + // Serial.println("startup"); //connect to internet and start sensor connectivity.connectWiFi(ssid, pass); sensorManager.sensorSetup(); - } +unsigned long lastTime = 0; // will store the last time the code was run + void loop() { SensorManager::eulerAngles eulerRotation = sensorManager.getEulerAngles(); SensorManager::acceleration rotationAcceleration = sensorManager.getAcelleration(); - unsigned long lastTime = 0; // will store the last time the code was run - - Serial.print(eulerRotation.roll); - Serial.print(" "); - Serial.print(eulerRotation.yaw); - Serial.print(" "); - Serial.print(eulerRotation.pitch); - Serial.println(); unsigned long currentTime = millis(); if (currentTime - lastTime >= 100) { // 100 ms has passed - String message = "{\"deviceId\": 1, \"rotationX\":\"" + String(eulerRotation.roll) + "\",\"rotationY\":\"" + String(eulerRotation.pitch) + "\",\"rotationZ\":\"" + String(eulerRotation.yaw) + "\",\"accelerationX\":\"" + String(rotationAcceleration.x) + "\",\"accelerationY\":\"" + String(rotationAcceleration.y) + "\",\"accelerationZ\":\"" + String(rotationAcceleration.z) + "\",\"type\":\"data\"}"; - Serial.println(connectivity.httpPost("192.168.137.146", "/", 3445, message.c_str(), message.length(), "json")); - Serial.println(message); + memset(buffer, 0, BUFFER_SIZE); + sprintf( + buffer, + "{\"deviceId\": %d, \"rotationX\": %f, \"rotationY\": %f, \"rotationZ\": %f, \"accelerationX\": %f, \"accelerationY\": %f, \"accelerationZ\": %f, \"type\": %s}", + DEVICE_ID, + eulerRotation.roll, + eulerRotation.pitch, + eulerRotation.yaw, + rotationAcceleration.x, + rotationAcceleration.y, + rotationAcceleration.z, + "data"); + // %d = int, %f = floatation, %s = string + connectivity.httpPost("192.168.137.45", "/", 3445, buffer, strlen(buffer), "application/json"); lastTime = currentTime; -} + } } //acceleration.X //acceleration.Y //acceleration.Z - diff --git a/code/arduino/Movement-sensor-code/SensorManager.cpp b/code/arduino/Movement-sensor-code/SensorManager.cpp index 075e5ce..77836e4 100644 --- a/code/arduino/Movement-sensor-code/SensorManager.cpp +++ b/code/arduino/Movement-sensor-code/SensorManager.cpp @@ -9,15 +9,15 @@ void SensorManager::sensorSetup() { //wait for the sensor to start before continue if (myIMU.begin() == false) { delay(1000); - Serial.println("."); + // Serial.println("."); } //start sensorfunction and start autocalibration //once calibration is enabled it attempts to every 5 min - Wire.setClock(400000); myIMU.enableGyroIntegratedRotationVector(100); //send data every 100ms myIMU.enableAccelerometer(100); //Send data update every 100ms Serial.println(F("magnetometer rotation enabled")); + myIMU.enableStepCounter(500); //Send data update every 500ms } //get sensordata SensorManager::RotationQuintillions SensorManager::getQuintillions() { @@ -39,7 +39,7 @@ SensorManager::RotationQuintillions SensorManager::getQuintillions() { return rotation; } } -//calculate Quintillions to Euler angles from -1π to +1π +//calculate Quintillions to Euler angles from -1π to +1π 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)); @@ -48,11 +48,24 @@ SensorManager::eulerAngles SensorManager::getEulerAngles() { eulerAngles EulerAngles = { roll, pitch, yaw }; return EulerAngles; } -SensorManager::acceleration SensorManager::getAcelleration(){ - float x = myIMU.getAccelX(); - float y = myIMU.getAccelY(); - float z = myIMU.getAccelZ(); - acceleration Acceleration = { x, y, z }; +SensorManager::acceleration SensorManager::getAcelleration() { + float x = myIMU.getAccelX(); + float y = myIMU.getAccelY(); + float z = myIMU.getAccelZ(); + acceleration Acceleration = { x, y, z }; - return Acceleration; + return Acceleration; +} + +bool SensorManager::sensorTap() { + int taps = 0; + if (myIMU.dataAvailable() == true) { + int taps = myIMU.getStepCount(); + } + if (taps) { + return true; + } + else { + return false; + } } \ 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 6a8a796..aaf4d71 100644 --- a/code/arduino/Movement-sensor-code/SensorManager.h +++ b/code/arduino/Movement-sensor-code/SensorManager.h @@ -21,7 +21,7 @@ public: eulerAngles getEulerAngles(); acceleration getAcelleration(); - + bool sensorTap(); private: struct RotationQuintillions { float i; diff --git a/code/arduino/Movement-sensor-code/headerFIle.h b/code/arduino/Movement-sensor-code/headerFIle.h index b7b8220..b065b3e 100644 --- a/code/arduino/Movement-sensor-code/headerFIle.h +++ b/code/arduino/Movement-sensor-code/headerFIle.h @@ -8,6 +8,8 @@ Connectivity connectivity; WebSocketsClient webSocket; #define USE_SERIAL Serial - #define ssid "1235678i" #define pass "12345678" +#define BUFFER_SIZE 1024 +#define DEVICE_ID 1 +char *buffer = (char *)malloc(sizeof(char) * BUFFER_SIZE); diff --git a/code/src/Fitbot/.idea/misc.xml b/code/src/Fitbot/.idea/misc.xml index 1807d5f..4bb90e7 100644 --- a/code/src/Fitbot/.idea/misc.xml +++ b/code/src/Fitbot/.idea/misc.xml @@ -16,6 +16,7 @@ + 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 0087588..89a857f 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 @@ -71,8 +71,10 @@ public class FitnessActivity extends RobotActivity implements RobotLifecycleCall public void onRobotFocusGained(QiContext qiContext) { // Find the VideoView by its ID - CompletableFuture.runAsync(() -> FitnessCycle.executeMovement("bicepcurl", 10, qiContext)); +// CompletableFuture.runAsync(() -> FitnessCycle.executeMovement("bicepcurl", 10, qiContext)); + Log.i("Motion", "qiContext provided"); personalMotionPreviewElement.provideQiContext(qiContext); + // FitnessCycle.playVideo(qiContext, videoView, this); } @@ -89,5 +91,7 @@ public class FitnessActivity extends RobotActivity implements RobotLifecycleCall @Override protected void onDestroy() { super.onDestroy(); + QiSDK.unregister(this, this); + this.personalMotionPreviewElement.onDestroy(); } } \ No newline at end of file diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/HelpActivity.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/HelpActivity.java index def4026..3ffa2e3 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/HelpActivity.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/activities/HelpActivity.java @@ -4,6 +4,7 @@ import android.support.v7.app.AppCompatActivity; import android.os.Bundle; import com.example.fitbot.R; +import com.example.fitbot.util.ButtonNavigation; public class HelpActivity extends AppCompatActivity { @@ -11,5 +12,8 @@ public class HelpActivity extends AppCompatActivity { protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); setContentView(R.layout.activity_help); + + ButtonNavigation.setupButtonNavigation(this, R.id.homeButton, MainActivity.class); + } } \ No newline at end of file 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 2297d65..50c6df2 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 @@ -16,6 +16,7 @@ import android.view.WindowManager; import android.widget.Button; import com.example.fitbot.R; +import com.example.fitbot.util.ButtonNavigation; public class MainActivity extends AppCompatActivity { @@ -60,6 +61,8 @@ public class MainActivity extends AppCompatActivity { if (getSupportActionBar() != null) { getSupportActionBar().hide(); } + ButtonNavigation.setupButtonNavigation(this, R.id.startButton, FitnessActivity.class); + ButtonNavigation.setupButtonNavigation(this, R.id.helpButton, HelpActivity.class); /*---Tool Bar---*/ setSupportActionBar(toolbar); // Make the toolbar act as the action bar diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java index 1601899..141b0bc 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/ui/components/PersonalMotionPreviewElement.java @@ -2,7 +2,6 @@ package com.example.fitbot.ui.components; import android.content.Context; import android.graphics.Canvas; -import android.graphics.Color; import android.graphics.Paint; import android.graphics.Path; import android.util.AttributeSet; @@ -13,8 +12,6 @@ import com.aldebaran.qi.sdk.QiContext; import com.example.fitbot.exercise.Exercise; import com.example.fitbot.util.FitnessCycle; import com.example.fitbot.util.path.GesturePath; -import com.example.fitbot.util.path.PathSegment; -import com.example.fitbot.util.processing.MotionData; import com.example.fitbot.util.processing.MotionProcessor; import org.joml.Matrix4f; @@ -22,8 +19,8 @@ import org.joml.Vector2f; import org.joml.Vector3f; import org.joml.Vector4f; +import java.util.concurrent.ConcurrentLinkedQueue; import java.util.concurrent.atomic.AtomicInteger; -import java.util.concurrent.atomic.AtomicReference; public class PersonalMotionPreviewElement extends View { @@ -44,6 +41,16 @@ public class PersonalMotionPreviewElement extends View { private final Paint targetPaint = new Paint(); private final Paint backgroundColor = new Paint(); + private Matrix4f modelMatrix = new Matrix4f(); // The model view matrix for the 3D to 2D transformation. + private Matrix4f viewMatrix = new Matrix4f() + .lookAt(new Vector3f(4, 4, 4), new Vector3f(0, 0, 0), new Vector3f(0, 1, 0)); // The view matrix for the 3D to 2D transformation. + private Matrix4f projectionMatrix = new Matrix4f(); // The projection matrix for the 3D to 2D transformation. + private final Vector4f objectPosition = new Vector4f(0, 0, 0, 1); // The location of the object in 3D space. + private ConcurrentLinkedQueue vectors = new ConcurrentLinkedQueue<>(); + + private Vector2f[] axisVectors; + + private static final String[] USER_PHRASES = { "Veel success met de oefening!", @@ -92,6 +99,25 @@ public class PersonalMotionPreviewElement extends View { this.exercise = exercise; this.paths = exercise.getPath(); + + this.axisVectors = new Vector2f[] { + + projectVertex(new Vector3f(-100.0f, 0, 0), getWidth(), getHeight()), + projectVertex(new Vector3f(100.0f, 0, 0), getWidth(), getHeight()), + projectVertex(new Vector3f(0, -100.0f, 0), getWidth(), getHeight()), + projectVertex(new Vector3f(0, 100.0f, 0), getWidth(), getHeight()), + projectVertex(new Vector3f(0, 0, -100.0f), getWidth(), getHeight()), + projectVertex(new Vector3f(0, 0, 100.0f), getWidth(), getHeight()) + + }; + } + + public void onDestroy() + { + if ( this.motionProcessor != null ) + this.motionProcessor.stopListening(); + + this.motionProcessor = null; } /** @@ -114,6 +140,11 @@ public class PersonalMotionPreviewElement extends View { int progress = (int)this.motionProcessor.getError(this.paths[0], processed); this.exerciseProgress.set(Math.min(1000, Math.max(0, progress))); Log.i("MotionProcessor", "Processed data: " + progress + " (" + preprocessed + ")"); + Vector2f parsed = projectVertex(processed, this.getWidth(), this.getHeight()); + this.vectors.add(parsed); + // Remove the first element if the array is too big + if (this.vectors.size() > 100) + this.vectors.poll(); }); saySomethingNice(); } @@ -139,6 +170,31 @@ public class PersonalMotionPreviewElement extends View { } + private Vector2f projectVertex(Vector3f point, int virtualWidth, int virtualHeight) { + modelMatrix + .identity() + .translate(-objectPosition.x, -objectPosition.y, -objectPosition.z); + + // Transform the projection matrix to a perspective projection matrix + // Perspective transformation conserves the depth of the object + projectionMatrix + .identity() + .perspective((float) Math.toRadians(70), (float) virtualWidth / virtualHeight, .01f, 10000.0f); + + // Convert world coordinates to screen-space using MVP matrix + Vector4f screenCoordinates = new Vector4f(point, 1.0f) + .mul(this.projectionMatrix) + .mul(this.viewMatrix) + .mul(this.modelMatrix); + + // Normalize screen coordinates from (-1, 1) to (0, virtualWidth) and (0, virtualHeight) + float normalizedX = (screenCoordinates.x / screenCoordinates.w + 1.0f) * 0.5f * virtualWidth; + float normalizedY = (1.0f - screenCoordinates.y / screenCoordinates.w) * 0.5f * virtualHeight; + return new Vector2f(normalizedX, normalizedY); + } + + + @Override public void onDraw(Canvas canvas) { canvas.drawRect(0, 0, getWidth(), getHeight(), backgroundColor); @@ -146,6 +202,16 @@ public class PersonalMotionPreviewElement extends View { if (this.exercise == null) return; + for (int i = 0; i < axisVectors.length/2; i++) + { + canvas.drawLine(axisVectors[i*2].x, axisVectors[i*2].y, axisVectors[i*2+1].x, axisVectors[i*2+1].y, this.targetPaint); + } + + for ( Vector2f point : this.vectors) + { + canvas.drawRect(point.x, point.y, point.x + 5, point.y + 5, this.referencePaint); + } +/* // Draw target circle float targetRadius = (this.screenDimensions.x + this.screenDimensions.y) / 5.0f; canvas.drawCircle(this.screenDimensions.x / 2, this.screenDimensions.y / 2, targetRadius, this.targetPaint); @@ -157,7 +223,7 @@ public class PersonalMotionPreviewElement extends View { (int)(255 * exerciseProgress.get()/1000.0f), 0 ) - ); + );*/ this.invalidate(); diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java index 41f6363..de35e7d 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionProcessor.java @@ -9,6 +9,8 @@ import com.google.gson.JsonObject; import com.google.gson.JsonParser; import org.jetbrains.annotations.NotNull; +import org.joml.Matrix3d; +import org.joml.Vector3d; import org.joml.Vector3f; import java.util.ArrayList; @@ -24,7 +26,7 @@ public class MotionProcessor { private Vector3f ZERO = new Vector3f(0, 0, 0); - private final float sampleRate = 10.0F; // samples/second + private final float sampleRate = 1.0f / 10.0F; // samples/second private IMotionDataConsumer motionDataConsumer = (p1, p2, p3, p4, p5) -> { }; private WebServer server; @@ -71,6 +73,9 @@ public class MotionProcessor { public void parsePacket(@NotNull String data) { try { + + Log.i("MotionProcessor", "Received packet data: " + data); + JsonElement json = JsonParser.parseString(data); if (!json.isJsonObject()) @@ -105,7 +110,7 @@ public class MotionProcessor { addMotionData(motionData); } catch (Exception e) { - // Don't do anything ... just ignore the exception + Log.i("MotionProcessor", "Failed to parse packet data."); } } @@ -161,12 +166,16 @@ public class MotionProcessor { // Rotate the acceleration vector back by the rotation vector to make it // perpendicular to the gravity vector, then apply double integration to get the relative position. // s = 1/2 * a * t^2 - return motionData.acceleration - .rotateX(-motionData.rotation.x) - .rotateY(-motionData.rotation.y) + // Step 2: Create rotation matrices for each axis + // Step 4: Rotate the acceleration vector + + return motionData.rotation + .mul(5); + /*return motionData.acceleration .rotateZ(-motionData.rotation.z) - .div(2) - .mul(sampleRate * sampleRate); + .rotateY(-motionData.rotation.y) + .rotateX(-motionData.rotation.x) + .mul(sampleRate * sampleRate / 2);*/ } /** diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebServer.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebServer.java index 7362113..7fe001a 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebServer.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebServer.java @@ -6,6 +6,7 @@ import java.io.BufferedReader; import java.io.IOException; import java.io.InputStream; import java.io.InputStreamReader; +import java.io.OutputStream; import java.net.InetSocketAddress; import java.net.ServerSocket; import java.net.Socket; @@ -63,6 +64,7 @@ public class WebServer implements Runnable { // Find a new connection Socket newSocket = this.serverSocket.accept(); InputStream streamIn = newSocket.getInputStream(); + OutputStream streamOut = newSocket.getOutputStream(); // Read the incoming data BufferedReader reader = new BufferedReader(new InputStreamReader(streamIn)); @@ -71,7 +73,14 @@ public class WebServer implements Runnable { while ((line = reader.readLine()) != null) builder.append(line).append("\n"); + // Send generic message back + streamOut.write("HTTP/1.1 200 OK\n".getBytes()); + streamOut.write("Content-Type: text/html\n".getBytes()); + streamOut.write("Connection: close\n".getBytes()); + streamIn.close(); // Closes the reader, stream and socket connection. + streamOut.close(); + newSocket.close(); String[] data = builder.toString().split("\n\n"); diff --git a/code/src/Fitbot/app/src/main/res/layout/activity_end_screen.xml b/code/src/Fitbot/app/src/main/res/layout/activity_end_screen.xml index fd3a2e9..b07bcad 100644 --- a/code/src/Fitbot/app/src/main/res/layout/activity_end_screen.xml +++ b/code/src/Fitbot/app/src/main/res/layout/activity_end_screen.xml @@ -56,14 +56,12 @@ style="@style/TextStyle" android:layout_width="wrap_content" android:layout_height="wrap_content" - android:layout_marginStart="539dp" - android:layout_marginEnd="533dp" android:text="Gefeliciteerd" - app:layout_constraintBottom_toTopOf="@+id/workoutText" + app:layout_constraintBottom_toBottomOf="parent" app:layout_constraintEnd_toEndOf="parent" - app:layout_constraintHorizontal_bias="0.0" app:layout_constraintStart_toStartOf="parent" - app:layout_constraintTop_toTopOf="@+id/myRectangleView" /> + app:layout_constraintTop_toTopOf="parent" + app:layout_constraintVertical_bias="0.155" /> + + +