From 33c49e23a7ec2174880ec833b93b9a0f13dcda85 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 10 Jun 2024 21:41:09 -0400 Subject: [PATCH 1/3] Add demo autos --- .../org/littletonrobotics/frc2024/Robot.java | 8 + .../frc2024/RobotContainer.java | 12 +- .../littletonrobotics/frc2024/RobotState.java | 66 ++++++++ .../frc2024/commands/DemoAutos.java | 69 ++++++++ .../apriltagvision/AprilTagVision.java | 149 ++++++++++++++---- .../frc2024/subsystems/drive/Drive.java | 2 - .../superstructure/Superstructure.java | 6 + .../subsystems/superstructure/arm/Arm.java | 6 + 8 files changed, 278 insertions(+), 40 deletions(-) create mode 100644 src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 15811f93..5cc46794 100644 --- a/src/main/java/org/littletonrobotics/frc2024/Robot.java +++ b/src/main/java/org/littletonrobotics/frc2024/Robot.java @@ -227,6 +227,14 @@ public void robotPeriodic() { Logger.recordOutput( "RobotState/SuperPoopParameters/FlywheelsRightSpeed", superPoopParameters.flywheelSpeeds().rightSpeed()); + var demoParameters = RobotState.getInstance().getDemoTagParameters(); + demoParameters.ifPresent( + parameters -> { + Logger.recordOutput("RobotState/DemoParameters/TargetPose", parameters.targetPose()); + Logger.recordOutput( + "RobotState/DemoParameters/TargetHeading", parameters.targetHeading()); + Logger.recordOutput("RobotState/DemoParameters/ArmAngle", parameters.armAngle()); + }); // Robot container periodic methods robotContainer.checkControllers(); diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 78f21948..634673dc 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -31,10 +31,7 @@ import org.littletonrobotics.frc2024.AutoSelector.AutoQuestion; import org.littletonrobotics.frc2024.AutoSelector.AutoQuestionResponse; import org.littletonrobotics.frc2024.FieldConstants.AprilTagLayoutType; -import org.littletonrobotics.frc2024.commands.ClimbingCommands; -import org.littletonrobotics.frc2024.commands.FeedForwardCharacterization; -import org.littletonrobotics.frc2024.commands.StaticCharacterization; -import org.littletonrobotics.frc2024.commands.WheelRadiusCharacterization; +import org.littletonrobotics.frc2024.commands.*; import org.littletonrobotics.frc2024.commands.auto.AutoBuilder; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVision; import org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO; @@ -402,6 +399,13 @@ private void configureAutos() { AutoQuestionResponse.SIX_SECONDS, AutoQuestionResponse.LAST_SECOND))), autoBuilder.davisInspirationalAuto()); + DemoAutos demoAutos = new DemoAutos(drive, superstructure, autoSelector::getResponses); + autoSelector.addRoutine( + "Davis Demo Auto", + List.of( + new AutoQuestion( + "Follow Tag?", List.of(AutoQuestionResponse.YES, AutoQuestionResponse.NO))), + demoAutos.davisDemoAuto()); // Set up feedforward characterization autoSelector.addRoutine( diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index 2c28467f..d9a0350f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -18,11 +18,13 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import java.util.NoSuchElementException; +import java.util.Optional; import java.util.function.BooleanSupplier; import lombok.Getter; import lombok.Setter; import lombok.experimental.ExtensionMethod; import org.littletonrobotics.frc2024.subsystems.drive.DriveConstants; +import org.littletonrobotics.frc2024.subsystems.superstructure.arm.ArmConstants; import org.littletonrobotics.frc2024.util.AllianceFlipUtil; import org.littletonrobotics.frc2024.util.GeomUtil; import org.littletonrobotics.frc2024.util.LoggedTunableNumber; @@ -52,6 +54,9 @@ public record AimingParameters( double effectiveDistance, FlywheelSpeeds flywheelSpeeds) {} + public record DemoAimingParameters( + Pose2d targetPose, Rotation2d targetHeading, Rotation2d armAngle) {} + private static final LoggedTunableNumber autoLookahead = new LoggedTunableNumber("RobotState/AutoLookahead", 0.5); private static final LoggedTunableNumber lookahead = @@ -60,6 +65,7 @@ public record AimingParameters( new LoggedTunableNumber("RobotState/SuperPoopLookahead", 0.1); private static final LoggedTunableNumber closeShootingZoneFeet = new LoggedTunableNumber("RobotState/CloseShootingZoneFeet", 10.0); + private static final double poseBufferSizeSeconds = 2.0; private static final double armAngleCoefficient = 57.254371165197; @@ -128,6 +134,9 @@ public static RobotState getInstance() { private AimingParameters latestParameters = null; private AimingParameters latestSuperPoopParameters = null; + // Demo parameters + private Pose3d demoTagPose = null; + private DemoAimingParameters latestDemoParamters = null; @Setter private BooleanSupplier lookaheadDisable = () -> false; @@ -320,6 +329,63 @@ public AimingParameters getSuperPoopAimingParameters() { return latestSuperPoopParameters; } + public void setDemoTagPose(Pose3d demoTagPose) { + this.demoTagPose = demoTagPose; + latestDemoParamters = null; + } + + private static final LoggedTunableNumber demoTargetDistance = + new LoggedTunableNumber("RobotState/DemoTargetDistance", 2.0); + private static final Translation3d upDirection = new Translation3d(0.0, 0.0, 1.0); + private static final Translation3d leftDirection = new Translation3d(0.0, 1.0, 0.0); + private static final Translation3d downDirection = new Translation3d(0.0, 0.0, -1.0); + private static final Translation3d rightDirection = new Translation3d(0.0, -1.0, 0.0); + + public Optional getDemoTagParameters() { + if (latestDemoParamters != null) { + // Use cached demo parameters. + return Optional.of(latestDemoParamters); + } + // Return empty optional if no demo tag pose. + if (demoTagPose == null) return Optional.empty(); + + // Calculate target pose. + // Determine tag rotation + double maxZ = 0.0; + int maxIndex = 0; + int index = 0; + for (var tagDirection : + new Translation3d[] {upDirection, leftDirection, downDirection, rightDirection}) { + double z = demoTagPose.transformBy(new Transform3d(tagDirection, new Rotation3d())).getZ(); + if (z > maxZ) { + maxZ = z; + maxIndex = index; + } + index++; + } + Rotation2d robotRotation = new Rotation2d(Math.PI + Math.PI / 2.0 * maxIndex); + Pose2d targetPose = + demoTagPose + .toPose2d() + .transformBy( + new Transform2d(new Translation2d(demoTargetDistance.get(), 0.0), robotRotation)); + + // Calculate heading without movement. + Translation2d demoTagFixed = demoTagPose.getTranslation().toTranslation2d(); + Translation2d robotToDemoTagFixed = demoTagFixed.minus(getEstimatedPose().getTranslation()); + Rotation2d targetHeading = robotToDemoTagFixed.getAngle(); + + // Calculate arm angle. + double z = demoTagPose.getZ(); + Rotation2d armAngle = + new Rotation2d( + robotToDemoTagFixed.getNorm() - ArmConstants.armOrigin.getX(), + z - ArmConstants.armOrigin.getY()); + + latestDemoParamters = new DemoAimingParameters(targetPose, targetHeading, armAngle); + return Optional.of(latestDemoParamters); + } + public ModuleLimits getModuleLimits() { return flywheelAccelerating && !DriverStation.isAutonomousEnabled() ? DriveConstants.moduleLimitsFlywheelSpinup diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java b/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java new file mode 100644 index 00000000..29606e3f --- /dev/null +++ b/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java @@ -0,0 +1,69 @@ +// Copyright (c) 2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package org.littletonrobotics.frc2024.commands; + +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import java.util.List; +import java.util.function.Supplier; +import lombok.RequiredArgsConstructor; +import org.littletonrobotics.frc2024.AutoSelector; +import org.littletonrobotics.frc2024.RobotState; +import org.littletonrobotics.frc2024.subsystems.drive.Drive; +import org.littletonrobotics.frc2024.subsystems.superstructure.Superstructure; +import org.littletonrobotics.frc2024.subsystems.superstructure.arm.Arm; + +@RequiredArgsConstructor +public class DemoAutos { + private final Drive drive; + private final Superstructure superstructure; + private final Supplier> responses; + + public Command davisDemoAuto() { + return Commands.either( + followTag(), + lookAtTag(), + () -> responses.get().get(0).equals(AutoSelector.AutoQuestionResponse.YES)); + } + + private Command followTag() { + return drive + .startEnd( + () -> + drive.setAutoAlignGoal( + () -> + RobotState.getInstance() + .getDemoTagParameters() + .map(RobotState.DemoAimingParameters::targetPose) + .orElseGet(() -> RobotState.getInstance().getEstimatedPose()), + Translation2d::new, + false), + drive::clearAutoAlignGoal) + .alongWith( + superstructure.setGoalWithConstraintsCommand( + Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get())); + } + + private Command lookAtTag() { + return drive + .startEnd( + () -> + drive.setHeadingGoal( + () -> + RobotState.getInstance() + .getDemoTagParameters() + .map(RobotState.DemoAimingParameters::targetHeading) + .orElseGet( + () -> RobotState.getInstance().getEstimatedPose().getRotation())), + drive::clearAutoAlignGoal) + .alongWith( + superstructure.setGoalWithConstraintsCommand( + Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get())); + } +} diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java index ea5c75f0..ce8a0e22 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java @@ -12,11 +12,7 @@ import static org.littletonrobotics.frc2024.subsystems.apriltagvision.AprilTagVisionIO.AprilTagVisionIOInputs; import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Quaternion; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.*; import edu.wpi.first.wpilibj.Timer; import java.util.*; import java.util.function.Supplier; @@ -34,6 +30,7 @@ public class AprilTagVision extends VirtualSubsystem { private static final LoggedTunableNumber timestampOffset = new LoggedTunableNumber("AprilTagVision/TimestampOffset", -(1.0 / 50.0)); + private static final double demoTagPosePersistenceSecs = 0.5; private final Supplier aprilTagTypeSupplier; private final AprilTagVisionIO[] io; @@ -42,6 +39,9 @@ public class AprilTagVision extends VirtualSubsystem { private final Map lastFrameTimes = new HashMap<>(); private final Map lastTagDetectionTimes = new HashMap<>(); + private Pose3d demoTagPose = null; + private double lastDemoTagPoseTimestamp = 0.0; + public AprilTagVision(Supplier aprilTagTypeSupplier, AprilTagVisionIO... io) { this.aprilTagTypeSupplier = aprilTagTypeSupplier; this.io = io; @@ -161,7 +161,7 @@ public void periodic() { aprilTagTypeSupplier.get().getLayout().getTagPose((int) values[i]); tagPose.ifPresent(tagPoses::add); } - if (tagPoses.size() == 0) continue; + if (tagPoses.isEmpty()) continue; // Calculate average distance to tag double totalDistance = 0.0; @@ -199,39 +199,120 @@ public void periodic() { "AprilTagVision/Inst" + instanceIndex + "/TagPoses", tagPoses.toArray(Pose3d[]::new)); } - // If no frames from instances, clear robot pose - if (inputs[instanceIndex].timestamps.length == 0) { - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new Pose2d()); - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new Pose3d()); + // Record demo tag pose + if (inputs[instanceIndex].demoFrame.length > 0) { + var values = inputs[instanceIndex].demoFrame; + double error0 = values[0]; + double error1 = values[8]; + Pose3d fieldToCameraPose = + new Pose3d(RobotState.getInstance().getEstimatedPose()) + .transformBy(cameraPoses[instanceIndex].toTransform3d()); + Pose3d fieldToTagPose0 = + fieldToCameraPose.transformBy( + new Transform3d( + new Translation3d(values[1], values[2], values[3]), + new Rotation3d(new Quaternion(values[4], values[5], values[6], values[7])))); + Pose3d fieldToTagPose1 = + fieldToCameraPose.transformBy( + new Transform3d( + new Translation3d(values[9], values[10], values[11]), + new Rotation3d( + new Quaternion(values[12], values[13], values[14], values[15])))); + Pose3d fieldToTagPose; + + // Find best pose + if (demoTagPose == null && error0 < error1) { + fieldToTagPose = fieldToTagPose0; + } else if (demoTagPose == null && error0 >= error1) { + fieldToTagPose = fieldToTagPose1; + } else if (error0 < error1 * ambiguityThreshold) { + fieldToTagPose = fieldToTagPose0; + } else if (error1 < error0 * ambiguityThreshold) { + fieldToTagPose = fieldToTagPose1; + } else { + var pose0Quaternion = fieldToTagPose0.getRotation().getQuaternion(); + var pose1Quaternion = fieldToTagPose1.getRotation().getQuaternion(); + var referenceQuaternion = demoTagPose.getRotation().getQuaternion(); + double pose0Distance = + Math.acos( + pose0Quaternion.getW() * referenceQuaternion.getW() + + pose0Quaternion.getX() * referenceQuaternion.getX() + + pose0Quaternion.getY() * referenceQuaternion.getY() + + pose0Quaternion.getZ() * referenceQuaternion.getZ()); + double pose1Distance = + Math.acos( + pose1Quaternion.getW() * referenceQuaternion.getW() + + pose1Quaternion.getX() * referenceQuaternion.getX() + + pose1Quaternion.getY() * referenceQuaternion.getY() + + pose1Quaternion.getZ() * referenceQuaternion.getZ()); + if (pose0Distance > Math.PI / 2) { + pose0Distance = Math.PI - pose0Distance; + } + if (pose1Distance > Math.PI / 2) { + pose1Distance = Math.PI - pose1Distance; + } + if (pose0Distance < pose1Distance) { + fieldToTagPose = fieldToTagPose0; + } else { + fieldToTagPose = fieldToTagPose1; + } + } + + // Save pose + if (fieldToTagPose != null) { + demoTagPose = fieldToTagPose; + lastDemoTagPoseTimestamp = Timer.getFPGATimestamp(); + } + + // If no frames from instances, clear robot pose + if (inputs[instanceIndex].timestamps.length == 0) { + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose", new Pose2d()); + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/RobotPose3d", new Pose3d()); + } + + // If no recent frames from instance, clear tag poses + if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) { + //noinspection RedundantArrayCreation + Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new Pose3d[] {}); + } + } + + // Clear demo tag pose + if (Timer.getFPGATimestamp() - lastDemoTagPoseTimestamp > demoTagPosePersistenceSecs) { + demoTagPose = null; } - // If no recent frames from instance, clear tag poses - if (Timer.getFPGATimestamp() - lastFrameTimes.get(instanceIndex) > targetLogTimeSecs) { - //noinspection RedundantArrayCreation - Logger.recordOutput("AprilTagVision/Inst" + instanceIndex + "/TagPoses", new Pose3d[] {}); + // Log robot poses + Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new)); + Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new)); + + // Log tag poses + List allTagPoses = new ArrayList<>(); + for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { + if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { + aprilTagTypeSupplier + .get() + .getLayout() + .getTagPose(detectionEntry.getKey()) + .ifPresent(allTagPoses::add); + } } - } + Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); - // Log robot poses - Logger.recordOutput("AprilTagVision/RobotPoses", allRobotPoses.toArray(Pose2d[]::new)); - Logger.recordOutput("AprilTagVision/RobotPoses3d", allRobotPoses3d.toArray(Pose3d[]::new)); - - // Log tag poses - List allTagPoses = new ArrayList<>(); - for (Map.Entry detectionEntry : lastTagDetectionTimes.entrySet()) { - if (Timer.getFPGATimestamp() - detectionEntry.getValue() < targetLogTimeSecs) { - aprilTagTypeSupplier - .get() - .getLayout() - .getTagPose(detectionEntry.getKey()) - .ifPresent(allTagPoses::add); + // Log demo tag pose + if (demoTagPose == null) { + Logger.recordOutput("AprilTagVision/DemoTagPose", new Pose3d[] {}); + } else { + Logger.recordOutput("AprilTagVision/DemoTagPose", demoTagPose); } - } - Logger.recordOutput("AprilTagVision/TagPoses", allTagPoses.toArray(Pose3d[]::new)); + Logger.recordOutput("AprilTagVision/DemoTagPoseId", new long[] {29}); - // Send results to robot state - allVisionObservations.stream() - .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) - .forEach(RobotState.getInstance()::addVisionObservation); + // Send results to robot state + allVisionObservations.stream() + .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) + .forEach(RobotState.getInstance()::addVisionObservation); + } + RobotState.getInstance() + .setDemoTagPose(demoTagPose); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index b9cc3f5f..26ef04de 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -384,10 +384,8 @@ public void setAutoAlignGoal( Supplier poseSupplier, Supplier feedforwardSupplier, boolean slowMode) { - if (DriverStation.isTeleopEnabled()) { currentDriveMode = DriveMode.AUTO_ALIGN; autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode); - } } /** Clears the current auto align goal. */ diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java index 26429ca9..a015cfde 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/Superstructure.java @@ -31,6 +31,7 @@ public enum Goal { BACKPACK_OUT_UNJAM, AIM, SUPER_POOP, + AIM_AT_DEMO_TAG, UNJAM_FEEDER, STATION_INTAKE, AMP, @@ -116,6 +117,11 @@ public void periodic() { climber.setGoal(Climber.Goal.IDLE); backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); } + case AIM_AT_DEMO_TAG -> { + arm.setGoal(Arm.Goal.AIM_AT_DEMO_TAG); + climber.setGoal(Climber.Goal.IDLE); + backpackActuator.setGoal(BackpackActuator.Goal.RETRACT); + } case STATION_INTAKE -> { arm.setGoal(Arm.Goal.STATION_INTAKE); climber.setGoal(Climber.Goal.IDLE); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java index fd0be936..458e0fe1 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/superstructure/arm/Arm.java @@ -81,6 +81,12 @@ public enum Goal { AIM(() -> RobotState.getInstance().getAimingParameters().armAngle().getDegrees()), SUPER_POOP( () -> RobotState.getInstance().getSuperPoopAimingParameters().armAngle().getDegrees()), + AIM_AT_DEMO_TAG( + () -> + RobotState.getInstance() + .getDemoTagParameters() + .map(parameters -> parameters.armAngle().getDegrees()) + .orElse(minAngle.getDegrees())), AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)), SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)), PODIUM(new LoggedTunableNumber("Arm/PodiumDegrees", 34.0)), From 5f7b240d6310d1f46993ce04351a914c132f4469 Mon Sep 17 00:00:00 2001 From: suryatho Date: Mon, 10 Jun 2024 21:46:04 -0400 Subject: [PATCH 2/3] formatting :( --- .../frc2024/subsystems/apriltagvision/AprilTagVision.java | 3 +-- .../org/littletonrobotics/frc2024/subsystems/drive/Drive.java | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java index ce8a0e22..9d3a928f 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/apriltagvision/AprilTagVision.java @@ -312,7 +312,6 @@ public void periodic() { .sorted(Comparator.comparingDouble(VisionObservation::timestamp)) .forEach(RobotState.getInstance()::addVisionObservation); } - RobotState.getInstance() - .setDemoTagPose(demoTagPose); + RobotState.getInstance().setDemoTagPose(demoTagPose); } } diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index 26ef04de..6f50109b 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -384,8 +384,8 @@ public void setAutoAlignGoal( Supplier poseSupplier, Supplier feedforwardSupplier, boolean slowMode) { - currentDriveMode = DriveMode.AUTO_ALIGN; - autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode); + currentDriveMode = DriveMode.AUTO_ALIGN; + autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode); } /** Clears the current auto align goal. */ From b28c0d3e65ac4f53430927c9c1aad2cb919c2898 Mon Sep 17 00:00:00 2001 From: Jonah <47046556+jwbonner@users.noreply.github.com> Date: Wed, 12 Jun 2024 21:20:34 -0400 Subject: [PATCH 3/3] Fix --- .../littletonrobotics/frc2024/RobotState.java | 21 ++----------------- .../frc2024/commands/DemoAutos.java | 2 +- .../frc2024/subsystems/drive/Drive.java | 6 ++++-- 3 files changed, 7 insertions(+), 22 deletions(-) diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotState.java b/src/main/java/org/littletonrobotics/frc2024/RobotState.java index d9a0350f..dfcb8544 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotState.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotState.java @@ -336,10 +336,6 @@ public void setDemoTagPose(Pose3d demoTagPose) { private static final LoggedTunableNumber demoTargetDistance = new LoggedTunableNumber("RobotState/DemoTargetDistance", 2.0); - private static final Translation3d upDirection = new Translation3d(0.0, 0.0, 1.0); - private static final Translation3d leftDirection = new Translation3d(0.0, 1.0, 0.0); - private static final Translation3d downDirection = new Translation3d(0.0, 0.0, -1.0); - private static final Translation3d rightDirection = new Translation3d(0.0, -1.0, 0.0); public Optional getDemoTagParameters() { if (latestDemoParamters != null) { @@ -350,25 +346,12 @@ public Optional getDemoTagParameters() { if (demoTagPose == null) return Optional.empty(); // Calculate target pose. - // Determine tag rotation - double maxZ = 0.0; - int maxIndex = 0; - int index = 0; - for (var tagDirection : - new Translation3d[] {upDirection, leftDirection, downDirection, rightDirection}) { - double z = demoTagPose.transformBy(new Transform3d(tagDirection, new Rotation3d())).getZ(); - if (z > maxZ) { - maxZ = z; - maxIndex = index; - } - index++; - } - Rotation2d robotRotation = new Rotation2d(Math.PI + Math.PI / 2.0 * maxIndex); Pose2d targetPose = demoTagPose .toPose2d() .transformBy( - new Transform2d(new Translation2d(demoTargetDistance.get(), 0.0), robotRotation)); + new Transform2d( + new Translation2d(demoTargetDistance.get(), 0.0), new Rotation2d(Math.PI))); // Calculate heading without movement. Translation2d demoTagFixed = demoTagPose.getTranslation().toTranslation2d(); diff --git a/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java b/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java index 29606e3f..c0b93a54 100644 --- a/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java +++ b/src/main/java/org/littletonrobotics/frc2024/commands/DemoAutos.java @@ -61,7 +61,7 @@ private Command lookAtTag() { .map(RobotState.DemoAimingParameters::targetHeading) .orElseGet( () -> RobotState.getInstance().getEstimatedPose().getRotation())), - drive::clearAutoAlignGoal) + drive::clearHeadingGoal) .alongWith( superstructure.setGoalWithConstraintsCommand( Superstructure.Goal.AIM_AT_DEMO_TAG, Arm.smoothProfileConstraints.get())); diff --git a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java index 6f50109b..3f89b446 100644 --- a/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java +++ b/src/main/java/org/littletonrobotics/frc2024/subsystems/drive/Drive.java @@ -384,8 +384,10 @@ public void setAutoAlignGoal( Supplier poseSupplier, Supplier feedforwardSupplier, boolean slowMode) { - currentDriveMode = DriveMode.AUTO_ALIGN; - autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode); + if (DriverStation.isEnabled()) { + currentDriveMode = DriveMode.AUTO_ALIGN; + autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode); + } } /** Clears the current auto align goal. */