diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java new file mode 100644 index 0000000..1815acc --- /dev/null +++ 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 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); + } +}