From dca17c786d49cff58ee1c5f2ba101453acf34aea Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Fri, 26 Jan 2024 20:53:59 -0600 Subject: [PATCH] RedCrab: First pass of odometry/localization --- .../cyberarm/engine/V2/CyberarmTelemetry.java | 75 ++++++++++++---- .../cyberarm/minibots/red_crab/Localizer.java | 87 +++++++++++++++++++ .../minibots/red_crab/RedCrabMinibot.java | 37 ++++++++ .../engines/RedCrabAutonomousEngine.java | 3 + .../red_crab/engines/RedCrabTeleOpEngine.java | 3 + 5 files changed, 187 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java diff --git a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java index 1815acc..60b04b6 100644 --- a/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java +++ b/TeamCode/src/main/java/dev/cyberarm/engine/V2/CyberarmTelemetry.java @@ -4,6 +4,9 @@ import android.util.Log; import java.io.ByteArrayOutputStream; import java.io.IOException; +import java.net.DatagramPacket; +import java.net.InetAddress; +import java.net.MulticastSocket; import java.nio.charset.StandardCharsets; import java.util.ArrayList; import java.util.Locale; @@ -16,8 +19,16 @@ public class CyberarmTelemetry { 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 MULTICAST_ADDRESS = "236.77.83.76"; + private static final int MULTICAST_PORT = 6388; + private static final int MULTICAST_TTL = 5; private static final String TAG = "CYBERARM_TELEMETRY"; private final ArrayList queueBuffer = new ArrayList<>(); + private InetAddress group; + private MulticastSocket multicastSocket; + + private boolean usable = false; + enum Encode { // Generic/Type encodings INTEGER, @@ -33,10 +44,35 @@ public class CyberarmTelemetry { GAMEPAD, // all 15 buttons + joysticks + triggers input values MOTOR, // current power, velocity, position, target position, and current (amps) SERVO, // current target position + CONTINUOUS_SERVO, // current power SENSOR_2M_DISTANCE, // Rev 2 meter distance sensor SENSOR_DIGITAL, // touch or other digital/binary sensor } + CyberarmTelemetry() { + try { + this.group = InetAddress.getByName(MULTICAST_ADDRESS); + this.multicastSocket = new MulticastSocket(MULTICAST_PORT); + this.multicastSocket.setTimeToLive(MULTICAST_TTL); + this.multicastSocket.joinGroup(group); + + this.usable = true; + } catch (IOException e) { + Log.e(TAG, "FAILED to create multicast socket!"); + e.printStackTrace(); + + this.usable = false; + } + } + + /** + * Whether the multicast socket was successfully created + * @return usable + */ + public boolean isUsable() { + return usable; + } + public void publish() throws IOException { ByteArrayOutputStream buffer = new ByteArrayOutputStream(); @@ -99,8 +135,15 @@ public class CyberarmTelemetry { output.write(buffer.toByteArray()); } - private void commitPacket(ByteArrayOutputStream buffer) { + private void commitPacket(ByteArrayOutputStream buffer) throws IOException { // TODO: send multicast packet(s) on LAN + + // Drop packet if multicast socket is not yet created or failed to be created. + if (!isUsable()) + return; + + DatagramPacket packet = new DatagramPacket(buffer.toByteArray(), buffer.size(), group, MULTICAST_PORT); + multicastSocket.send(packet); } private void packInt(ByteArrayOutputStream stream, int i) throws IOException { @@ -113,28 +156,24 @@ public class CyberarmTelemetry { stream.write(Long.toBinaryString(i).getBytes()); } - private void packFloat(ByteArrayOutputStream stream, float i, int precision) throws IOException { + private void packFloat(ByteArrayOutputStream stream, float i) 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)); + String[] string = String.format(Locale.US, "%.8f", i).split("\\."); + int wholeNum = Integer.parseInt(string[0]); + int decimalPart = Integer.parseInt(string[1]); + + stream.write(wholeNum); + stream.write(decimalPart); } - private void packFloat(ByteArrayOutputStream stream, float i) throws IOException - { - packFloat(stream, i, 3); - } - - private void packDouble(ByteArrayOutputStream stream, double i, int precision) throws IOException { + private void packDouble(ByteArrayOutputStream stream, double i) 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)); - } + String[] string = String.format(Locale.US, "%.8f", i).split("\\."); + int wholeNum = Integer.parseInt(string[0]); + int decimalPart = Integer.parseInt(string[1]); - private void packDouble(ByteArrayOutputStream stream, double i) throws IOException - { - packDouble(stream, i, 3); + stream.write(wholeNum); + stream.write(decimalPart); } private void packString(ByteArrayOutputStream stream, String i) throws IOException { diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java new file mode 100644 index 0000000..e213c40 --- /dev/null +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java @@ -0,0 +1,87 @@ +package dev.cyberarm.minibots.red_crab; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.slf4j.helpers.Util; + +import dev.cyberarm.engine.V2.Utilities; + +public class Localizer { + private final RedCrabMinibot robot; + private double rawX = 0, rawY = 0, rawR = 0; + private double trackWidthMM = 365.0, forwardOffsetMM = -140.0, wheelDiameterMM = 90.0; + private int lastEncoderXLeft = 0, lastEncoderXRight = 0, lastEncoderYCenter = 0; + + public Localizer(RedCrabMinibot robot) { + this.robot = robot; + + // Preset last encoder to current location to not require resetting encoders, ever. (🤞) + this.lastEncoderXLeft = robot.deadWheelXLeft.getCurrentPosition(); + this.lastEncoderXRight = robot.deadWheelXRight.getCurrentPosition(); + this.lastEncoderYCenter = robot.deadWheelYCenter.getCurrentPosition(); + } + + public void reset() { + rawX = 0; + rawY = 0; + rawR = 0; + } + + // FIXME + public void teleport(double xMM, double yMM) { + this.rawX = xMM; + this.rawY = yMM; + } + + // FIXME + public void teleport(double xMM, double yMM, double headingDegrees) { + this.rawX = xMM; + this.rawY = yMM; + this.rawR = (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :( + } + + public void integrate() { + int leftEncoder = robot.deadWheelXLeft.getCurrentPosition(); + int rightEncoder = robot.deadWheelXRight.getCurrentPosition(); + int centerEncoder = robot.deadWheelYCenter.getCurrentPosition(); + + double heading = robot.imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); + + int deltaLeft = leftEncoder - lastEncoderXLeft; + int deltaRight = rightEncoder - lastEncoderXRight; + int deltaCenter = centerEncoder - lastEncoderYCenter; + + double phi = (deltaLeft - deltaRight) / trackWidthMM; + double deltaMiddle = (deltaLeft + deltaRight) / 2.0; + double deltaPerp = deltaCenter - forwardOffsetMM * phi; + + double deltaX = deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading); + double deltaY = deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading); + + rawX += deltaX; + rawY += deltaY; + rawR += phi; + + lastEncoderXLeft = leftEncoder; + lastEncoderXRight = rightEncoder; + lastEncoderYCenter = centerEncoder; + } + + public double xMM() { + return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawX); + } + + public double yMM() { + return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawY); + } + + // FIXME + public double headingDegrees() { + return rawR; + } + + // FIXME? (report radians as halves or proper whole?) + public double headingRadians() { + return rawR; + } +} diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java index 1e4438b..c13443c 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/RedCrabMinibot.java @@ -20,6 +20,7 @@ import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfig import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Action; import org.timecrafters.TimeCraftersConfigurationTool.library.backend.config.Variable; +import dev.cyberarm.drivers.EncoderCustomKB2040; import dev.cyberarm.engine.V2.CyberarmEngine; import dev.cyberarm.engine.V2.Utilities; @@ -82,6 +83,8 @@ public class RedCrabMinibot { public final DcMotorEx frontLeft, frontRight, backLeft, backRight, winch; public final DcMotorEx clawArm; public final Servo leftClaw, rightClaw, clawWrist, droneLatch, hookArm; + public final DcMotorEx deadWheelXLeft, deadWheelXRight; + public final EncoderCustomKB2040 deadWheelYCenter; final CyberarmEngine engine; @@ -110,6 +113,8 @@ public class RedCrabMinibot { /// Doohickey public VisionPortal visionPortal = null; + public static Localizer localizer; + public RedCrabMinibot(boolean autonomous) { engine = CyberarmEngine.instance; @@ -228,8 +233,25 @@ public class RedCrabMinibot { hookArm.setDirection(Servo.Direction.FORWARD); // hookArm.setPosition(HOOK_ARM_STOW_POSITION); // LEAVE OFF: + /// DEAD WHEELS /// + deadWheelXLeft = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_left"); + deadWheelXRight = (DcMotorEx) engine.hardwareMap.dcMotor.get("deadwheel_x_right"); + deadWheelYCenter = engine.hardwareMap.get(EncoderCustomKB2040.class, "deadwheel_y_center"); + + deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE); + deadWheelXRight.setDirection(DcMotorSimple.Direction.FORWARD); +// deadWheelYCenter.setDirection(DcMotorSimple.Direction.FORWARD); + // Bulk read from hubs Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL); + + // Initialize localizer + if (autonomous || RedCrabMinibot.localizer == null) { + RedCrabMinibot.localizer = new Localizer(this); + } + + if (autonomous) + RedCrabMinibot.localizer.reset(); } public void reloadConfig() { @@ -290,6 +312,21 @@ public class RedCrabMinibot { public void standardTelemetry() { engine.telemetry.addLine(); + if (RedCrabMinibot.localizer != null) { + engine.telemetry.addLine("Localizer"); + engine.telemetry.addData("X (MM)", "%.2fmm", RedCrabMinibot.localizer.xMM()); + engine.telemetry.addData("Y (MM)", "%.2fmm", RedCrabMinibot.localizer.yMM()); + engine.telemetry.addData("R (De)", "%.2fdeg", RedCrabMinibot.localizer.headingDegrees()); + engine.telemetry.addLine(); + } + + engine.telemetry.addLine("Deadwheels"); + engine.telemetry.addData("X Left", deadWheelXLeft.getCurrentPosition()); + engine.telemetry.addData("X Right", deadWheelXRight.getCurrentPosition()); + // Use .getLastPosition instead of .getCurrentPosition here to not make an additional round trip just for telemetry + engine.telemetry.addData("Y Center", deadWheelYCenter.getLastPosition()); + engine.telemetry.addLine(); + engine.telemetry.addLine("Motors"); engine.telemetry.addData( "Front Left", diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java index 142e653..22abc6f 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabAutonomousEngine.java @@ -10,6 +10,9 @@ public abstract class RedCrabAutonomousEngine extends CyberarmEngine { @Override public void loop() { Utilities.hubsClearBulkReadCache(hardwareMap); + if (RedCrabMinibot.localizer != null) { + RedCrabMinibot.localizer.integrate(); + } super.loop(); diff --git a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java index 1162cb9..ef0d484 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/engines/RedCrabTeleOpEngine.java @@ -22,6 +22,9 @@ public class RedCrabTeleOpEngine extends CyberarmEngine { @Override public void loop() { Utilities.hubsClearBulkReadCache(hardwareMap); + if (RedCrabMinibot.localizer != null) { + RedCrabMinibot.localizer.integrate(); + } super.loop(); }