From 585ded2381d9a955303d845844f4b8392de30f54 Mon Sep 17 00:00:00 2001 From: Cyberarm Date: Mon, 29 Jan 2024 21:06:42 -0600 Subject: [PATCH] RedCrab: Localizer _seems_ to be working now, more testing needed. --- .../cyberarm/minibots/red_crab/Localizer.java | 139 ++++++++++++------ .../minibots/red_crab/RedCrabMinibot.java | 29 ++-- 2 files changed, 114 insertions(+), 54 deletions(-) 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 index 589e6e0..8fa532e 100644 --- a/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java +++ b/TeamCode/src/main/java/dev/cyberarm/minibots/red_crab/Localizer.java @@ -1,6 +1,8 @@ package dev.cyberarm.minibots.red_crab; +import com.arcrobotics.ftclib.geometry.Pose2d; import com.arcrobotics.ftclib.kinematics.HolonomicOdometry; +import com.arcrobotics.ftclib.kinematics.Odometry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; @@ -10,79 +12,132 @@ 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; - + private double rawX = 0, rawY = 0, rawR = 0, offsetX = 0, offsetY = 0; + private final double trackWidthMM = 365.0, forwardOffsetMM = 140.0, wheelDiameterMM = 90.0; + private final int encoderTicksPerRevolution = 8192; + private final double encoderGearRatio = 1; + private double lastEncoderXLeftMM, lastEncoderXRightMM, lastEncoderYCenterMM; +// private double xDeltaMultiplier = 0.87012987, yDeltaMultiplier = 0.25; + private double xDeltaMultiplier = 1, yDeltaMultiplier = 1; + private HolonomicOdometry odometry; 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(); + this.lastEncoderXLeftMM = ticksToMM(robot.deadWheelXLeft.getCurrentPosition()); + this.lastEncoderXRightMM = ticksToMM(robot.deadWheelXRight.getCurrentPosition()); + this.lastEncoderYCenterMM = ticksToMM(robot.deadWheelYCenter.getCurrentPosition()); + + this.odometry = new HolonomicOdometry( + this::leftDistance, + this::rightDistance, + this::centerDistance, + trackWidthMM, + forwardOffsetMM + ); } public void reset() { + robot.resetDeadWheels(); + + odometry = new HolonomicOdometry( + this::leftDistance, + this::rightDistance, + this::centerDistance, + trackWidthMM, + forwardOffsetMM + ); + rawX = 0; rawY = 0; rawR = 0; + + offsetX = 0; + offsetY = 0; } - // FIXME + // Meant for setting starting location offset public void teleport(double xMM, double yMM) { - this.rawX = xMM; - this.rawY = yMM; + this.offsetX = xMM; + this.offsetY = yMM; } - // FIXME + // FIXME: We need to be able to set rotation to +/- 180 so we can use absolute field coordinates for target location(s) + // and use odometry position as "true" field location. public void teleport(double xMM, double yMM, double headingDegrees) { - this.rawX = xMM; - this.rawY = yMM; - this.rawR = (AngleUnit.DEGREES).toRadians(AngleUnit.normalizeDegrees(headingDegrees)); // cursed :( + this.offsetX = xMM; + this.offsetY = yMM; + this.rawR = 0; // FIXME HERE // (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(); + odometry.updatePose(); - 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 heading = rawR + 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; +// double leftEncoder = ticksToMM(robot.deadWheelXLeft.getCurrentPosition()); +// double rightEncoder = ticksToMM(robot.deadWheelXRight.getCurrentPosition()); +// double centerEncoder = ticksToMM(robot.deadWheelYCenter.getCurrentPosition()); +// +// double deltaLeft = leftEncoder - lastEncoderXLeftMM; +// double deltaRight = rightEncoder - lastEncoderXRightMM; +// double deltaCenter = centerEncoder - lastEncoderYCenterMM; +// +// double phi = (deltaLeft - deltaRight) / trackWidthMM; +// double deltaMiddle = (deltaLeft + deltaRight) / 2.0; +// // double deltaPerp = deltaCenter - forwardOffsetMM * phi; +// double deltaPerp = deltaCenter - (deltaRight - deltaLeft) * forwardOffsetMM / trackWidthMM; +// +// double heading = rawR + (phi / 2.0); +// double deltaX = (deltaMiddle * Math.cos(heading) - deltaPerp * Math.sin(heading)) * xDeltaMultiplier; +// double deltaY = (deltaMiddle * Math.sin(heading) + deltaPerp * Math.cos(heading)) * yDeltaMultiplier; +// +// rawX += deltaX; +// rawY += deltaY; +// rawR += phi; +// +// lastEncoderXLeftMM = leftEncoder; +// lastEncoderXRightMM = rightEncoder; +// lastEncoderYCenterMM = centerEncoder; } public double xMM() { - return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawX); + return odometry.getPose().getX() + offsetX; //rawX; } public double yMM() { - return Utilities.ticksToUnit(8192, 1, wheelDiameterMM, DistanceUnit.MM, (int)rawY); + return odometry.getPose().getY() + offsetY; //rawY; } - // FIXME + public double xIn() { + return xMM() * 0.03937008; + } + public double yIn() { + return yMM() * 0.03937008; + } + + // Returns true 360 degrees public double headingDegrees() { - return rawR; + double degrees = headingRadians() * 180.0 / Math.PI; + return ((degrees + 360.0) % 360.0) % 360.0; } - // FIXME? (report radians as halves or proper whole?) + // Returns annoying half-split +/- PI radians public double headingRadians() { - return rawR; + return odometry.getPose().getHeading(); // rawR; + } + + private double ticksToMM(int ticks) { + return Utilities.ticksToUnit(encoderTicksPerRevolution, encoderGearRatio, wheelDiameterMM, DistanceUnit.MM, ticks); + } + + public double leftDistance() { + return ticksToMM(robot.deadWheelXLeft.getCurrentPosition()); + } + + public double rightDistance() { + return ticksToMM(robot.deadWheelXRight.getCurrentPosition()); + } + + public double centerDistance() { + return ticksToMM(robot.deadWheelYCenter.getCurrentPosition()); } } 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 18fef77..b21e8fb 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 @@ -242,16 +242,8 @@ public class RedCrabMinibot { 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); - - deadWheelXLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - deadWheelXRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); - deadWheelYCenter.reset(); - - deadWheelXLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - deadWheelXRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + if (autonomous) + resetDeadWheels(); // Bulk read from hubs Utilities.hubsBulkReadMode(engine.hardwareMap, LynxModule.BulkCachingMode.MANUAL); @@ -320,6 +312,19 @@ public class RedCrabMinibot { RedCrabMinibot.DRONE_LAUNCH_CONFIRMATION_TIME_MS = config.variable("Robot", "DroneLauncher_Tuning", "launch_confirmation_time_ms").value(); } + public void resetDeadWheels() { + deadWheelXLeft.setDirection(DcMotorSimple.Direction.REVERSE); + deadWheelXRight.setDirection(DcMotorSimple.Direction.FORWARD); +// deadWheelYCenter.setDirection(DcMotorSimple.Direction.FORWARD); + + deadWheelXLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + deadWheelXRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + deadWheelYCenter.reset(); + + deadWheelXLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + deadWheelXRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + } + public void standardTelemetry() { engine.telemetry.addLine(); @@ -327,8 +332,8 @@ public class RedCrabMinibot { 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("X (MM)", "%.2fmm (%.2f IN)", RedCrabMinibot.localizer.xMM(), RedCrabMinibot.localizer.xIn()); + engine.telemetry.addData("Y (MM)", "%.2fmm (%.2f IN)", RedCrabMinibot.localizer.yMM(), RedCrabMinibot.localizer.yIn()); engine.telemetry.addData("R (De)", "%.2fdeg", RedCrabMinibot.localizer.headingDegrees()); engine.telemetry.addLine(); }