From ed49392210601ffbd5d0c828aadb69ee76756b26 Mon Sep 17 00:00:00 2001 From: Luca Warmenhoven Date: Wed, 8 May 2024 15:41:04 +0200 Subject: [PATCH] Updated math behind path error calculation, fixed WebSocket not starting. --- .../java/com/example/fitbot/MainActivity.java | 9 +---- .../java/com/example/fitbot/MainScreen.java | 21 +++++++++-- .../fitbot/util/processing/GesturePath.java | 28 +++++++++++++++ .../fitbot/util/processing/MotionData.java | 17 +++++++++ .../util/processing/MotionProcessor.java | 33 +++++++++++------ .../fitbot/util/processing/Vector3.java | 36 +++++++++++++++++++ .../example/fitbot/util/server/WebSocket.java | 11 +++--- 7 files changed, 129 insertions(+), 26 deletions(-) create mode 100644 code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainActivity.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainActivity.java index e90e88c..4a77e68 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainActivity.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainActivity.java @@ -1,8 +1,6 @@ package com.example.fitbot; -import android.content.Intent; import android.os.Bundle; -import android.widget.Button; import com.aldebaran.qi.sdk.QiContext; import com.aldebaran.qi.sdk.QiSDK; @@ -14,7 +12,6 @@ import com.aldebaran.qi.sdk.object.conversation.Say; import com.aldebaran.qi.sdk.object.locale.Language; import com.aldebaran.qi.sdk.object.locale.Locale; import com.aldebaran.qi.sdk.object.locale.Region; -import com.example.fitbot.ui.SportMenuActivity; public class MainActivity extends RobotActivity implements RobotLifecycleCallbacks { @@ -22,12 +19,8 @@ public class MainActivity extends RobotActivity implements RobotLifecycleCallbac protected void onCreate(Bundle savedInstanceState) { super.onCreate(savedInstanceState); // Register the RobotLifecycleCallbacks to this Activity. - Button button = findViewById(R.id.menu_switch_btn); - button.setOnClickListener(v -> { - startActivity(new Intent(MainActivity.this, SportMenuActivity.class)); - }); - QiSDK.register(this, this); + QiSDK.register(this, this); } @Override diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java index 323641c..8b72054 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/MainScreen.java @@ -1,7 +1,6 @@ package com.example.fitbot; import android.annotation.SuppressLint; -import android.content.Intent; import android.os.Bundle; import android.support.design.widget.NavigationView; import android.support.v4.view.GravityCompat; @@ -9,9 +8,10 @@ import android.support.v4.widget.DrawerLayout; import android.support.v7.app.ActionBarDrawerToggle; import android.support.v7.app.AppCompatActivity; import android.support.v7.widget.Toolbar; -import android.widget.Button; -import com.example.fitbot.ui.SportMenuActivity; +import com.example.fitbot.util.processing.GesturePath; +import com.example.fitbot.util.processing.MotionProcessor; +import com.example.fitbot.util.processing.Vector3; public class MainScreen extends AppCompatActivity { @@ -31,6 +31,21 @@ public class MainScreen extends AppCompatActivity { navigationView = findViewById(R.id.nav_view); toolbar = findViewById(R.id.toolbar); + MotionProcessor motionProcessor = new MotionProcessor(); + motionProcessor.startListening(); + + motionProcessor.parsePacket("sampleRate 1"); + + Vector3[] path = new Vector3[100]; + + for ( int i = 0; i < 100; i++ ) { + path[i] = new Vector3(0, Math.sin(i * Math.PI / 50), 0); + motionProcessor.parsePacket("data 0;" + Math.cos(i * Math.PI / 50) + ";0;0;0;0"); + } + + motionProcessor.calculatePath(new GesturePath(path)); + motionProcessor.printData(); + /*---Tool Bar---*/ // setSupportActionBar(toolbar); diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java new file mode 100644 index 0000000..db15312 --- /dev/null +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/GesturePath.java @@ -0,0 +1,28 @@ +package com.example.fitbot.util.processing; + +public class GesturePath { + + public Vector3[] vectors; + + public GesturePath(Vector3[] vectors) + { + this.vectors = vectors; + } + + public Vector3 getError(Vector3 compare) { + Vector3 error = new Vector3(0, 0, 0); + double distance, previous = Double.MAX_VALUE; + + // Calculate the minimum distance between the vectors. + // This is used to determine the error. + for (int i = 0; i < vectors.length; i++) { + distance = vectors[i].distance(compare); + if ( distance < previous ) { + error = vectors[i]; + previous = distance; + } + } + return error; + } + +} diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java index 8278192..4c4ea0b 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/MotionData.java @@ -61,4 +61,21 @@ public class MotionData { ); } + /** + * Class for representing parsed motion data. + * This data is parsed in the `MotionProcessor` class. + */ + public static class ParsedVector { + public Vector3 relativePosition; + public Vector3 relativeError; + public ParsedVector(Vector3 relativePosition, Vector3 relativeError) { + this.relativePosition = relativePosition; + this.relativeError = relativeError; + } + + public String toString() { + return "position[" + relativePosition.toString() + "] error[" + relativeError.toString() + "]"; + } + } + } 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 9711e10..aac3d09 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 @@ -16,9 +16,10 @@ public class MotionProcessor { public static final String DELIMITER = ";"; private final List motionData = new ArrayList<>(); + private final List parsedData = new ArrayList<>(); private final Vector3[] relativePath = new Vector3[MAX_PATH_LENGTH]; private Vector3 ZERO = new Vector3(0, 0, 0); - private int sampleRate = 1; + private double sampleRate = 1.0D; // samples/second public MotionProcessor() {} @@ -31,7 +32,9 @@ public class MotionProcessor { */ public void startListening() { // Create socket server - WebSocket socket = WebSocket.createServer(80); + WebSocket socket = WebSocket.createServer(); + + Log.i("MotionProcessor", "Listening for incoming connections."); // Check if the socket if (socket != null) { @@ -50,9 +53,10 @@ public class MotionProcessor { * Function for parsing arbitrary packet data. * @param data The data to parse. */ - private void parsePacket(@NotNull String data) { + public void parsePacket(@NotNull String data) { // If the message starts with 'data', it's a data packet. if ( data.startsWith("data")) { + Log.i("MotionProcessor", "Received data packet: " + data.split(" ")[1]); MotionData parsedData = MotionData.decode(data.split(" ")[1]); if (parsedData != null) { this.motionData.add(parsedData); @@ -67,7 +71,7 @@ public class MotionProcessor { ); Log.i("MotionProcessor", "Device calibrated at " + ZERO.toString()); } else if ( data.startsWith("sampleRate")) { - this.sampleRate = Integer.parseInt(data.split(" ")[1]); + this.sampleRate = Double.parseDouble(data.split(" ")[1]); } } @@ -75,7 +79,7 @@ public class MotionProcessor { * Function for calculating the path of the device. * This only works when the ZERO point has been provided. */ - private void calculatePath() { + public void calculatePath(GesturePath path) { int offset = Math.max(0, motionData.size() - MAX_PATH_LENGTH); Vector3 previous = ZERO.copy(); @@ -83,13 +87,22 @@ public class MotionProcessor { for (int i = offset; i < motionData.size(); i++) { MotionData data = motionData.get(i); - // Get relative vector from the ZERO point Vector3 relativePosition = data.acceleration - .rotate(data.rotation.negate()) // Rotate acceleration vector with it's angle to get the acceleration relative to the vector of gravity - .map((vector) -> vector.) // Convert acceleration to relative spatial change + .rotate(data.rotation.negate()) // Rotate acceleration vector with it's negated angle to get the acceleration relative to the vector of gravity + .multiply(sampleRate * sampleRate / 6.0D) // Apply double integration to get the relative position + .add(previous); // Add the previous position to get the new position - relativePath[i - offset] + Vector3 relativeError = path.getError(relativePosition); + + previous = relativePosition; + + // Get relative vector from the ZERO point + parsedData.add(new MotionData.ParsedVector(relativePosition, relativeError)); + } + } + public void printData() { + for (MotionData.ParsedVector vector : parsedData) { + Log.i("MotionProcessor", vector.toString()); } } - } diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java index a862af8..1a8b38b 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/processing/Vector3.java @@ -26,6 +26,26 @@ public class Vector3 { return new Vector3(this.x, this.y, this.z); } + /** + * Get the magnitude of the vector. + * + * @return The magnitude of the vector. + */ + public double magnitude() { + return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z); + } + + /** + * Normalize the vector. + * + * @return The normalized vector. + */ + public Vector3 normalize() { + double mag = this.magnitude(); + if (mag == 0) throw new IllegalArgumentException("Cannot normalize the zero vector."); + return new Vector3(this.x / mag, this.y / mag, this.z / mag); + } + /** * Subtract the vector from another vector. * @@ -155,6 +175,16 @@ public class Vector3 { ); } + /** + * Get the distance between this vector and another vector. + * + * @param compare The other vector. + * @return The distance between the two vectors. + */ + public double distance(Vector3 compare) { + return Math.sqrt(Math.pow(compare.x - x, 2) + Math.pow(compare.y - y, 2) + Math.pow(compare.z - z, 2)); + } + public Vector3 map(VectorMapFunction function) { return function.apply(this); } @@ -162,4 +192,10 @@ public class Vector3 { public interface VectorMapFunction { Vector3 apply(Vector3 vector); } + + @Override + public String toString() + { + return "Vector3(" + this.x + ", " + this.y + ", " + this.z + ")"; + } } diff --git a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java index e48e1f0..0cc2957 100644 --- a/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java +++ b/code/src/Fitbot/app/src/main/java/com/example/fitbot/util/server/WebSocket.java @@ -24,14 +24,15 @@ public class WebSocket { /** * Function for creating a new WebSocket server given the provided port. - * @param port The port to configure the server to. * @return A WebSocket connection, or null if something went wrong. */ - public static @Nullable WebSocket createServer(int port) { + public static @Nullable WebSocket createServer() { try { - WebSocket socket = new WebSocket(); - socket.serverSocket = new ServerSocket(port); - return socket; + WebSocket webSocket = new WebSocket(); + webSocket.serverSocket = new ServerSocket(); + webSocket.serverSocket.bind(webSocket.serverSocket.getLocalSocketAddress()); + Log.i("WebSocket -- Creating new WebSocket server", "Server created: " + webSocket.serverSocket.getLocalSocketAddress() + ", " + webSocket.serverSocket.getLocalPort()); + return webSocket; } catch (IOException error) { String cause = error.getMessage() == null ? "Unknown reason" : error.getMessage();