diff --git a/src/main/java/org/littletonrobotics/frc2024/Robot.java b/src/main/java/org/littletonrobotics/frc2024/Robot.java index 87aad4fa..2adc89ed 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.updateDemoControls(); diff --git a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java index 4831b8e0..2fd6dfef 100644 --- a/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java +++ b/src/main/java/org/littletonrobotics/frc2024/RobotContainer.java @@ -33,10 +33,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; @@ -415,6 +412,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 b6dc746c..c4f00afa 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 DemoFollowParameters( + Pose2d targetPose, Rotation2d targetHeading, Rotation2d armAngle) {} + public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpeeds) {} private static final LoggedTunableNumber autoLookahead = @@ -62,6 +67,7 @@ public record DemoShotParameters(Rotation2d armAngle, FlywheelSpeeds flywheelSpe 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; @@ -130,6 +136,9 @@ public static RobotState getInstance() { private AimingParameters latestParameters = null; private AimingParameters latestSuperPoopParameters = null; + // Demo parameters + private Pose3d demoTagPose = null; + private DemoFollowParameters latestDemoParamters = null; @Setter @Getter private DemoShotParameters demoShotParameters = @@ -326,6 +335,46 @@ 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); + + 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. + Pose2d targetPose = + demoTagPose + .toPose2d() + .transformBy( + new Transform2d( + new Translation2d(demoTargetDistance.get(), 0.0), new Rotation2d(Math.PI))); + + // 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 DemoFollowParameters(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..38446faa --- /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.DemoFollowParameters::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.DemoFollowParameters::targetHeading) + .orElseGet( + () -> RobotState.getInstance().getEstimatedPose().getRotation())), + drive::clearHeadingGoal) + .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..9d3a928f 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,119 @@ 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..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,7 +384,7 @@ public void setAutoAlignGoal( Supplier poseSupplier, Supplier feedforwardSupplier, boolean slowMode) { - if (DriverStation.isTeleopEnabled()) { + if (DriverStation.isEnabled()) { currentDriveMode = DriveMode.AUTO_ALIGN; autoAlignController = new AutoAlignController(poseSupplier, feedforwardSupplier, slowMode); } 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 1ac593f1..ce19f020 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, DEMO_SHOT, UNJAM_FEEDER, STATION_INTAKE, @@ -117,6 +118,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 DEMO_SHOT -> { arm.setGoal(Arm.Goal.DEMO_SHOT); 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 039e5a9f..47e24f16 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())), DEMO_SHOT(() -> RobotState.getInstance().getDemoShotParameters().armAngle().getDegrees()), AMP(new LoggedTunableNumber("Arm/AmpDegrees", 110.0)), SUBWOOFER(new LoggedTunableNumber("Arm/SubwooferDegrees", 55.0)),