Experiment: Mocked up concept for telemetry system that can be received by any LAN connected device

2025-12-12 23:32:36 +00:00 · 2024-01-26 00:15:41 -06:00
parent 633f6fa0f8
commit 06228b955c
1 changed files with 151 additions and 0 deletions
--- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java
+++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java
@@ -0,0 +1,151 @@
+package dev.cyberarm.engine.V2;
+
+import android.util.Log;
+
+import java.io.ByteArrayOutputStream;
+import java.io.IOException;
+import java.nio.charset.StandardCharsets;
+import java.util.ArrayList;
+import java.util.Locale;
+
+/**
+ * One way (Robot Controller -> Client), UDP based, telemetry via multicast.
+ */
+public class CyberarmTelemetry {
+    private static final int PROTOCOL_IDENTIFIER = 0x54494d45;
+    private static final int MAX_PACKET_RAW_SIZE = 508; // bytes
+    private static final int PACKET_HEADER_SIZE = 16; // bytes
+    private static final int MAX_PACKET_BODY_SIZE = MAX_PACKET_RAW_SIZE - PACKET_HEADER_SIZE; // bytes
+    private static final String TAG = "CYBERARM_TELEMETRY";
+    private final ArrayList<ByteArrayOutputStream> queueBuffer = new ArrayList<>();
+    enum Encode {
+        // Generic/Type encodings
+        INTEGER,
+        LONG,
+        FLOAT,
+        DOUBLE,
+        STRING,
+        BOOLEAN,
+
+        // Special encodings
+        POSE_POSITION, // position of robot in 2D space, relative to field origin
+        TELEMETRY, // string telemetry
+        GAMEPAD, // all 15 buttons + joysticks + triggers input values
+        MOTOR, // current power, velocity, position, target position, and current (amps)
+        SERVO, // current target position
+        SENSOR_2M_DISTANCE, // Rev 2 meter distance sensor
+        SENSOR_DIGITAL, // touch or other digital/binary sensor
+    }
+
+    public void publish() throws IOException
+    {
+        ByteArrayOutputStream buffer = new ByteArrayOutputStream();
+
+        for (ByteArrayOutputStream data : queueBuffer) {
+            if (buffer.size() + data.size() >= MAX_PACKET_BODY_SIZE) {
+                ByteArrayOutputStream output = new ByteArrayOutputStream();
+
+                packHeader(output, buffer);
+                commitPacket(output);
+
+                buffer.reset();
+            }
+
+            buffer.write(data.toByteArray());
+        }
+
+        // We're a lossy protocol, assume that all data in the buffer has been processed and committed
+        queueBuffer.clear();
+    }
+
+    public void addPose(double x, double y, double r) {
+        addPosition(x, y, r);
+    }
+
+    public void addPosition(double x, double y, double r) {
+        try {
+            ByteArrayOutputStream buffer = new ByteArrayOutputStream();
+
+            buffer.write(Encode.POSE_POSITION.ordinal());
+            packDouble(buffer, x);
+            packDouble(buffer, y);
+            packDouble(buffer, r);
+
+            queueBuffer.add(buffer);
+        } catch (IOException e) {
+            Log.e(TAG, "An error occurred while adding robot Pose (Position) to queue");
+            e.printStackTrace();
+        }
+    }
+
+    public void addTelemetry(String msg) {
+        try {
+            ByteArrayOutputStream buffer = new ByteArrayOutputStream();
+
+            buffer.write(Encode.TELEMETRY.ordinal());
+            packString(buffer, msg);
+
+            queueBuffer.add(buffer);
+        } catch (IOException e) {
+            Log.e(TAG, "An error occurred while adding Telemetry message to queue");
+            e.printStackTrace();
+        }
+    }
+
+    // ------- HELPERS -------- //
+    private void packHeader(ByteArrayOutputStream output, ByteArrayOutputStream buffer) throws IOException {
+        output.write(PROTOCOL_IDENTIFIER);
+        output.write(buffer.size()); // BUFFER SIZE (multiple buffers may fit in one packet)
+        output.write(buffer.toByteArray());
+    }
+
+    private void commitPacket(ByteArrayOutputStream buffer) {
+        // TODO: send multicast packet(s) on LAN
+    }
+
+    private void packInt(ByteArrayOutputStream stream, int i) throws IOException {
+        stream.write(Encode.INTEGER.ordinal());
+        stream.write(i);
+    }
+
+    private void packLong(ByteArrayOutputStream stream, long i) throws IOException {
+        stream.write(Encode.LONG.ordinal());
+        stream.write(Long.toBinaryString(i).getBytes());
+    }
+
+    private void packFloat(ByteArrayOutputStream stream, float i, int precision) throws IOException {
+        stream.write(Encode.FLOAT.ordinal());
+        String string = String.format("%." + precision + "f", i, Locale.US);
+        stream.write(string.length());
+        stream.write(string.getBytes(StandardCharsets.UTF_8));
+    }
+
+    private void packFloat(ByteArrayOutputStream stream, float i) throws IOException
+    {
+        packFloat(stream, i, 3);
+    }
+
+    private void packDouble(ByteArrayOutputStream stream, double i, int precision) throws IOException {
+        stream.write(Encode.DOUBLE.ordinal());
+        String string = String.format("%." + precision + "f", i, Locale.US);
+        stream.write(string.length());
+        stream.write(string.getBytes(StandardCharsets.UTF_8));
+    }
+
+    private void packDouble(ByteArrayOutputStream stream, double i) throws IOException
+    {
+        packDouble(stream, i, 3);
+    }
+
+    private void packString(ByteArrayOutputStream stream, String i) throws IOException {
+        stream.write(Encode.STRING.ordinal());
+        stream.write(i.length());
+        stream.write(i.getBytes(StandardCharsets.UTF_8));
+    }
+
+    private void packBoolean(ByteArrayOutputStream stream, boolean i) throws IOException {
+        stream.write(Encode.BOOLEAN.ordinal());
+        byte[] bit = {(byte) (i ? 1 : 0)};
+        stream.write(bit);
+    }
+}