From 460551eed4f4c714d6cbd9d68a8c225937efd2d7 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Sun, 23 Feb 2025 11:14:51 -0600 Subject: [PATCH 01/16] Separate Coral Reef scoring poses by level --- src/main/java/frc/robot/AutoCommands.java | 79 +++++++++++++++++-- src/main/java/frc/robot/Constants.java | 51 +++++++++--- src/main/java/frc/robot/RobotContainer.java | 17 ---- ...efCommand.java => DriveToNearestPose.java} | 35 +++++--- .../robot/commands/DriveToPoseCommand.java | 4 +- 5 files changed, 140 insertions(+), 46 deletions(-) rename src/main/java/frc/robot/commands/{DriveToReefCommand.java => DriveToNearestPose.java} (52%) diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index b6b3f35..9a09838 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -2,17 +2,26 @@ import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.wpilibj.util.Color.kGreen; +import static frc.robot.Constants.AlignmentConstants.REEF_L2_SCORE_POSES_RED; +import static frc.robot.Constants.AlignmentConstants.REEF_L2_SCORE_POSE_BLUE; +import static frc.robot.Constants.AlignmentConstants.REEF_L3_SCORE_POSES_RED; +import static frc.robot.Constants.AlignmentConstants.REEF_L3_SCORE_POSE_BLUE; +import static frc.robot.Constants.AlignmentConstants.REEF_L4_SCORE_POSES_RED; +import static frc.robot.Constants.AlignmentConstants.REEF_L4_SCORE_POSE_BLUE; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.AlignToReefCommand; -import frc.robot.commands.DriveToReefCommand; +import frc.robot.commands.DriveToNearestPose; import frc.robot.subsystems.AlignmentSubsystem; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.GamePieceManipulatorSubsystem; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.LEDSubsystem; +import java.util.List; import org.photonvision.PhotonCamera; /** @@ -50,12 +59,53 @@ public AutoCommands( this.gamePieceManipulatorSubsystem = gamePieceManipulatorSubsystem; this.ledSubsystem = ledSubsystem; this.highCamera = highCamera; + + publishScoringPoses(); + } + + /** Sends the reef poses to the dashboard for debugging */ + private void publishScoringPoses() { + NetworkTableInstance.getDefault() + .getTable("reef_blue_L4") + .getStructArrayTopic("branches", Pose2d.struct) + .publish() + .set(REEF_L4_SCORE_POSE_BLUE.toArray(new Pose2d[REEF_L4_SCORE_POSE_BLUE.size()])); + + NetworkTableInstance.getDefault() + .getTable("reef_red_L4") + .getStructArrayTopic("branches", Pose2d.struct) + .publish() + .set(REEF_L4_SCORE_POSES_RED.toArray(new Pose2d[REEF_L4_SCORE_POSES_RED.size()])); + + NetworkTableInstance.getDefault() + .getTable("reef_blue_L3") + .getStructArrayTopic("branches", Pose2d.struct) + .publish() + .set(REEF_L3_SCORE_POSE_BLUE.toArray(new Pose2d[REEF_L3_SCORE_POSE_BLUE.size()])); + + NetworkTableInstance.getDefault() + .getTable("reef_red_L3") + .getStructArrayTopic("branches", Pose2d.struct) + .publish() + .set(REEF_L3_SCORE_POSES_RED.toArray(new Pose2d[REEF_L3_SCORE_POSES_RED.size()])); + + NetworkTableInstance.getDefault() + .getTable("reef_blue_L2") + .getStructArrayTopic("branches", Pose2d.struct) + .publish() + .set(REEF_L2_SCORE_POSE_BLUE.toArray(new Pose2d[REEF_L2_SCORE_POSE_BLUE.size()])); + + NetworkTableInstance.getDefault() + .getTable("reef_red_L2") + .getStructArrayTopic("branches", Pose2d.struct) + .publish() + .set(REEF_L2_SCORE_POSES_RED.toArray(new Pose2d[REEF_L2_SCORE_POSES_RED.size()])); } /** * Creates a command that will: *
    - *
  1. Drive to the nearest reef scoring location
    2. + *
  2. Drive to the nearest L4 reef scoring location
    3. *
  3. Align to the reef using the CANranges
    4. *
  4. Get the arm and elevator into position to score on level 4
    5. *
  5. Eject the coral
    6. @@ -64,13 +114,13 @@ public AutoCommands( * @return new command */ public Command autoScoreCoralLevel4() { - return autoScoreCoral(armSubsystem::moveToLevel4); + return autoScoreCoral(armSubsystem::moveToLevel4, REEF_L4_SCORE_POSES_RED, REEF_L4_SCORE_POSE_BLUE); } /** * Creates a command that will: *
      - *
    1. Drive to the nearest reef scoring location
      2. + *
    2. Drive to the nearest L3 reef scoring location
      3. *
    3. Align to the reef using the CANranges
      4. *
    4. Get the arm and elevator into position to score on level 3
      5. *
    5. Eject the coral
      6. @@ -79,12 +129,27 @@ public Command autoScoreCoralLevel4() { * @return new command */ public Command autoScoreCoralLevel3() { - return autoScoreCoral(armSubsystem::moveToLevel3); + return autoScoreCoral(armSubsystem::moveToLevel3, REEF_L3_SCORE_POSES_RED, REEF_L3_SCORE_POSE_BLUE); + } + + /** + * Creates a command that will: + *
        + *
      1. Drive to the nearest L2 reef scoring location
        2. + *
      2. Align to the reef using the CANranges
        3. + *
      3. Get the arm and elevator into position to score on level 3
        4. + *
      4. Eject the coral
        5. + *
      + * + * @return new command + */ + public Command autoScoreCoralLevel2() { + return autoScoreCoral(armSubsystem::moveToLevel2, REEF_L2_SCORE_POSES_RED, REEF_L2_SCORE_POSE_BLUE); } - private Command autoScoreCoral(Runnable armMethod) { + private Command autoScoreCoral(Runnable armMethod, List redPoses, List bluePoses) { var alignToReef = new AlignToReefCommand(drivetrain, alignmentSubsystem, Meters.of(0.34)); - var driveToReef = new DriveToReefCommand(drivetrain, () -> drivetrain.getState().Pose); + var driveToReef = new DriveToNearestPose(drivetrain, () -> drivetrain.getState().Pose, redPoses, bluePoses); return ledSubsystem .setLEDSegmentsAsCommand(kGreen, armSubsystem::isAtPosition, driveToReef::isFinished, alignToReef::isFinished) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b66b2b3..1378e23 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,6 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Transform3d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; @@ -318,16 +317,22 @@ public static class AlignmentConstants { public static final AngularAcceleration MAX_ALIGN_ANGULAR_ACCELERATION = RadiansPerSecondPerSecond .of(6.0 * Math.PI); - /** Pose of the robot relative to a reef branch for scoring */ - public static final Transform2d RELATIVE_SCORING_POSE = new Transform2d( + /** Pose of the robot relative to a reef branch for scoring coral on L4 */ + public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L4 = new Transform2d( + inchesToMeters(-40), + inchesToMeters(12), + Rotation2d.fromDegrees(-90)); + /** Pose of the robot relative to a reef branch for scoring coral on L3 */ + public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L3 = new Transform2d( inchesToMeters(-40), inchesToMeters(12), Rotation2d.fromDegrees(-90)); - /** Pose on the opposite side of the field. Use with `relativeTo` to flip a pose to the opposite alliance */ - public static final Pose2d FLIPPING_POSE = new Pose2d( - new Translation2d(FIELD_LENGTH_METERS, FIELD_WIDTH_METERS), - new Rotation2d(Math.PI)); + /** Pose of the robot relative to a reef branch for scoring coral on L2 */ + public static final Transform2d RELATIVE_SCORING_POSE_CORAL_L2 = new Transform2d( + inchesToMeters(-40), + inchesToMeters(12), + Rotation2d.fromDegrees(90)); // spotless:off /* The reef branches are in the arrays like this: @@ -360,7 +365,6 @@ public static class AlignmentConstants { new Pose2d(4.994328, 3.841097, Rotation2d.fromDegrees(180)), // 9 new Pose2d(4.873353, 3.632614, Rotation2d.fromDegrees(120)), // 10 new Pose2d(4.589334, 3.466500, Rotation2d.fromDegrees(120)))// 11 - .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE)) .collect(toUnmodifiableList()); /** @@ -381,7 +385,36 @@ public static class AlignmentConstants { new Pose2d(12.553672, 4.210903, Rotation2d.fromDegrees(0)), // 9 new Pose2d(12.598000, 4.292000, Rotation2d.fromDegrees(-60)), // 10 new Pose2d(12.958666, 4.585500, Rotation2d.fromDegrees(-60)))// 11 - .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE)) + .collect(toUnmodifiableList()); + + /** Poses of the robot for scoring on L4 on the red alliance */ + public static final List REEF_L4_SCORE_POSES_RED = REEF_BRANCH_POSES_RED.stream() + .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L4)) + .collect(toUnmodifiableList()); + + /** Poses of the robot for scoring on L4 on the blue alliance */ + public static final List REEF_L4_SCORE_POSE_BLUE = REEF_BRANCH_POSES_BLUE.stream() + .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L4)) + .collect(toUnmodifiableList()); + + /** Poses of the robot for scoring on L3 on the red alliance */ + public static final List REEF_L3_SCORE_POSES_RED = REEF_BRANCH_POSES_RED.stream() + .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L3)) + .collect(toUnmodifiableList()); + + /** Poses of the robot for scoring on L3 on the blue alliance */ + public static final List REEF_L3_SCORE_POSE_BLUE = REEF_BRANCH_POSES_BLUE.stream() + .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L3)) + .collect(toUnmodifiableList()); + + /** Poses of the robot for scoring on L2 on the red alliance */ + public static final List REEF_L2_SCORE_POSES_RED = REEF_BRANCH_POSES_RED.stream() + .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L2)) + .collect(toUnmodifiableList()); + + /** Poses of the robot for scoring on L2 on the blue alliance */ + public static final List REEF_L2_SCORE_POSE_BLUE = REEF_BRANCH_POSES_BLUE.stream() + .map(reefPose -> reefPose.plus(RELATIVE_SCORING_POSE_CORAL_L2)) .collect(toUnmodifiableList()); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6be8d9e..d046d4a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,8 +8,6 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kForward; import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kReverse; -import static frc.robot.Constants.AlignmentConstants.REEF_BRANCH_POSES_BLUE; -import static frc.robot.Constants.AlignmentConstants.REEF_BRANCH_POSES_RED; import static frc.robot.Constants.TeleopDriveConstants.MAX_TELEOP_ANGULAR_VELOCITY; import static frc.robot.Constants.TeleopDriveConstants.MAX_TELEOP_VELOCITY; @@ -19,8 +17,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -131,19 +127,6 @@ public RobotContainer() { .finallyDo(gamePieceManipulatorSubsystem::stop)); ledSubsystem.setDefaultCommand(new DefaultLEDCommand(ledSubsystem)); new LEDBootAnimationCommand(ledSubsystem).schedule(); - - // Send the reef poses to the dashboard for debugging - NetworkTableInstance.getDefault() - .getTable("reef_blue") - .getStructArrayTopic("branches", Pose2d.struct) - .publish() - .set(REEF_BRANCH_POSES_BLUE.toArray(new Pose2d[REEF_BRANCH_POSES_BLUE.size()])); - - NetworkTableInstance.getDefault() - .getTable("reef_red") - .getStructArrayTopic("branches", Pose2d.struct) - .publish() - .set(REEF_BRANCH_POSES_RED.toArray(new Pose2d[REEF_BRANCH_POSES_RED.size()])); } private void configureBindings() { diff --git a/src/main/java/frc/robot/commands/DriveToReefCommand.java b/src/main/java/frc/robot/commands/DriveToNearestPose.java similarity index 52% rename from src/main/java/frc/robot/commands/DriveToReefCommand.java rename to src/main/java/frc/robot/commands/DriveToNearestPose.java index 3d77cb0..1dab1a7 100644 --- a/src/main/java/frc/robot/commands/DriveToReefCommand.java +++ b/src/main/java/frc/robot/commands/DriveToNearestPose.java @@ -1,28 +1,35 @@ package frc.robot.commands; -import static frc.robot.Constants.AlignmentConstants.REEF_BRANCH_POSES_BLUE; -import static frc.robot.Constants.AlignmentConstants.REEF_BRANCH_POSES_RED; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.util.List; import java.util.function.Supplier; /** - * Command to drive to the scoring pose for the nearest reef branch. + * Command to drive to the pose for the nearest to the list of poses provided. */ -public class DriveToReefCommand extends DriveToPoseCommand { +public class DriveToNearestPose extends DriveToPoseCommand { + + private final List redPoses; + private final List bluePoses; /** - * Constructs a DriveToReefCommand + * Constructs a DriveToNearestPose * * @param drivetrainSubsystem drivetrain subsystem * @param poseProvider provider to call to get the robot pose + * @param redPses list of poses for when the robot is on the red alliance + * @param bluePoses list of poses for when the robot is on the blue alliance */ - public DriveToReefCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider) { - super(drivetrainSubsystem, poseProvider); + public DriveToNearestPose( + CommandSwerveDrivetrain drivetrainSubsystem, + Supplier poseProvider, + List redPoses, + List bluePoses) { + this(drivetrainSubsystem, poseProvider, DEFAULT_XY_CONSTRAINTS, DEFAULT_OMEGA_CONSTRAINTS, redPoses, bluePoses); } /** @@ -32,20 +39,26 @@ public DriveToReefCommand(CommandSwerveDrivetrain drivetrainSubsystem, Supplier< * @param poseProvider provider to call to get the robot pose * @param translationConstraints translation motion profile constraints * @param omegaConstraints rotation motion profile constraints + * @param redPses list of poses for when the robot is on the red alliance + * @param bluePoses list of poses for when the robot is on the blue alliance */ - public DriveToReefCommand( + public DriveToNearestPose( CommandSwerveDrivetrain drivetrainSubsystem, Supplier poseProvider, TrapezoidProfile.Constraints translationConstraints, - TrapezoidProfile.Constraints omegaConstraints) { + TrapezoidProfile.Constraints omegaConstraints, + List redPoses, + List bluePoses) { super(drivetrainSubsystem, poseProvider, translationConstraints, omegaConstraints); + this.redPoses = redPoses; + this.bluePoses = bluePoses; } @Override public void initialize() { var robotPose = poseProvider.get(); var isRed = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red; - setGoal(robotPose.nearest(isRed ? REEF_BRANCH_POSES_RED : REEF_BRANCH_POSES_BLUE)); + setGoal(robotPose.nearest(isRed ? redPoses : bluePoses)); super.initialize(); } diff --git a/src/main/java/frc/robot/commands/DriveToPoseCommand.java b/src/main/java/frc/robot/commands/DriveToPoseCommand.java index 822437c..727c80d 100644 --- a/src/main/java/frc/robot/commands/DriveToPoseCommand.java +++ b/src/main/java/frc/robot/commands/DriveToPoseCommand.java @@ -44,10 +44,10 @@ public class DriveToPoseCommand extends Command { private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); private static final Angle THETA_TOLERANCE = Degrees.of(1.0); - private static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = new TrapezoidProfile.Constraints( + protected static final TrapezoidProfile.Constraints DEFAULT_XY_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_DRIVE_TO_POSE_TRANSLATION_VELOCITY.in(MetersPerSecond), MAX_DRIVE_TO_POSE_TRANSLATION_ACCELERATION.in(MetersPerSecondPerSecond)); - private static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints( + protected static final TrapezoidProfile.Constraints DEFAULT_OMEGA_CONSTRAINTS = new TrapezoidProfile.Constraints( MAX_DRIVE_TO_POSE_ANGULAR_VELOCITY.in(RadiansPerSecond), MAX_DRIVE_TO_POSE_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); From 0db1cabac68c0403a2d5ad0282ae04ccea0de3c6 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Sun, 23 Feb 2025 11:59:51 -0600 Subject: [PATCH 02/16] We cannot auto align for L2, the robot's sensors are on the other side --- src/main/java/frc/robot/AutoCommands.java | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index 9a09838..00ea273 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -133,18 +133,16 @@ public Command autoScoreCoralLevel3() { } /** - * Creates a command that will: - *
        - *
      1. Drive to the nearest L2 reef scoring location
        2. - *
      2. Align to the reef using the CANranges
        3. - *
      3. Get the arm and elevator into position to score on level 3
        4. - *
      4. Eject the coral
        5. - *
      + * Creates a command that will drive to the nearest L2 reef scoring location * * @return new command */ - public Command autoScoreCoralLevel2() { - return autoScoreCoral(armSubsystem::moveToLevel2, REEF_L2_SCORE_POSES_RED, REEF_L2_SCORE_POSE_BLUE); + public Command driveToCoralLevel2() { + return new DriveToNearestPose( + drivetrain, + () -> drivetrain.getState().Pose, + REEF_L2_SCORE_POSES_RED, + REEF_L2_SCORE_POSE_BLUE); } private Command autoScoreCoral(Runnable armMethod, List redPoses, List bluePoses) { From 7fc9d053ddc13f80ecba4a9e763287d96ceddebc Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Mon, 24 Feb 2025 19:39:05 -0600 Subject: [PATCH 03/16] Aligning with tag, working pretty well --- src/main/java/frc/robot/AutoCommands.java | 2 +- .../robot/commands/AlignToReefCommand.java | 116 +++++++++++++++--- 2 files changed, 101 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index 00ea273..d7970ac 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -146,8 +146,8 @@ public Command driveToCoralLevel2() { } private Command autoScoreCoral(Runnable armMethod, List redPoses, List bluePoses) { - var alignToReef = new AlignToReefCommand(drivetrain, alignmentSubsystem, Meters.of(0.34)); var driveToReef = new DriveToNearestPose(drivetrain, () -> drivetrain.getState().Pose, redPoses, bluePoses); + var alignToReef = new AlignToReefCommand(drivetrain, alignmentSubsystem, Meters.of(0.34), highCamera); return ledSubsystem .setLEDSegmentsAsCommand(kGreen, armSubsystem::isAtPosition, driveToReef::isFinished, alignToReef::isFinished) diff --git a/src/main/java/frc/robot/commands/AlignToReefCommand.java b/src/main/java/frc/robot/commands/AlignToReefCommand.java index e8a1ddd..e99636d 100644 --- a/src/main/java/frc/robot/commands/AlignToReefCommand.java +++ b/src/main/java/frc/robot/commands/AlignToReefCommand.java @@ -1,8 +1,12 @@ package frc.robot.commands; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inch; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; +import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static frc.robot.Constants.AlignmentConstants.ALIGNMENT_TOLERANCE; @@ -10,24 +14,50 @@ import static frc.robot.Constants.AlignmentConstants.MAX_ALIGN_ANGULAR_VELOCITY; import static frc.robot.Constants.AlignmentConstants.MAX_ALIGN_TRANSLATION_ACCELERATION; import static frc.robot.Constants.AlignmentConstants.MAX_ALIGN_TRANSLATION_VELOCITY; +import static frc.robot.Constants.DriveToPoseConstants.THETA_kD; +import static frc.robot.Constants.DriveToPoseConstants.THETA_kI; +import static frc.robot.Constants.DriveToPoseConstants.THETA_kP; +import static frc.robot.Constants.DriveToPoseConstants.X_kD; +import static frc.robot.Constants.DriveToPoseConstants.X_kI; +import static frc.robot.Constants.DriveToPoseConstants.X_kP; +import static frc.robot.Constants.DriveToPoseConstants.Y_kD; +import static frc.robot.Constants.DriveToPoseConstants.Y_kI; +import static java.util.stream.Collectors.toUnmodifiableSet; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.AlignmentSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; +import java.util.Optional; +import java.util.Set; +import java.util.stream.Stream; +import org.photonvision.PhotonCamera; /** - * Command to align parallel to the reef at a specific distance + * Command to align parallel to the reef at a specific distance. This uses CANRanges and an AprilTag. */ public class AlignToReefCommand extends Command { + // ID of the tags on the reef + private static final Set FIDUCIAL_IDS = Stream.of(17, 18, 19, 20, 21, 22, 6, 7, 8, 9, 10, 11) + .collect(toUnmodifiableSet()); + // Alignment, depending which reef branch we want to score on + private static final double LEFT_TAG_ALIGNMENT = -0.23; + private static final double RIGHT_TAG_ALIGNMENT = 0.08; + + private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); + private static final Distance TAG_TOLERANCE = Inch.of(0.6); + private static final Angle THETA_TOLERANCE = Degrees.of(1.0); + private final CommandSwerveDrivetrain drivetrain; private final AlignmentSubsystem alignmentSubsystem; + private final PhotonCamera photonCamera; private final Distance targetDistance; private static final TrapezoidProfile.Constraints TRANSLATION_CONSTRAINTS = new TrapezoidProfile.Constraints( @@ -37,17 +67,31 @@ public class AlignToReefCommand extends Command { MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); - private final ProfiledPIDController rotationController = new ProfiledPIDController( - 5.0, - 0.0, - 0.0, + private final ProfiledPIDController robotYController = new ProfiledPIDController( + X_kP, + X_kI, + X_kD, + TRANSLATION_CONSTRAINTS); + + private final ProfiledPIDController robotXController = new ProfiledPIDController( + 6.0, + Y_kI, + Y_kD, TRANSLATION_CONSTRAINTS); - private final ProfiledPIDController distanceController = new ProfiledPIDController(5.0, 0.0, 0.0, OMEGA_CONSTRAINTS); + + private final ProfiledPIDController thetaController = new ProfiledPIDController( + THETA_kP, + THETA_kI, + THETA_kD, + OMEGA_CONSTRAINTS); private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() .withDriveRequestType(DriveRequestType.Velocity) .withSteerRequestType(SteerRequestType.MotionMagicExpo); + private double alignmentTarget; + private boolean sawTag = false; + /** * Constructs a new command * @@ -58,40 +102,63 @@ public class AlignToReefCommand extends Command { public AlignToReefCommand( CommandSwerveDrivetrain drivetrain, AlignmentSubsystem alignmentSubsystem, - Distance targetDistance) { + Distance targetDistance, + PhotonCamera photonCamera) { this.drivetrain = drivetrain; this.alignmentSubsystem = alignmentSubsystem; + this.photonCamera = photonCamera; this.targetDistance = targetDistance; + robotYController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); + robotXController.setTolerance(TAG_TOLERANCE.in(Meters)); + thetaController.enableContinuousInput(-Math.PI, Math.PI); + thetaController.setTolerance(THETA_TOLERANCE.in(Radians)); + addRequirements(drivetrain, alignmentSubsystem); } @Override public void initialize() { + robotYController.setGoal(targetDistance.in(Meters)); + thetaController.setGoal(0); + var leftDistance = alignmentSubsystem.getLeftDistance().in(Meters); var rightDistance = alignmentSubsystem.getRightDistance().in(Meters); var rotation = rightDistance - leftDistance; var averageDistance = (leftDistance + rightDistance) / 2; - rotationController.reset(rotation); - distanceController.reset(averageDistance); - - distanceController.setGoal(targetDistance.in(Meters)); - rotationController.setGoal(0); + thetaController.reset(rotation); + robotYController.reset(averageDistance); + + getTagY().ifPresentOrElse(tagY -> { + alignmentTarget = Math.abs(tagY - RIGHT_TAG_ALIGNMENT) < Math.abs(tagY - LEFT_TAG_ALIGNMENT) ? RIGHT_TAG_ALIGNMENT + : LEFT_TAG_ALIGNMENT; + robotXController.setGoal(alignmentTarget); + robotXController.reset(tagY); + sawTag = true; + }, () -> { + sawTag = false; + robotXController.reset(0); + }); } @Override public void execute() { var leftDistance = alignmentSubsystem.getLeftDistance().in(Meters); var rightDistance = alignmentSubsystem.getRightDistance().in(Meters); + var tagY = getTagY(); if (leftDistance < 1.2 && rightDistance < 1.2) { // We see something to align to var rotation = rightDistance - leftDistance; var averageDistance = (leftDistance + rightDistance) / 2; - var rotationCorrection = rotationController.calculate(rotation); - var distanceCorrection = -distanceController.calculate(averageDistance); + var rotationCorrection = thetaController.calculate(rotation); + var distanceCorrection = -robotYController.calculate(averageDistance); + if (tagY.isPresent() && sawTag) { + var xCorrection = robotXController.calculate(tagY.get()); + robotCentricRequest.withVelocityX(xCorrection); + } drivetrain .setControl(robotCentricRequest.withRotationalRate(rotationCorrection).withVelocityY(distanceCorrection)); @@ -101,15 +168,32 @@ public void execute() { } } + private Optional getTagY() { + var photoResults = photonCamera.getAllUnreadResults(); + var lastTagResult = photoResults.stream() + .filter(result -> result.hasTargets()) + .flatMap(result -> result.getTargets().stream()) + .filter(target -> FIDUCIAL_IDS.contains(target.getFiducialId())) + .reduce((first, second) -> second); + + if (lastTagResult.isPresent()) { + var tag = lastTagResult.get(); + var cameraToTarget = tag.bestCameraToTarget; + return Optional.of(cameraToTarget.getY()); + } + return Optional.empty(); + } + @Override public boolean isFinished() { return alignmentSubsystem.getLeftDistance().isNear(targetDistance, ALIGNMENT_TOLERANCE) - && alignmentSubsystem.getRightDistance().isNear(targetDistance, ALIGNMENT_TOLERANCE); + && alignmentSubsystem.getRightDistance().isNear(targetDistance, ALIGNMENT_TOLERANCE) + && robotXController.atGoal(); } @Override public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.Idle()); + drivetrain.setControl(new SwerveRequest.SwerveDriveBrake()); } } From 53c4e837dd144a17bec65fb638dc00d4e065fdf5 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Mon, 24 Feb 2025 19:56:58 -0600 Subject: [PATCH 04/16] Pretty good 2 coral auto --- .../deploy/pathplanner/paths/One Coral.path | 10 +++--- .../deploy/pathplanner/paths/Two Coral.path | 20 +++++------ src/main/java/frc/robot/AutoCommands.java | 33 ++++++++++++++----- src/main/java/frc/robot/RobotContainer.java | 2 +- 4 files changed, 41 insertions(+), 24 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/One Coral.path b/src/main/deploy/pathplanner/paths/One Coral.path index e00b82d..dc1d3fc 100644 --- a/src/main/deploy/pathplanner/paths/One Coral.path +++ b/src/main/deploy/pathplanner/paths/One Coral.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.949275662251656, - "y": 6.298654801324502 + "x": 5.36, + "y": 5.27 }, "prevControl": { - "x": 6.610306291390727, - "y": 7.054118377483443 + "x": 6.021030629139071, + "y": 6.02546357615894 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 2.0, + "velocity": 0.0, "rotation": 149.56576424655643 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Two Coral.path b/src/main/deploy/pathplanner/paths/Two Coral.path index 5055f6e..1bbac84 100644 --- a/src/main/deploy/pathplanner/paths/Two Coral.path +++ b/src/main/deploy/pathplanner/paths/Two Coral.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.949275662251656, - "y": 6.298654801324502 + "x": 5.36, + "y": 5.27 }, "prevControl": null, "nextControl": { - "x": 3.4649627483443712, - "y": 6.857988410596025 + "x": 2.8756870860927153, + "y": 5.829333609271523 }, "isLocked": false, "linkedName": "CoralScoreID20point" @@ -24,7 +24,7 @@ "y": 7.867694536423841 }, "nextControl": { - "x": 0.3949129253532285, + "x": 0.39491292535322864, "y": 6.39290168821105 }, "isLocked": false, @@ -32,12 +32,12 @@ }, { "anchor": { - "x": 3.174399834437086, - "y": 5.586775662251655 + "x": 3.82, + "y": 5.43 }, "prevControl": { - "x": 1.9249793046357615, - "y": 6.051676324503311 + "x": 2.5705794701986755, + "y": 5.894900662251656 }, "nextControl": null, "isLocked": false, @@ -76,7 +76,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": -150.78075330951535 }, "reversed": false, diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index d7970ac..3dbf625 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -9,7 +9,6 @@ import static frc.robot.Constants.AlignmentConstants.REEF_L4_SCORE_POSES_RED; import static frc.robot.Constants.AlignmentConstants.REEF_L4_SCORE_POSE_BLUE; -import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.Command; @@ -152,15 +151,33 @@ private Command autoScoreCoral(Runnable armMethod, List redPoses, List

      drivetrain.setControl(new SwerveRequest.SwerveDriveBrake()))) + armSubsystem.run(armMethod) + .until(armSubsystem::isAtPosition) + .alongWith(alignToReef) .andThen( armSubsystem.run(armMethod) - .until(armSubsystem::isAtPosition) - .alongWith(alignToReef) - .andThen( - armSubsystem.run(armMethod) - .alongWith(gamePieceManipulatorSubsystem.run(gamePieceManipulatorSubsystem::ejectCoral)) - .withTimeout(1.0)))); + .alongWith(gamePieceManipulatorSubsystem.run(gamePieceManipulatorSubsystem::ejectCoral)) + .withTimeout(1.0))); + } + + public Command autoScoreCoralLevel4Auto() { + return autoScoreCoralAuto(armSubsystem::moveToLevel4, REEF_L4_SCORE_POSES_RED, REEF_L4_SCORE_POSE_BLUE); + } + + private Command autoScoreCoralAuto(Runnable armMethod, List redPoses, List bluePoses) { + var driveToReef = new DriveToNearestPose(drivetrain, () -> drivetrain.getState().Pose, redPoses, bluePoses); + var alignToReef = new AlignToReefCommand(drivetrain, alignmentSubsystem, Meters.of(0.34), highCamera); + + return ledSubsystem + .setLEDSegmentsAsCommand(kGreen, armSubsystem::isAtPosition, driveToReef::isFinished, alignToReef::isFinished) + .withDeadline( + armSubsystem.run(armMethod) + .until(armSubsystem::isAtPosition) + .alongWith(alignToReef) + .andThen( + armSubsystem.run(armMethod) + .alongWith(gamePieceManipulatorSubsystem.run(gamePieceManipulatorSubsystem::ejectCoral)) + .withTimeout(1.0))); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d046d4a..ebce5d1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -100,7 +100,7 @@ public RobotContainer() { } // Configure PathPlanner - NamedCommands.registerCommand("scoreCoralLevel4", autoCommands.autoScoreCoralLevel4()); + NamedCommands.registerCommand("scoreCoralLevel4", autoCommands.autoScoreCoralLevel4Auto()); NamedCommands.registerCommand("scoreCoralLevel3", autoCommands.autoScoreCoralLevel3()); NamedCommands.registerCommand( "intakeCoral", From 22f641217869bb8a7c787b53fb281f8bcc2481dc Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Mon, 24 Feb 2025 20:05:03 -0600 Subject: [PATCH 05/16] 9ish second 2 coral --- src/main/deploy/pathplanner/paths/Two Coral.path | 8 ++++---- src/main/java/frc/robot/AutoCommands.java | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Two Coral.path b/src/main/deploy/pathplanner/paths/Two Coral.path index 1bbac84..03faed8 100644 --- a/src/main/deploy/pathplanner/paths/Two Coral.path +++ b/src/main/deploy/pathplanner/paths/Two Coral.path @@ -54,9 +54,9 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.7854545454545454, - "maxWaypointRelativePos": 1.1823376623376558, + "maxWaypointRelativePos": 1.0, "constraints": { - "maxVelocity": 0.5, + "maxVelocity": 0.75, "maxAcceleration": 8.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -68,8 +68,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, + "maxVelocity": 5.0, + "maxAcceleration": 6.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index 3dbf625..f3597a1 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -157,7 +157,7 @@ private Command autoScoreCoral(Runnable armMethod, List redPoses, List

      redPoses, Li .andThen( armSubsystem.run(armMethod) .alongWith(gamePieceManipulatorSubsystem.run(gamePieceManipulatorSubsystem::ejectCoral)) - .withTimeout(1.0))); + .withTimeout(0.5))); } } From 7883035c5e2d2a2787f7e08aaa57402d323c6e33 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Mon, 24 Feb 2025 20:50:34 -0600 Subject: [PATCH 06/16] Working 3 coral --- .../deploy/pathplanner/autos/Three Coral.auto | 93 +++++++++++++++++++ .../deploy/pathplanner/autos/Two Coral.auto | 2 +- .../deploy/pathplanner/paths/Three Coral.path | 89 ++++++++++++++++++ .../deploy/pathplanner/paths/Two Coral.path | 28 +++--- 4 files changed, 197 insertions(+), 15 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Three Coral.auto create mode 100644 src/main/deploy/pathplanner/paths/Three Coral.path diff --git a/src/main/deploy/pathplanner/autos/Three Coral.auto b/src/main/deploy/pathplanner/autos/Three Coral.auto new file mode 100644 index 0000000..15b0e7c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Three Coral.auto @@ -0,0 +1,93 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "One Coral" + } + }, + { + "type": "named", + "data": { + "name": "scoreCoralLevel4" + } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Two Coral" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCoralLevel4" + } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCoralLevel4" + } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Two Coral.auto b/src/main/deploy/pathplanner/autos/Two Coral.auto index 82b8a5e..9333e73 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral.auto +++ b/src/main/deploy/pathplanner/autos/Two Coral.auto @@ -23,7 +23,7 @@ } }, { - "type": "parallel", + "type": "deadline", "data": { "commands": [ { diff --git a/src/main/deploy/pathplanner/paths/Three Coral.path b/src/main/deploy/pathplanner/paths/Three Coral.path new file mode 100644 index 0000000..ce38429 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral.path @@ -0,0 +1,89 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.915335264900662, + "y": 5.506870860927152 + }, + "prevControl": null, + "nextControl": { + "x": 3.0808692052980136, + "y": 6.339205298013245 + }, + "isLocked": false, + "linkedName": "CoralTwoEnd" + }, + { + "anchor": { + "x": 1.220364238410596, + "y": 7.046854304635761 + }, + "prevControl": { + "x": 2.0266763245033115, + "y": 7.765997516556291 + }, + "nextControl": { + "x": 0.3731207068101823, + "y": 6.2912046683435 + }, + "isLocked": false, + "linkedName": "CoralPickupLeft" + }, + { + "anchor": { + "x": 3.82, + "y": 5.43 + }, + "prevControl": { + "x": 2.931204470198675, + "y": 6.263476821192053 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.9565217391304481, + "rotationDegrees": 126.04541757739437 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7854545454545454, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.75, + "maxAcceleration": 8.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 5.0, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -149.8788174325336 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -150.78075330951535 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Two Coral.path b/src/main/deploy/pathplanner/paths/Two Coral.path index 03faed8..782b8c2 100644 --- a/src/main/deploy/pathplanner/paths/Two Coral.path +++ b/src/main/deploy/pathplanner/paths/Two Coral.path @@ -8,40 +8,40 @@ }, "prevControl": null, "nextControl": { - "x": 2.8756870860927153, - "y": 5.829333609271523 + "x": 4.2422185430463575, + "y": 6.857988410596026 }, "isLocked": false, "linkedName": "CoralScoreID20point" }, { "anchor": { - "x": 1.2421564569536423, - "y": 7.148551324503311 + "x": 1.220364238410596, + "y": 7.046854304635761 }, "prevControl": { - "x": 2.048468543046358, - "y": 7.867694536423841 + "x": 2.0266763245033115, + "y": 7.765997516556291 }, "nextControl": { - "x": 0.39491292535322864, - "y": 6.39290168821105 + "x": 0.3731207068101823, + "y": 6.2912046683435 }, "isLocked": false, - "linkedName": null + "linkedName": "CoralPickupLeft" }, { "anchor": { - "x": 3.82, - "y": 5.43 + "x": 3.915335264900662, + "y": 5.506870860927152 }, "prevControl": { - "x": 2.5705794701986755, - "y": 5.894900662251656 + "x": 1.9800289735099343, + "y": 6.437127483443708 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "CoralTwoEnd" } ], "rotationTargets": [ From e0d8a0a9276dab92ae9862579485782b10b4febc Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Mon, 24 Feb 2025 20:57:05 -0600 Subject: [PATCH 07/16] Revert mistaken teleop change --- src/main/java/frc/robot/AutoCommands.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index f3597a1..06e33df 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -151,13 +151,14 @@ private Command autoScoreCoral(Runnable armMethod, List redPoses, List

      redPoses, Li .andThen( armSubsystem.run(armMethod) .alongWith(gamePieceManipulatorSubsystem.run(gamePieceManipulatorSubsystem::ejectCoral)) - .withTimeout(0.5))); + .withTimeout(0.25))); } } From ee4722f6cdd9981fdf52f273c66deb36792987d9 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 12:55:23 -0600 Subject: [PATCH 08/16] LEDs, naming, and alignment improvements --- .../deploy/pathplanner/autos/One Coral.auto | 23 +++- .../deploy/pathplanner/autos/Three Coral.auto | 17 ++- .../deploy/pathplanner/autos/Two Coral.auto | 23 +++- src/main/java/frc/robot/AutoCommands.java | 53 +++++++-- src/main/java/frc/robot/RobotContainer.java | 20 ++-- .../robot/commands/AlignToReefCommand.java | 112 +++++++++--------- .../frc/robot/commands/AlignToTagCommand.java | 89 -------------- .../robot/commands/led/DefaultLEDCommand.java | 8 +- 8 files changed, 174 insertions(+), 171 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AlignToTagCommand.java diff --git a/src/main/deploy/pathplanner/autos/One Coral.auto b/src/main/deploy/pathplanner/autos/One Coral.auto index 18499ea..584848a 100644 --- a/src/main/deploy/pathplanner/autos/One Coral.auto +++ b/src/main/deploy/pathplanner/autos/One Coral.auto @@ -5,9 +5,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "One Coral" + "commands": [ + { + "type": "path", + "data": { + "pathName": "One Coral" + } + }, + { + "type": "named", + "data": { + "name": "ledDefault" + } + } + ] } }, { @@ -15,6 +28,12 @@ "data": { "name": "scoreCoralLevel4" } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Three Coral.auto b/src/main/deploy/pathplanner/autos/Three Coral.auto index 15b0e7c..aa9ff7b 100644 --- a/src/main/deploy/pathplanner/autos/Three Coral.auto +++ b/src/main/deploy/pathplanner/autos/Three Coral.auto @@ -5,9 +5,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "One Coral" + "commands": [ + { + "type": "path", + "data": { + "pathName": "One Coral" + } + }, + { + "type": "named", + "data": { + "name": "ledDefault" + } + } + ] } }, { diff --git a/src/main/deploy/pathplanner/autos/Two Coral.auto b/src/main/deploy/pathplanner/autos/Two Coral.auto index 9333e73..75178af 100644 --- a/src/main/deploy/pathplanner/autos/Two Coral.auto +++ b/src/main/deploy/pathplanner/autos/Two Coral.auto @@ -5,9 +5,22 @@ "data": { "commands": [ { - "type": "path", + "type": "deadline", "data": { - "pathName": "One Coral" + "commands": [ + { + "type": "path", + "data": { + "pathName": "One Coral" + } + }, + { + "type": "named", + "data": { + "name": "ledDefault" + } + } + ] } }, { @@ -46,6 +59,12 @@ "data": { "name": "scoreCoralLevel4" } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } } ] } diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index 06e33df..b6f3cd1 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -1,6 +1,9 @@ package frc.robot; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Percent; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.wpilibj.util.Color.kGreen; import static frc.robot.Constants.AlignmentConstants.REEF_L2_SCORE_POSES_RED; import static frc.robot.Constants.AlignmentConstants.REEF_L2_SCORE_POSE_BLUE; @@ -11,9 +14,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.LEDPattern.GradientType; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.commands.AlignToReefCommand; import frc.robot.commands.DriveToNearestPose; +import frc.robot.commands.IntakeCoralCommand; import frc.robot.subsystems.AlignmentSubsystem; import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -30,6 +37,7 @@ public class AutoCommands { private final CommandSwerveDrivetrain drivetrain; private final ArmSubsystem armSubsystem; + private final IndexerSubsystem indexerSubsystem; private final AlignmentSubsystem alignmentSubsystem; private final GamePieceManipulatorSubsystem gamePieceManipulatorSubsystem; private final LEDSubsystem ledSubsystem; @@ -40,6 +48,7 @@ public class AutoCommands { * * @param drivetrain drivetrain subsystem * @param armSubsystem arm subsystem + * @param indexerSubsystem indexer subsystem * @param alignmentSubsystem alinment subsystem * @param gamePieceManipulatorSubsystem game piece manipulator subsystem * @param indexerSubsystem indexer subsystem @@ -47,6 +56,7 @@ public class AutoCommands { public AutoCommands( CommandSwerveDrivetrain drivetrain, ArmSubsystem armSubsystem, + IndexerSubsystem indexerSubsysteml, AlignmentSubsystem alignmentSubsystem, GamePieceManipulatorSubsystem gamePieceManipulatorSubsystem, IndexerSubsystem indexerSubsystem, @@ -54,6 +64,7 @@ public AutoCommands( PhotonCamera highCamera) { this.drivetrain = drivetrain; this.armSubsystem = armSubsystem; + this.indexerSubsystem = indexerSubsystem; this.alignmentSubsystem = alignmentSubsystem; this.gamePieceManipulatorSubsystem = gamePieceManipulatorSubsystem; this.ledSubsystem = ledSubsystem; @@ -112,8 +123,8 @@ private void publishScoringPoses() { * * @return new command */ - public Command autoScoreCoralLevel4() { - return autoScoreCoral(armSubsystem::moveToLevel4, REEF_L4_SCORE_POSES_RED, REEF_L4_SCORE_POSE_BLUE); + public Command scoreCoralLevel4() { + return scoreCoralTeleop(armSubsystem::moveToLevel4, REEF_L4_SCORE_POSES_RED, REEF_L4_SCORE_POSE_BLUE); } /** @@ -127,8 +138,8 @@ public Command autoScoreCoralLevel4() { * * @return new command */ - public Command autoScoreCoralLevel3() { - return autoScoreCoral(armSubsystem::moveToLevel3, REEF_L3_SCORE_POSES_RED, REEF_L3_SCORE_POSE_BLUE); + public Command scoreCoralLevel3() { + return scoreCoralTeleop(armSubsystem::moveToLevel3, REEF_L3_SCORE_POSES_RED, REEF_L3_SCORE_POSE_BLUE); } /** @@ -144,12 +155,17 @@ public Command driveToCoralLevel2() { REEF_L2_SCORE_POSE_BLUE); } - private Command autoScoreCoral(Runnable armMethod, List redPoses, List bluePoses) { + private Command scoreCoralTeleop(Runnable armMethod, List redPoses, List bluePoses) { var driveToReef = new DriveToNearestPose(drivetrain, () -> drivetrain.getState().Pose, redPoses, bluePoses); var alignToReef = new AlignToReefCommand(drivetrain, alignmentSubsystem, Meters.of(0.34), highCamera); return ledSubsystem - .setLEDSegmentsAsCommand(kGreen, armSubsystem::isAtPosition, driveToReef::isFinished, alignToReef::isFinished) + .setLEDSegmentsAsCommand( + kGreen, + driveToReef::isFinished, + armSubsystem::isAtPosition, + alignToReef::atDistanceGoal, + alignToReef::atLateralGoal) .withDeadline( driveToReef.andThen( armSubsystem.run(armMethod) @@ -161,16 +177,24 @@ private Command autoScoreCoral(Runnable armMethod, List redPoses, List

      redPoses, List bluePoses) { - var driveToReef = new DriveToNearestPose(drivetrain, () -> drivetrain.getState().Pose, redPoses, bluePoses); var alignToReef = new AlignToReefCommand(drivetrain, alignmentSubsystem, Meters.of(0.34), highCamera); return ledSubsystem - .setLEDSegmentsAsCommand(kGreen, armSubsystem::isAtPosition, driveToReef::isFinished, alignToReef::isFinished) + .setLEDSegmentsAsCommand( + kGreen, + armSubsystem::isAtPosition, + alignToReef::atDistanceGoal, + alignToReef::atLateralGoal) .withDeadline( armSubsystem.run(armMethod) .until(armSubsystem::isAtPosition) @@ -178,7 +202,16 @@ private Command autoScoreCoralAuto(Runnable armMethod, List redPoses, Li .andThen( armSubsystem.run(armMethod) .alongWith(gamePieceManipulatorSubsystem.run(gamePieceManipulatorSubsystem::ejectCoral)) - .withTimeout(0.25))); + .withTimeout(0.25))) + .finallyDo(() -> ledSubsystem.runPattern(LEDPattern.kOff)); + } + + public Command intakeCoral() { + return new IntakeCoralCommand(indexerSubsystem, gamePieceManipulatorSubsystem, armSubsystem).deadlineFor( + ledSubsystem.runPatternAsCommand( + LEDPattern.gradient(GradientType.kContinuous, Color.kGray, Color.kWhite) + .scrollAtRelativeSpeed(Percent.per(Second).of(100)) + .breathe(Seconds.of(0.75)))); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ebce5d1..c8c035f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.EjectCoralCommand; -import frc.robot.commands.IntakeCoralCommand; import frc.robot.commands.PhotonVisionCommand; import frc.robot.commands.TuneArmCommand; import frc.robot.commands.led.DefaultLEDCommand; @@ -75,6 +74,7 @@ public class RobotContainer { private final AutoCommands autoCommands = new AutoCommands( drivetrain, armSubsystem, + indexerSubsystem, alignmentSubsystem, gamePieceManipulatorSubsystem, indexerSubsystem, @@ -100,11 +100,10 @@ public RobotContainer() { } // Configure PathPlanner - NamedCommands.registerCommand("scoreCoralLevel4", autoCommands.autoScoreCoralLevel4Auto()); - NamedCommands.registerCommand("scoreCoralLevel3", autoCommands.autoScoreCoralLevel3()); - NamedCommands.registerCommand( - "intakeCoral", - new IntakeCoralCommand(indexerSubsystem, gamePieceManipulatorSubsystem, armSubsystem)); + NamedCommands.registerCommand("scoreCoralLevel4", autoCommands.scoreCoralLevel4Auto()); + NamedCommands.registerCommand("ledDefault", new DefaultLEDCommand(ledSubsystem)); + NamedCommands + .registerCommand("intakeCoral", autoCommands.intakeCoral().andThen(new DefaultLEDCommand(ledSubsystem))); NamedCommands.registerCommand("parkArm", armSubsystem.runOnce(armSubsystem::park).finallyDo(armSubsystem::stop)); autoChooser = AutoBuilder.buildAutoChooser("Tests"); SmartDashboard.putData("Auto Mode", autoChooser); @@ -149,10 +148,7 @@ private void configureBindings() { drivetrain.registerTelemetry(drivetrainTelemetry::telemeterize); // Coral bindings - controlBindings.intakeCoral() - .ifPresent( - trigger -> trigger - .onTrue(new IntakeCoralCommand(indexerSubsystem, gamePieceManipulatorSubsystem, armSubsystem))); + controlBindings.intakeCoral().ifPresent(trigger -> trigger.onTrue(autoCommands.intakeCoral())); controlBindings.ejectCoral() .ifPresent( trigger -> trigger @@ -224,8 +220,8 @@ private void configureBindings() { controlBindings.slowMode().ifPresent(trigger -> trigger.whileTrue(slowModeCommand)); - controlBindings.scoreCoralLevel3().ifPresent(trigger -> trigger.whileTrue(autoCommands.autoScoreCoralLevel3())); - controlBindings.scoreCoralLevel4().ifPresent(trigger -> trigger.whileTrue(autoCommands.autoScoreCoralLevel4())); + controlBindings.scoreCoralLevel3().ifPresent(trigger -> trigger.whileTrue(autoCommands.scoreCoralLevel3())); + controlBindings.scoreCoralLevel4().ifPresent(trigger -> trigger.whileTrue(autoCommands.scoreCoralLevel4())); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/AlignToReefCommand.java b/src/main/java/frc/robot/commands/AlignToReefCommand.java index e99636d..9cb0caf 100644 --- a/src/main/java/frc/robot/commands/AlignToReefCommand.java +++ b/src/main/java/frc/robot/commands/AlignToReefCommand.java @@ -1,12 +1,10 @@ package frc.robot.commands; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inch; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.MetersPerSecondPerSecond; -import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.RadiansPerSecond; import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; import static frc.robot.Constants.AlignmentConstants.ALIGNMENT_TOLERANCE; @@ -14,14 +12,12 @@ import static frc.robot.Constants.AlignmentConstants.MAX_ALIGN_ANGULAR_VELOCITY; import static frc.robot.Constants.AlignmentConstants.MAX_ALIGN_TRANSLATION_ACCELERATION; import static frc.robot.Constants.AlignmentConstants.MAX_ALIGN_TRANSLATION_VELOCITY; -import static frc.robot.Constants.DriveToPoseConstants.THETA_kD; -import static frc.robot.Constants.DriveToPoseConstants.THETA_kI; -import static frc.robot.Constants.DriveToPoseConstants.THETA_kP; import static frc.robot.Constants.DriveToPoseConstants.X_kD; import static frc.robot.Constants.DriveToPoseConstants.X_kI; import static frc.robot.Constants.DriveToPoseConstants.X_kP; import static frc.robot.Constants.DriveToPoseConstants.Y_kD; import static frc.robot.Constants.DriveToPoseConstants.Y_kI; +import static frc.robot.Constants.DriveToPoseConstants.Y_kP; import static java.util.stream.Collectors.toUnmodifiableSet; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; @@ -29,7 +25,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.subsystems.AlignmentSubsystem; @@ -48,12 +43,13 @@ public class AlignToReefCommand extends Command { private static final Set FIDUCIAL_IDS = Stream.of(17, 18, 19, 20, 21, 22, 6, 7, 8, 9, 10, 11) .collect(toUnmodifiableSet()); // Alignment, depending which reef branch we want to score on - private static final double LEFT_TAG_ALIGNMENT = -0.23; - private static final double RIGHT_TAG_ALIGNMENT = 0.08; + private static final double LEFT_LATERAL_TARGET = -0.23; + private static final double RIGHT_LATERAL_TARGET = 0.08; - private static final Distance TRANSLATION_TOLERANCE = Inches.of(0.5); - private static final Distance TAG_TOLERANCE = Inch.of(0.6); - private static final Angle THETA_TOLERANCE = Degrees.of(1.0); + private static final Distance DISTANCE_TOLERANCE = Inches.of(0.5); + private static final Distance LATERAL_TOLERANCE = Inch.of(0.6); + // Theta is the difference between the two CANrange distances, in meters + private static final double THETA_TOLERANCE = 0.017; private final CommandSwerveDrivetrain drivetrain; private final AlignmentSubsystem alignmentSubsystem; @@ -67,29 +63,25 @@ public class AlignToReefCommand extends Command { MAX_ALIGN_ANGULAR_VELOCITY.in(RadiansPerSecond), MAX_ALIGN_ANGULAR_ACCELERATION.in(RadiansPerSecondPerSecond)); - private final ProfiledPIDController robotYController = new ProfiledPIDController( + private final ProfiledPIDController distanceController = new ProfiledPIDController( X_kP, X_kI, X_kD, TRANSLATION_CONSTRAINTS); - private final ProfiledPIDController robotXController = new ProfiledPIDController( - 6.0, + private final ProfiledPIDController lateralController = new ProfiledPIDController( + Y_kP, Y_kI, Y_kD, TRANSLATION_CONSTRAINTS); - private final ProfiledPIDController thetaController = new ProfiledPIDController( - THETA_kP, - THETA_kI, - THETA_kD, - OMEGA_CONSTRAINTS); + private final ProfiledPIDController thetaController = new ProfiledPIDController(5.0, 0.0, 0.0, OMEGA_CONSTRAINTS); private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() .withDriveRequestType(DriveRequestType.Velocity) .withSteerRequestType(SteerRequestType.MotionMagicExpo); - private double alignmentTarget; + private double tagLateralTarget; private boolean sawTag = false; /** @@ -98,6 +90,7 @@ public class AlignToReefCommand extends Command { * @param drivetrain drivetrain subsystem * @param alignmentSubsystem alignment subsystem * @param targetDistance distance the robot should be from the reef + * @param photonCamera photon camera to use for AprilTag */ public AlignToReefCommand( CommandSwerveDrivetrain drivetrain, @@ -109,59 +102,65 @@ public AlignToReefCommand( this.photonCamera = photonCamera; this.targetDistance = targetDistance; - robotYController.setTolerance(TRANSLATION_TOLERANCE.in(Meters)); - robotXController.setTolerance(TAG_TOLERANCE.in(Meters)); - thetaController.enableContinuousInput(-Math.PI, Math.PI); - thetaController.setTolerance(THETA_TOLERANCE.in(Radians)); + distanceController.setTolerance(DISTANCE_TOLERANCE.in(Meters)); + lateralController.setTolerance(LATERAL_TOLERANCE.in(Meters)); + thetaController.setTolerance(THETA_TOLERANCE); addRequirements(drivetrain, alignmentSubsystem); } @Override public void initialize() { - robotYController.setGoal(targetDistance.in(Meters)); + distanceController.setGoal(targetDistance.in(Meters)); thetaController.setGoal(0); var leftDistance = alignmentSubsystem.getLeftDistance().in(Meters); var rightDistance = alignmentSubsystem.getRightDistance().in(Meters); - var rotation = rightDistance - leftDistance; + var theta = rightDistance - leftDistance; var averageDistance = (leftDistance + rightDistance) / 2; - thetaController.reset(rotation); - robotYController.reset(averageDistance); - - getTagY().ifPresentOrElse(tagY -> { - alignmentTarget = Math.abs(tagY - RIGHT_TAG_ALIGNMENT) < Math.abs(tagY - LEFT_TAG_ALIGNMENT) ? RIGHT_TAG_ALIGNMENT - : LEFT_TAG_ALIGNMENT; - robotXController.setGoal(alignmentTarget); - robotXController.reset(tagY); - sawTag = true; - }, () -> { - sawTag = false; - robotXController.reset(0); - }); + thetaController.reset(theta); + distanceController.reset(averageDistance); + sawTag = false; + + // If a tag is visible, set side-to-side goal + getTagY().ifPresent(tagY -> initLateralTag(tagY)); + } + + private void initLateralTag(double tagY) { + // Choose the nearest alignment target + tagLateralTarget = Math.abs(tagY - RIGHT_LATERAL_TARGET) < Math.abs(tagY - LEFT_LATERAL_TARGET) + ? RIGHT_LATERAL_TARGET + : LEFT_LATERAL_TARGET; + lateralController.setGoal(tagLateralTarget); + lateralController.reset(tagY); + sawTag = true; } @Override public void execute() { var leftDistance = alignmentSubsystem.getLeftDistance().in(Meters); var rightDistance = alignmentSubsystem.getRightDistance().in(Meters); - var tagY = getTagY(); if (leftDistance < 1.2 && rightDistance < 1.2) { // We see something to align to - var rotation = rightDistance - leftDistance; + var theta = rightDistance - leftDistance; var averageDistance = (leftDistance + rightDistance) / 2; - var rotationCorrection = thetaController.calculate(rotation); - var distanceCorrection = -robotYController.calculate(averageDistance); - if (tagY.isPresent() && sawTag) { - var xCorrection = robotXController.calculate(tagY.get()); - robotCentricRequest.withVelocityX(xCorrection); - } - - drivetrain - .setControl(robotCentricRequest.withRotationalRate(rotationCorrection).withVelocityY(distanceCorrection)); + var thetaCorrection = thetaController.calculate(theta); + var distanceCorrection = -distanceController.calculate(averageDistance); + getTagY().ifPresentOrElse(tagY -> { + if (!sawTag) { + // This is the first time a tag has been seen, set goal + initLateralTag(tagY); + } + // Tag is visible, do alignment + var lateralCorrection = lateralController.calculate(tagY); + robotCentricRequest.withVelocityX(lateralCorrection); + }, () -> robotCentricRequest.withVelocityX(0)); + + // The robot is rotated 90 degrees, so Y is distance and X is lateral + drivetrain.setControl(robotCentricRequest.withRotationalRate(thetaCorrection).withVelocityY(distanceCorrection)); } else { // We don't see anything or it's too far away to safely align drivetrain.setControl(new SwerveRequest.Idle()); @@ -174,7 +173,7 @@ private Optional getTagY() { .filter(result -> result.hasTargets()) .flatMap(result -> result.getTargets().stream()) .filter(target -> FIDUCIAL_IDS.contains(target.getFiducialId())) - .reduce((first, second) -> second); + .findFirst(); if (lastTagResult.isPresent()) { var tag = lastTagResult.get(); @@ -186,9 +185,16 @@ private Optional getTagY() { @Override public boolean isFinished() { + return atDistanceGoal() && atLateralGoal(); + } + + public boolean atDistanceGoal() { return alignmentSubsystem.getLeftDistance().isNear(targetDistance, ALIGNMENT_TOLERANCE) - && alignmentSubsystem.getRightDistance().isNear(targetDistance, ALIGNMENT_TOLERANCE) - && robotXController.atGoal(); + && alignmentSubsystem.getRightDistance().isNear(targetDistance, ALIGNMENT_TOLERANCE); + } + + public boolean atLateralGoal() { + return lateralController.atGoal(); } @Override diff --git a/src/main/java/frc/robot/commands/AlignToTagCommand.java b/src/main/java/frc/robot/commands/AlignToTagCommand.java deleted file mode 100644 index a55c3e6..0000000 --- a/src/main/java/frc/robot/commands/AlignToTagCommand.java +++ /dev/null @@ -1,89 +0,0 @@ -package frc.robot.commands; - -import static java.util.stream.Collectors.toUnmodifiableSet; - -import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType; -import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import java.util.Set; -import java.util.stream.Stream; -import org.photonvision.PhotonCamera; - -/** - * Command to align to an AprilTag on the reef - */ -public class AlignToTagCommand extends Command { - - // ID of the tags on the reef - private final Set FIDUCIAL_IDS = Stream.of(17, 18, 19, 20, 21, 22, 6, 7, 8, 9, 10, 11) - .collect(toUnmodifiableSet()); - - private final CommandSwerveDrivetrain drivetrain; - private final PhotonCamera photonCamera; - - private final PIDController positionController = new PIDController(5.0, 0, 0); - - private final double leftAlign = -0.2; - private final double rightAlignY = 0.2; - - private final SwerveRequest.RobotCentric robotCentricRequest = new SwerveRequest.RobotCentric() - .withDriveRequestType(DriveRequestType.Velocity) - .withSteerRequestType(SteerRequestType.MotionMagicExpo); - - /** - * Constructs a new command - * - * @param drivetrain drivetrain subsystem - */ - public AlignToTagCommand(CommandSwerveDrivetrain drivetrain, PhotonCamera photonCamera) { - this.drivetrain = drivetrain; - this.photonCamera = photonCamera; - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - positionController.reset(); - } - - @Override - public void execute() { - var photoResults = photonCamera.getAllUnreadResults(); - var lastTagResult = photoResults.stream() - .filter(result -> result.hasTargets()) - .flatMap(result -> result.getTargets().stream()) - .filter(target -> FIDUCIAL_IDS.contains(target.getFiducialId())) - .reduce((first, second) -> second); - - lastTagResult.ifPresentOrElse(tag -> { - var cameraToTarget = tag.bestCameraToTarget; - if (Math.abs(cameraToTarget.getY() - leftAlign) < Math.abs(cameraToTarget.getY() - rightAlignY)) { - positionController.setSetpoint(leftAlign); - } else { - positionController.setSetpoint(rightAlignY); - } - - var correction = positionController.calculate(cameraToTarget.getY()); - drivetrain.setControl(robotCentricRequest.withVelocityX(correction)); - }, () -> { - // TODO what do you do here? - // Did we ever see a tag, if so, keep moving that direction for until a timeout? What did we do in 2022? - }); - - } - - @Override - public boolean isFinished() { - return positionController.atSetpoint(); - } - - @Override - public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.Idle()); - } - -} diff --git a/src/main/java/frc/robot/commands/led/DefaultLEDCommand.java b/src/main/java/frc/robot/commands/led/DefaultLEDCommand.java index 9ad110c..8108559 100644 --- a/src/main/java/frc/robot/commands/led/DefaultLEDCommand.java +++ b/src/main/java/frc/robot/commands/led/DefaultLEDCommand.java @@ -1,7 +1,11 @@ package frc.robot.commands.led; +import static edu.wpi.first.units.Units.Percent; +import static edu.wpi.first.units.Units.Second; + import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.util.Color; @@ -62,7 +66,9 @@ public void execute() { .setCandyCane(candyCaneState ? Color.kBlue : Color.kOrange, candyCaneState ? Color.kOrange : Color.kBlue); break; default: - ledSubsystem.runPattern(LEDPattern.kOff); + ledSubsystem.runPattern( + LEDPattern.gradient(GradientType.kContinuous, Color.kBlue, Color.kOrange) + .scrollAtRelativeSpeed(Percent.per(Second).of(25))); break; } } From b242e0de6b0203f11c9b22530020e9bec7f6253b Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 13:48:46 -0600 Subject: [PATCH 09/16] Shoot algae while elevator and arm are probably moving --- src/main/java/frc/robot/Constants.java | 2 + src/main/java/frc/robot/RobotContainer.java | 13 ++-- .../frc/robot/commands/AlgaeBargeCommand.java | 72 +++++++++++++++++++ .../frc/robot/controls/ControlBindings.java | 4 +- .../controls/JoystickControlBindings.java | 2 +- .../robot/controls/XBoxControlBindings.java | 5 ++ .../frc/robot/subsystems/ArmSubsystem.java | 9 +++ 7 files changed, 98 insertions(+), 9 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AlgaeBargeCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1378e23..72e512b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -238,6 +238,7 @@ public static class ArmConstants { public static final Distance ALGAE_LEVEL_2_HEIGHT = Meters.of(0.317); public static final Distance ALGAE_BARGE_HEIGHT = Meters.of(0.72); public static final Distance ALGAE_PROCESSOR_HEIGHT = Meters.of(0.0); + public static final Distance ALGAE_HOLD_HEIGHT = Meters.zero(); public static final Angle ARM_INTAKE_ANGLE = Rotations.of(0.765); public static final Angle LEVEL_2_ANGLE = Rotations.of(0.68212890625); @@ -247,6 +248,7 @@ public static class ArmConstants { public static final Angle ALGAE_LEVEL_2_ANGLE = Rotations.of(0.105); public static final Angle ALGAE_BARGE_ANGLE = Rotation.of(0.405); public static final Angle ALGAE_PROCESSOR_ANGLE = Rotation.of(0.963); + public static final Angle ALGAE_HOLD_ANGLE = Rotations.of(0.35); // Values for Mechanism2d visualization public static final Distance ARM_PIVOT_LENGTH = Meters.of(0.577); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c8c035f..42e01fe 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -22,6 +22,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.AlgaeBargeCommand; import frc.robot.commands.EjectCoralCommand; import frc.robot.commands.PhotonVisionCommand; import frc.robot.commands.TuneArmCommand; @@ -193,15 +194,15 @@ private void configureBindings() { gamePieceManipulatorSubsystem.stop(); }))); - controlBindings.moveArmToBarge().ifPresent(trigger -> trigger.onTrue(Commands.run(() -> { - armSubsystem.moveToBarge(); + controlBindings.holdAlgae().ifPresent(trigger -> trigger.onTrue(Commands.run(() -> { + armSubsystem.moveToHoldAlgae(); gamePieceManipulatorSubsystem.activeHoldAlgae(); }, armSubsystem, gamePieceManipulatorSubsystem).finallyDo(armSubsystem::stop))); - controlBindings.shootAlgae().ifPresent(trigger -> trigger.whileTrue(Commands.run(() -> { - armSubsystem.moveToBarge(); - gamePieceManipulatorSubsystem.shootAlgae(); - }, armSubsystem, gamePieceManipulatorSubsystem).finallyDo(gamePieceManipulatorSubsystem::stop))); + controlBindings.shootAlgae() + .ifPresent( + trigger -> trigger + .whileTrue(new AlgaeBargeCommand(armSubsystem, gamePieceManipulatorSubsystem, ledSubsystem))); controlBindings.moveArmToProcessor().ifPresent(trigger -> trigger.onTrue(Commands.run(() -> { armSubsystem.moveToProcessor(); diff --git a/src/main/java/frc/robot/commands/AlgaeBargeCommand.java b/src/main/java/frc/robot/commands/AlgaeBargeCommand.java new file mode 100644 index 0000000..06e64fb --- /dev/null +++ b/src/main/java/frc/robot/commands/AlgaeBargeCommand.java @@ -0,0 +1,72 @@ +package frc.robot.commands; + +import static edu.wpi.first.units.Units.Meters; +import static frc.robot.Constants.ArmConstants.ALGAE_BARGE_ANGLE; +import static frc.robot.Constants.ArmConstants.ALGAE_BARGE_HEIGHT; + +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.GamePieceManipulatorSubsystem; +import frc.robot.subsystems.LEDSubsystem; + +/** + * Command to try to launch algae onto the barge. + */ +public class AlgaeBargeCommand extends Command { + private final ArmSubsystem armSubsystem; + private final GamePieceManipulatorSubsystem gamePieceManipulatorSubsystem; + private final LEDSubsystem ledSubsystem; + + /** + * Constructs a new AlgaeBargeCommand. + * + * @param armSubsystem arm subsystem + * @param gamePieceManipulatorSubsystem game piece manipulator subsystem + * @param ledSubsystem led subsystem + */ + public AlgaeBargeCommand( + ArmSubsystem armSubsystem, + GamePieceManipulatorSubsystem gamePieceManipulatorSubsystem, + LEDSubsystem ledSubsystem) { + this.armSubsystem = armSubsystem; + this.gamePieceManipulatorSubsystem = gamePieceManipulatorSubsystem; + this.ledSubsystem = ledSubsystem; + + addRequirements(armSubsystem, gamePieceManipulatorSubsystem, ledSubsystem); + } + + @Override + public void initialize() { + gamePieceManipulatorSubsystem.activeHoldAlgae(); + } + + @Override + public void execute() { + // Move the elevator and arm to the barge + armSubsystem.moveToBarge(); + ledSubsystem.setLEDSegments(Color.kGreen, this::isElevatorReady, this::isArmReady); + + if (isElevatorReady() && isArmReady()) { + // Launch the algae when the elevator and arm are >= 75% of the way to the barge + gamePieceManipulatorSubsystem.shootAlgae(); + } + } + + private boolean isElevatorReady() { + return armSubsystem.getElevatorMeters() >= ALGAE_BARGE_HEIGHT.in(Meters) * 0.75; + } + + private boolean isArmReady() { + return armSubsystem.getArmAngle().gte(ALGAE_BARGE_ANGLE); + } + + @Override + public void end(boolean interrupted) { + gamePieceManipulatorSubsystem.stop(); + armSubsystem.stop(); + ledSubsystem.runPattern(LEDPattern.kOff); + } + +} diff --git a/src/main/java/frc/robot/controls/ControlBindings.java b/src/main/java/frc/robot/controls/ControlBindings.java index 1f9d794..a6b7b4a 100644 --- a/src/main/java/frc/robot/controls/ControlBindings.java +++ b/src/main/java/frc/robot/controls/ControlBindings.java @@ -154,11 +154,11 @@ public Optional moveArmToReefAlgaeLevel2() { } /** - * Moves the arm to the position to score algae on the barge. + * Moves the arm to the position to hold algae and runs the wheels to grip it. * * @return optional command */ - public Optional moveArmToBarge() { + public Optional holdAlgae() { return Optional.empty(); } diff --git a/src/main/java/frc/robot/controls/JoystickControlBindings.java b/src/main/java/frc/robot/controls/JoystickControlBindings.java index 3de2dfc..7d88d05 100644 --- a/src/main/java/frc/robot/controls/JoystickControlBindings.java +++ b/src/main/java/frc/robot/controls/JoystickControlBindings.java @@ -123,7 +123,7 @@ public Optional slowMode() { } @Override - public Optional moveArmToBarge() { + public Optional holdAlgae() { return Optional.of(leftJoystick.button(10)); } diff --git a/src/main/java/frc/robot/controls/XBoxControlBindings.java b/src/main/java/frc/robot/controls/XBoxControlBindings.java index 6662d0a..c77cb61 100644 --- a/src/main/java/frc/robot/controls/XBoxControlBindings.java +++ b/src/main/java/frc/robot/controls/XBoxControlBindings.java @@ -78,4 +78,9 @@ public Optional scoreCoralLevel4() { public Optional scoreCoralLevel3() { return Optional.of(driverController.a()); } + + @Override + public Optional shootAlgae() { + return Optional.of(driverController.leftTrigger()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index c363fe0..427cd3c 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -14,6 +14,8 @@ import static edu.wpi.first.units.Units.Volts; import static frc.robot.Constants.ArmConstants.ALGAE_BARGE_ANGLE; import static frc.robot.Constants.ArmConstants.ALGAE_BARGE_HEIGHT; +import static frc.robot.Constants.ArmConstants.ALGAE_HOLD_ANGLE; +import static frc.robot.Constants.ArmConstants.ALGAE_HOLD_HEIGHT; import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_1_ANGLE; import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_1_HEIGHT; import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_2_ANGLE; @@ -434,6 +436,13 @@ public void moveToBarge() { moveToPosition(ALGAE_BARGE_HEIGHT, ALGAE_BARGE_ANGLE); } + /** + * Moves the arm to the position to hold algae. + */ + public void moveToHoldAlgae() { + moveToPosition(ALGAE_HOLD_HEIGHT, ALGAE_HOLD_ANGLE); + } + /** * Moves the arm to the position to score in the processor. */ From 2788d0e0620f598fa5e27b783a99016dd7007ab0 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 14:23:06 -0600 Subject: [PATCH 10/16] Button bindings from Tate --- src/main/java/frc/robot/Constants.java | 8 ++++---- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../frc/robot/controls/ControlBindings.java | 4 ++-- .../controls/JoystickControlBindings.java | 19 ++----------------- .../frc/robot/subsystems/ArmSubsystem.java | 12 ++++++------ 5 files changed, 16 insertions(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 72e512b..2d6f3f3 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -234,8 +234,8 @@ public static class ArmConstants { public static final Distance LEVEL_2_HEIGHT = Meters.of(0.1946611669921875); public static final Distance LEVEL_3_HEIGHT = Meters.of(0); public static final Distance LEVEL_4_HEIGHT = Meters.of(0.675); - public static final Distance ALGAE_LEVEL_1_HEIGHT = Meters.of(0.0); - public static final Distance ALGAE_LEVEL_2_HEIGHT = Meters.of(0.317); + public static final Distance ALGAE_LOWER_HEIGHT = Meters.of(0.0); + public static final Distance ALGAE_UPPER_HEIGHT = Meters.of(0.317); public static final Distance ALGAE_BARGE_HEIGHT = Meters.of(0.72); public static final Distance ALGAE_PROCESSOR_HEIGHT = Meters.of(0.0); public static final Distance ALGAE_HOLD_HEIGHT = Meters.zero(); @@ -244,8 +244,8 @@ public static class ArmConstants { public static final Angle LEVEL_2_ANGLE = Rotations.of(0.68212890625); public static final Angle LEVEL_3_ANGLE = Rotations.of(0.172); public static final Angle LEVEL_4_ANGLE = Rotations.of(0.16); - public static final Angle ALGAE_LEVEL_1_ANGLE = Rotations.of(0.06); - public static final Angle ALGAE_LEVEL_2_ANGLE = Rotations.of(0.105); + public static final Angle ALGAE_LOWER_ANGLE = Rotations.of(0.06); + public static final Angle ALGAE_UPPER_ANGLE = Rotations.of(0.105); public static final Angle ALGAE_BARGE_ANGLE = Rotation.of(0.405); public static final Angle ALGAE_PROCESSOR_ANGLE = Rotation.of(0.963); public static final Angle ALGAE_HOLD_ANGLE = Rotations.of(0.35); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 42e01fe..5b75b0b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -175,7 +175,7 @@ private void configureBindings() { controlBindings.tuneArm().ifPresent(trigger -> trigger.onTrue(new TuneArmCommand(armSubsystem))); - controlBindings.moveArmToReefAlgaeLevel1() + controlBindings.moveArmToReefLowerAlgae() .ifPresent( trigger -> trigger.onTrue( armSubsystem.run(armSubsystem::moveToAlgaeLevel1) @@ -184,7 +184,7 @@ private void configureBindings() { armSubsystem.stop(); gamePieceManipulatorSubsystem.stop(); }))); - controlBindings.moveArmToReefAlgaeLevel2() + controlBindings.moveArmToReefUpperAlgae() .ifPresent( trigger -> trigger.onTrue( armSubsystem.run(armSubsystem::moveToAlgaeLevel2) diff --git a/src/main/java/frc/robot/controls/ControlBindings.java b/src/main/java/frc/robot/controls/ControlBindings.java index a6b7b4a..49131c5 100644 --- a/src/main/java/frc/robot/controls/ControlBindings.java +++ b/src/main/java/frc/robot/controls/ControlBindings.java @@ -140,7 +140,7 @@ public Optional tuneArm() { * * @return optional trigger */ - public Optional moveArmToReefAlgaeLevel1() { + public Optional moveArmToReefLowerAlgae() { return Optional.empty(); } @@ -149,7 +149,7 @@ public Optional moveArmToReefAlgaeLevel1() { * * @return optional trigger */ - public Optional moveArmToReefAlgaeLevel2() { + public Optional moveArmToReefUpperAlgae() { return Optional.empty(); } diff --git a/src/main/java/frc/robot/controls/JoystickControlBindings.java b/src/main/java/frc/robot/controls/JoystickControlBindings.java index 7d88d05..eba1039 100644 --- a/src/main/java/frc/robot/controls/JoystickControlBindings.java +++ b/src/main/java/frc/robot/controls/JoystickControlBindings.java @@ -67,11 +67,6 @@ public Optional climb() { return Optional.of(rightJoystick.button(9)); } - @Override - public Optional seedFieldCentric() { - return Optional.of(leftJoystick.button(7)); - } - @Override public Optional intakeCoral() { return Optional.of(leftJoystick.trigger()); @@ -87,23 +82,18 @@ public Optional releaseCoral() { return Optional.of(rightJoystick.trigger()); } - @Override - public Optional parkArm() { - return Optional.of(leftJoystick.povLeft()); - } - @Override public Optional tuneArm() { return Optional.of(leftJoystick.button(8)); } @Override - public Optional moveArmToReefAlgaeLevel1() { + public Optional moveArmToReefLowerAlgae() { return Optional.of(leftJoystick.povDown()); } @Override - public Optional moveArmToReefAlgaeLevel2() { + public Optional moveArmToReefUpperAlgae() { return Optional.of(leftJoystick.povUp()); } @@ -129,11 +119,6 @@ public Optional holdAlgae() { @Override public Optional shootAlgae() { - return Optional.of(leftJoystick.button(2)); - } - - @Override - public Optional moveArmToProcessor() { return Optional.of(leftJoystick.button(9)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 427cd3c..fbe5bf3 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -16,10 +16,10 @@ import static frc.robot.Constants.ArmConstants.ALGAE_BARGE_HEIGHT; import static frc.robot.Constants.ArmConstants.ALGAE_HOLD_ANGLE; import static frc.robot.Constants.ArmConstants.ALGAE_HOLD_HEIGHT; -import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_1_ANGLE; -import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_1_HEIGHT; -import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_2_ANGLE; -import static frc.robot.Constants.ArmConstants.ALGAE_LEVEL_2_HEIGHT; +import static frc.robot.Constants.ArmConstants.ALGAE_LOWER_ANGLE; +import static frc.robot.Constants.ArmConstants.ALGAE_LOWER_HEIGHT; +import static frc.robot.Constants.ArmConstants.ALGAE_UPPER_ANGLE; +import static frc.robot.Constants.ArmConstants.ALGAE_UPPER_HEIGHT; import static frc.robot.Constants.ArmConstants.ARM_DANGER_MAX; import static frc.robot.Constants.ArmConstants.ARM_DANGER_MIN; import static frc.robot.Constants.ArmConstants.ARM_DANGER_TOLERANCE; @@ -419,14 +419,14 @@ public void moveToLevel4() { * Moves arm to the position to extract algae from the lower reef location */ public void moveToAlgaeLevel1() { - moveToPosition(ALGAE_LEVEL_1_HEIGHT, ALGAE_LEVEL_1_ANGLE); + moveToPosition(ALGAE_LOWER_HEIGHT, ALGAE_LOWER_ANGLE); } /** * Moves arm to the position to extract algae from the higher reef location */ public void moveToAlgaeLevel2() { - moveToPosition(ALGAE_LEVEL_2_HEIGHT, ALGAE_LEVEL_2_ANGLE); + moveToPosition(ALGAE_UPPER_HEIGHT, ALGAE_UPPER_ANGLE); } /** From e5a63d36749648682ccc6548c0bd677888f9bbfc Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 18:02:15 -0600 Subject: [PATCH 11/16] Intake animation tuning --- src/main/java/frc/robot/AutoCommands.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/AutoCommands.java b/src/main/java/frc/robot/AutoCommands.java index b6f3cd1..0381824 100644 --- a/src/main/java/frc/robot/AutoCommands.java +++ b/src/main/java/frc/robot/AutoCommands.java @@ -209,9 +209,10 @@ private Command autoScoreCoralAuto(Runnable armMethod, List redPoses, Li public Command intakeCoral() { return new IntakeCoralCommand(indexerSubsystem, gamePieceManipulatorSubsystem, armSubsystem).deadlineFor( ledSubsystem.runPatternAsCommand( - LEDPattern.gradient(GradientType.kContinuous, Color.kGray, Color.kWhite) - .scrollAtRelativeSpeed(Percent.per(Second).of(100)) - .breathe(Seconds.of(0.75)))); + LEDPattern.gradient(GradientType.kContinuous, Color.kBlack, Color.kWhite) + .scrollAtRelativeSpeed(Percent.per(Second).of(200)) + .reversed() + .breathe(Seconds.of(0.5)))); } } From 6064892294fdf5eabc409b27956c9a020f40e71f Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 18:02:32 -0600 Subject: [PATCH 12/16] L2 adjustments from the parking lot --- src/main/java/frc/robot/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2d6f3f3..1fecef8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -231,7 +231,7 @@ public static class ArmConstants { .withMotionMagicCruiseVelocity(6.0); public static final Distance ELEVATOR_INTAKE_POSITION = Meters.of(0); - public static final Distance LEVEL_2_HEIGHT = Meters.of(0.1946611669921875); + public static final Distance LEVEL_2_HEIGHT = Meters.of(0.2); public static final Distance LEVEL_3_HEIGHT = Meters.of(0); public static final Distance LEVEL_4_HEIGHT = Meters.of(0.675); public static final Distance ALGAE_LOWER_HEIGHT = Meters.of(0.0); @@ -241,7 +241,7 @@ public static class ArmConstants { public static final Distance ALGAE_HOLD_HEIGHT = Meters.zero(); public static final Angle ARM_INTAKE_ANGLE = Rotations.of(0.765); - public static final Angle LEVEL_2_ANGLE = Rotations.of(0.68212890625); + public static final Angle LEVEL_2_ANGLE = Rotations.of(0.705); public static final Angle LEVEL_3_ANGLE = Rotations.of(0.172); public static final Angle LEVEL_4_ANGLE = Rotations.of(0.16); public static final Angle ALGAE_LOWER_ANGLE = Rotations.of(0.06); From c3d00ec23257a7a564a91770fb2759addd66bde4 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 19:41:44 -0600 Subject: [PATCH 13/16] Better 3 coral auto --- .../deploy/pathplanner/autos/Three Coral.auto | 42 ++++++++- .../deploy/pathplanner/autos/Two Coral.auto | 75 ---------------- .../deploy/pathplanner/paths/One Coral.path | 18 ++-- .../pathplanner/paths/Three Coral A.path | 68 ++++++++++++++ .../pathplanner/paths/Three Coral B.path | 54 +++++++++++ .../deploy/pathplanner/paths/Three Coral.path | 89 ------------------- .../deploy/pathplanner/paths/Two Coral A.path | 68 ++++++++++++++ .../deploy/pathplanner/paths/Two Coral B.path | 54 +++++++++++ .../deploy/pathplanner/paths/Two Coral.path | 89 ------------------- src/main/deploy/pathplanner/settings.json | 12 +-- src/main/java/frc/robot/Constants.java | 2 +- .../java/frc/robot/DrivetrainTelemetry.java | 6 ++ src/main/java/frc/robot/RobotContainer.java | 21 ++++- .../robot/controls/XBoxControlBindings.java | 5 ++ .../frc/robot/generated/TunerConstants.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 9 +- 16 files changed, 334 insertions(+), 280 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Two Coral.auto create mode 100644 src/main/deploy/pathplanner/paths/Three Coral A.path create mode 100644 src/main/deploy/pathplanner/paths/Three Coral B.path delete mode 100644 src/main/deploy/pathplanner/paths/Three Coral.path create mode 100644 src/main/deploy/pathplanner/paths/Two Coral A.path create mode 100644 src/main/deploy/pathplanner/paths/Two Coral B.path delete mode 100644 src/main/deploy/pathplanner/paths/Two Coral.path diff --git a/src/main/deploy/pathplanner/autos/Three Coral.auto b/src/main/deploy/pathplanner/autos/Three Coral.auto index aa9ff7b..d1cb173 100644 --- a/src/main/deploy/pathplanner/autos/Three Coral.auto +++ b/src/main/deploy/pathplanner/autos/Three Coral.auto @@ -42,7 +42,26 @@ { "type": "path", "data": { - "pathName": "Two Coral" + "pathName": "Two Coral A" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Two Coral B" } }, { @@ -73,7 +92,26 @@ { "type": "path", "data": { - "pathName": "Three Coral" + "pathName": "Three Coral A" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral B" } }, { diff --git a/src/main/deploy/pathplanner/autos/Two Coral.auto b/src/main/deploy/pathplanner/autos/Two Coral.auto deleted file mode 100644 index 75178af..0000000 --- a/src/main/deploy/pathplanner/autos/Two Coral.auto +++ /dev/null @@ -1,75 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "One Coral" - } - }, - { - "type": "named", - "data": { - "name": "ledDefault" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "scoreCoralLevel4" - } - }, - { - "type": "named", - "data": { - "name": "parkArm" - } - }, - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Two Coral" - } - }, - { - "type": "named", - "data": { - "name": "intakeCoral" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "scoreCoralLevel4" - } - }, - { - "type": "named", - "data": { - "name": "parkArm" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/One Coral.path b/src/main/deploy/pathplanner/paths/One Coral.path index dc1d3fc..ec6b542 100644 --- a/src/main/deploy/pathplanner/paths/One Coral.path +++ b/src/main/deploy/pathplanner/paths/One Coral.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.36, - "y": 5.27 + "x": 5.4698468543046355, + "y": 5.383381622516556 }, "prevControl": { - "x": 6.021030629139071, - "y": 6.02546357615894 + "x": 6.130877483443706, + "y": 6.138845198675496 }, "nextControl": null, "isLocked": false, @@ -33,10 +33,10 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -179.62607495827103 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral A.path b/src/main/deploy/pathplanner/paths/Three Coral A.path new file mode 100644 index 0000000..4371b44 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral A.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8136382450331126, + "y": 5.579511589403973 + }, + "prevControl": null, + "nextControl": { + "x": 2.979172185430464, + "y": 6.411846026490066 + }, + "isLocked": false, + "linkedName": "CoralTwoEnd" + }, + { + "anchor": { + "x": 1.4746067880794704, + "y": 7.21392798013245 + }, + "prevControl": { + "x": 2.171957781456954, + "y": 6.22601407284768 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CoralThreeEndA" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9506493506493497, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 126.6897228324759 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -150.78075330951535 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral B.path b/src/main/deploy/pathplanner/paths/Three Coral B.path new file mode 100644 index 0000000..e5b01d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral B.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4746067880794704, + "y": 7.21392798013245 + }, + "prevControl": null, + "nextControl": { + "x": 2.7676117549716492, + "y": 5.9136589404007 + }, + "isLocked": false, + "linkedName": "CoralThreeEndA" + }, + { + "anchor": { + "x": 2.93, + "y": 4.47 + }, + "prevControl": { + "x": 1.888658940397351, + "y": 5.586775662251655 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 126.6897228324759 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral.path b/src/main/deploy/pathplanner/paths/Three Coral.path deleted file mode 100644 index ce38429..0000000 --- a/src/main/deploy/pathplanner/paths/Three Coral.path +++ /dev/null @@ -1,89 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 3.915335264900662, - "y": 5.506870860927152 - }, - "prevControl": null, - "nextControl": { - "x": 3.0808692052980136, - "y": 6.339205298013245 - }, - "isLocked": false, - "linkedName": "CoralTwoEnd" - }, - { - "anchor": { - "x": 1.220364238410596, - "y": 7.046854304635761 - }, - "prevControl": { - "x": 2.0266763245033115, - "y": 7.765997516556291 - }, - "nextControl": { - "x": 0.3731207068101823, - "y": 6.2912046683435 - }, - "isLocked": false, - "linkedName": "CoralPickupLeft" - }, - { - "anchor": { - "x": 3.82, - "y": 5.43 - }, - "prevControl": { - "x": 2.931204470198675, - "y": 6.263476821192053 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9565217391304481, - "rotationDegrees": 126.04541757739437 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7854545454545454, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.75, - "maxAcceleration": 8.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -149.8788174325336 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -150.78075330951535 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Two Coral A.path b/src/main/deploy/pathplanner/paths/Two Coral A.path new file mode 100644 index 0000000..5fea998 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Two Coral A.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.4698468543046355, + "y": 5.383381622516556 + }, + "prevControl": null, + "nextControl": { + "x": 4.352065397350993, + "y": 6.971370033112582 + }, + "isLocked": false, + "linkedName": "CoralScoreID20point" + }, + { + "anchor": { + "x": 1.5545115894039734, + "y": 7.3301531456953635 + }, + "prevControl": { + "x": 2.9128932119205295, + "y": 5.986299668874171 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CoralTwoEndA" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9485714285714312, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": 126.6897228324759 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 149.56576424655643 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Two Coral B.path b/src/main/deploy/pathplanner/paths/Two Coral B.path new file mode 100644 index 0000000..83dbfa3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Two Coral B.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.5545115894039734, + "y": 7.3301531456953635 + }, + "prevControl": null, + "nextControl": { + "x": 2.5787458609235197, + "y": 6.429408112579148 + }, + "isLocked": false, + "linkedName": "CoralTwoEndA" + }, + { + "anchor": { + "x": 3.8136382450331126, + "y": 5.579511589403973 + }, + "prevControl": { + "x": 2.7603476821192054, + "y": 6.719971026490066 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "CoralTwoEnd" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0.0, + "rotation": -150.78075330951535 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 126.6897228324759 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Two Coral.path b/src/main/deploy/pathplanner/paths/Two Coral.path deleted file mode 100644 index 782b8c2..0000000 --- a/src/main/deploy/pathplanner/paths/Two Coral.path +++ /dev/null @@ -1,89 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.36, - "y": 5.27 - }, - "prevControl": null, - "nextControl": { - "x": 4.2422185430463575, - "y": 6.857988410596026 - }, - "isLocked": false, - "linkedName": "CoralScoreID20point" - }, - { - "anchor": { - "x": 1.220364238410596, - "y": 7.046854304635761 - }, - "prevControl": { - "x": 2.0266763245033115, - "y": 7.765997516556291 - }, - "nextControl": { - "x": 0.3731207068101823, - "y": 6.2912046683435 - }, - "isLocked": false, - "linkedName": "CoralPickupLeft" - }, - { - "anchor": { - "x": 3.915335264900662, - "y": 5.506870860927152 - }, - "prevControl": { - "x": 1.9800289735099343, - "y": 6.437127483443708 - }, - "nextControl": null, - "isLocked": false, - "linkedName": "CoralTwoEnd" - } - ], - "rotationTargets": [ - { - "waypointRelativePos": 0.9565217391304481, - "rotationDegrees": 126.04541757739437 - } - ], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.7854545454545454, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 0.75, - "maxAcceleration": 8.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 5.0, - "maxAcceleration": 6.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0.0, - "rotation": -150.78075330951535 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 149.56576424655643 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 841a21e..1686d7d 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -4,19 +4,19 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 5.0, - "defaultMaxAccel": 8.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxVel": 4.25, + "defaultMaxAccel": 6.0, + "defaultMaxAngVel": 360.0, + "defaultMaxAngAccel": 540.0, "defaultNominalVoltage": 12.0, "robotMass": 60.8, "robotMOI": 5.0, "robotTrackwidth": 0.546, "driveWheelRadius": 0.052959, "driveGearing": 6.746031746031747, - "maxDriveSpeed": 5.45, + "maxDriveSpeed": 4.367, "driveMotorType": "krakenX60FOC", - "driveCurrentLimit": 80.0, + "driveCurrentLimit": 100.0, "wheelCOF": 1.542, "flModuleX": 0.301625, "flModuleY": 0.302, diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1fecef8..1d0e020 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -155,7 +155,7 @@ public static class ClimbConstants { public static final Current CLIMB_SUPPLY_CURRENT_LIMIT = Amps.of(40); public static final double CLIMB_ROTOR_TO_SENSOR_RATIO = 144; - public static final Angle CLIMB_FORWARD_SOFT_LIMIT = Rotations.of(0.831299); + public static final Angle CLIMB_FORWARD_SOFT_LIMIT = Rotations.of(0.826); public static final Voltage CLIMB_VOLTAGE = Volts.of(6); } diff --git a/src/main/java/frc/robot/DrivetrainTelemetry.java b/src/main/java/frc/robot/DrivetrainTelemetry.java index aba663d..b9ccb37 100644 --- a/src/main/java/frc/robot/DrivetrainTelemetry.java +++ b/src/main/java/frc/robot/DrivetrainTelemetry.java @@ -30,6 +30,10 @@ public class DrivetrainTelemetry { .getStructArrayTopic("Module Targets", SwerveModuleState.struct) .publish(); private final DoublePublisher periodPublisher = driveStats.getDoubleTopic("Period").publish(); + + private final DoublePublisher xSpeedPublisher = driveStats.getDoubleTopic("xSpeed").publish(); + private final DoublePublisher ySpeedPublisher = driveStats.getDoubleTopic("ySpeed").publish(); + private final Field2d field2d = new Field2d(); private final Timer frequencyTimer = new Timer(); @@ -53,6 +57,8 @@ public void telemeterize(SwerveDriveState state) { moduleTargetsPublisher.set(state.ModuleTargets); periodPublisher.accept(state.OdometryPeriod); field2d.setRobotPose(state.Pose); + xSpeedPublisher.set(state.Speeds.vxMetersPerSecond); + ySpeedPublisher.set(state.Speeds.vyMetersPerSecond); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5b75b0b..5f91aa1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,7 +5,10 @@ package frc.robot; import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.wpilibj.util.Color.kBlack; import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kForward; import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kReverse; import static frc.robot.Constants.TeleopDriveConstants.MAX_TELEOP_ANGULAR_VELOCITY; @@ -18,8 +21,12 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.LEDPattern; +import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.AlgaeBargeCommand; @@ -144,7 +151,19 @@ private void configureBindings() { controlBindings.climb() .ifPresent( trigger -> trigger - .whileTrue(climbSubsystem.run(() -> climbSubsystem.climb()).finallyDo(climbSubsystem::stop))); + .whileTrue( + climbSubsystem.run(() -> climbSubsystem.climb()) + .alongWith( + ledSubsystem.runPatternAsCommand( + LEDPattern + .gradient( + GradientType.kContinuous, + kBlack, + DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red ? Color.kRed + : Color.kBlue) + .reversed() + .scrollAtRelativeSpeed(Percent.per(Second).of(100)))) + .finallyDo(climbSubsystem::stop))); drivetrain.registerTelemetry(drivetrainTelemetry::telemeterize); diff --git a/src/main/java/frc/robot/controls/XBoxControlBindings.java b/src/main/java/frc/robot/controls/XBoxControlBindings.java index c77cb61..261bac1 100644 --- a/src/main/java/frc/robot/controls/XBoxControlBindings.java +++ b/src/main/java/frc/robot/controls/XBoxControlBindings.java @@ -83,4 +83,9 @@ public Optional scoreCoralLevel3() { public Optional shootAlgae() { return Optional.of(driverController.leftTrigger()); } + + @Override + public Optional climb() { + return Optional.of(driverController.rightTrigger()); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 74ffa79..aa5f191 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -50,7 +50,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(80.0); + private static final Current kSlipCurrent = Amps.of(100.0); // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index edb4c47..4cb5864 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -21,11 +21,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; -import edu.wpi.first.wpilibj.RobotState; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; -import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import frc.robot.subsystems.sysid.SysIdSwerveTranslationTorque; @@ -136,9 +133,7 @@ public CommandSwerveDrivetrain( startSimThread(); } configureAutoBuilder(); - configNeutralMode(NeutralModeValue.Coast); - new Trigger(RobotState::isEnabled).onTrue(Commands.runOnce(() -> configNeutralMode(NeutralModeValue.Brake))) - .onFalse(Commands.runOnce(() -> configNeutralMode(NeutralModeValue.Coast)).ignoringDisable(true)); + configNeutralMode(NeutralModeValue.Brake); } /** @@ -213,7 +208,7 @@ private void configureAutoBuilder() { // PID constants for translation new PIDConstants(8, 0, 0), // PID constants for rotation - new PIDConstants(4, 0, 0)), + new PIDConstants(2.75, 0, 0)), config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, From f37bf1fd154e3dfb87b8e9136e289472a66197f0 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 20:08:29 -0600 Subject: [PATCH 14/16] Initial reorg and right auto --- .../pathplanner/autos/Odometry Back.auto | 19 --- .../deploy/pathplanner/autos/Odometry.auto | 19 --- .../deploy/pathplanner/autos/One Coral.auto | 44 ------ ...Three Coral.auto => Three Coral Left.auto} | 10 +- .../pathplanner/autos/Three Coral Right.auto | 144 ++++++++++++++++++ .../pathplanner/paths/Odometry Path Back.path | 54 ------- ...One Coral.path => Three Coral Left 1.path} | 4 +- ...o Coral A.path => Three Coral Left 2.path} | 6 +- ...o Coral B.path => Three Coral Left 3.path} | 6 +- ...e Coral A.path => Three Coral Left 4.path} | 6 +- ...e Coral B.path => Three Coral Left 5.path} | 4 +- ...try Path.path => Three Coral Right 1.path} | 32 ++-- .../paths/Three Coral Right 2.path | 68 +++++++++ .../paths/Three Coral Right 3.path | 54 +++++++ .../paths/Three Coral Right 4.path | 68 +++++++++ .../paths/Three Coral Right 5.path | 54 +++++++ src/main/deploy/pathplanner/settings.json | 5 +- 17 files changed, 426 insertions(+), 171 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/Odometry Back.auto delete mode 100644 src/main/deploy/pathplanner/autos/Odometry.auto delete mode 100644 src/main/deploy/pathplanner/autos/One Coral.auto rename src/main/deploy/pathplanner/autos/{Three Coral.auto => Three Coral Left.auto} (91%) create mode 100644 src/main/deploy/pathplanner/autos/Three Coral Right.auto delete mode 100644 src/main/deploy/pathplanner/paths/Odometry Path Back.path rename src/main/deploy/pathplanner/paths/{One Coral.path => Three Coral Left 1.path} (93%) rename src/main/deploy/pathplanner/paths/{Two Coral A.path => Three Coral Left 2.path} (92%) rename src/main/deploy/pathplanner/paths/{Two Coral B.path => Three Coral Left 3.path} (90%) rename src/main/deploy/pathplanner/paths/{Three Coral A.path => Three Coral Left 4.path} (92%) rename src/main/deploy/pathplanner/paths/{Three Coral B.path => Three Coral Left 5.path} (93%) rename src/main/deploy/pathplanner/paths/{Odometry Path.path => Three Coral Right 1.path} (54%) create mode 100644 src/main/deploy/pathplanner/paths/Three Coral Right 2.path create mode 100644 src/main/deploy/pathplanner/paths/Three Coral Right 3.path create mode 100644 src/main/deploy/pathplanner/paths/Three Coral Right 4.path create mode 100644 src/main/deploy/pathplanner/paths/Three Coral Right 5.path diff --git a/src/main/deploy/pathplanner/autos/Odometry Back.auto b/src/main/deploy/pathplanner/autos/Odometry Back.auto deleted file mode 100644 index 18874f9..0000000 --- a/src/main/deploy/pathplanner/autos/Odometry Back.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Odometry Path Back" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Odometry.auto b/src/main/deploy/pathplanner/autos/Odometry.auto deleted file mode 100644 index ec8633f..0000000 --- a/src/main/deploy/pathplanner/autos/Odometry.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Odometry Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/One Coral.auto b/src/main/deploy/pathplanner/autos/One Coral.auto deleted file mode 100644 index 584848a..0000000 --- a/src/main/deploy/pathplanner/autos/One Coral.auto +++ /dev/null @@ -1,44 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "deadline", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "One Coral" - } - }, - { - "type": "named", - "data": { - "name": "ledDefault" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "scoreCoralLevel4" - } - }, - { - "type": "named", - "data": { - "name": "parkArm" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Three Coral.auto b/src/main/deploy/pathplanner/autos/Three Coral Left.auto similarity index 91% rename from src/main/deploy/pathplanner/autos/Three Coral.auto rename to src/main/deploy/pathplanner/autos/Three Coral Left.auto index d1cb173..690658f 100644 --- a/src/main/deploy/pathplanner/autos/Three Coral.auto +++ b/src/main/deploy/pathplanner/autos/Three Coral Left.auto @@ -11,7 +11,7 @@ { "type": "path", "data": { - "pathName": "One Coral" + "pathName": "Three Coral Left 1" } }, { @@ -42,7 +42,7 @@ { "type": "path", "data": { - "pathName": "Two Coral A" + "pathName": "Three Coral Left 2" } }, { @@ -61,7 +61,7 @@ { "type": "path", "data": { - "pathName": "Two Coral B" + "pathName": "Three Coral Left 3" } }, { @@ -92,7 +92,7 @@ { "type": "path", "data": { - "pathName": "Three Coral A" + "pathName": "Three Coral Left 4" } }, { @@ -111,7 +111,7 @@ { "type": "path", "data": { - "pathName": "Three Coral B" + "pathName": "Three Coral Left 5" } }, { diff --git a/src/main/deploy/pathplanner/autos/Three Coral Right.auto b/src/main/deploy/pathplanner/autos/Three Coral Right.auto new file mode 100644 index 0000000..120e318 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Three Coral Right.auto @@ -0,0 +1,144 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral Right 1" + } + }, + { + "type": "named", + "data": { + "name": "ledDefault" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCoralLevel4" + } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral Right 2" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral Right 3" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCoralLevel4" + } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral Right 4" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Three Coral Right 5" + } + }, + { + "type": "named", + "data": { + "name": "intakeCoral" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "scoreCoralLevel4" + } + }, + { + "type": "named", + "data": { + "name": "parkArm" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Odometry Path Back.path b/src/main/deploy/pathplanner/paths/Odometry Path Back.path deleted file mode 100644 index a6020ad..0000000 --- a/src/main/deploy/pathplanner/paths/Odometry Path Back.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 5.0, - "y": 0.0 - }, - "prevControl": null, - "nextControl": { - "x": 4.0, - "y": 1.2246467991473532e-16 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 0.0, - "y": 0.0 - }, - "prevControl": { - "x": 1.0, - "y": -1.2246467991473532e-16 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/One Coral.path b/src/main/deploy/pathplanner/paths/Three Coral Left 1.path similarity index 93% rename from src/main/deploy/pathplanner/paths/One Coral.path rename to src/main/deploy/pathplanner/paths/Three Coral Left 1.path index ec6b542..91125dd 100644 --- a/src/main/deploy/pathplanner/paths/One Coral.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Left 1.path @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "CoralScoreID20point" + "linkedName": "ThreeCoralLeftScore1" } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": 149.56576424655643 }, "reversed": false, - "folder": null, + "folder": "Three Coral Left", "idealStartingState": { "velocity": 0, "rotation": -179.62607495827103 diff --git a/src/main/deploy/pathplanner/paths/Two Coral A.path b/src/main/deploy/pathplanner/paths/Three Coral Left 2.path similarity index 92% rename from src/main/deploy/pathplanner/paths/Two Coral A.path rename to src/main/deploy/pathplanner/paths/Three Coral Left 2.path index 5fea998..2446faa 100644 --- a/src/main/deploy/pathplanner/paths/Two Coral A.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Left 2.path @@ -12,7 +12,7 @@ "y": 6.971370033112582 }, "isLocked": false, - "linkedName": "CoralScoreID20point" + "linkedName": "ThreeCoralLeftScore1" }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "CoralTwoEndA" + "linkedName": "ThreeCoralLeftPickup1" } ], "rotationTargets": [], @@ -59,7 +59,7 @@ "rotation": 126.6897228324759 }, "reversed": false, - "folder": null, + "folder": "Three Coral Left", "idealStartingState": { "velocity": 0, "rotation": 149.56576424655643 diff --git a/src/main/deploy/pathplanner/paths/Two Coral B.path b/src/main/deploy/pathplanner/paths/Three Coral Left 3.path similarity index 90% rename from src/main/deploy/pathplanner/paths/Two Coral B.path rename to src/main/deploy/pathplanner/paths/Three Coral Left 3.path index 83dbfa3..ec365b6 100644 --- a/src/main/deploy/pathplanner/paths/Two Coral B.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Left 3.path @@ -12,7 +12,7 @@ "y": 6.429408112579148 }, "isLocked": false, - "linkedName": "CoralTwoEndA" + "linkedName": "ThreeCoralLeftPickup1" }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "CoralTwoEnd" + "linkedName": "ThreeCoralLeftScore2" } ], "rotationTargets": [], @@ -45,7 +45,7 @@ "rotation": -150.78075330951535 }, "reversed": false, - "folder": null, + "folder": "Three Coral Left", "idealStartingState": { "velocity": 0, "rotation": 126.6897228324759 diff --git a/src/main/deploy/pathplanner/paths/Three Coral A.path b/src/main/deploy/pathplanner/paths/Three Coral Left 4.path similarity index 92% rename from src/main/deploy/pathplanner/paths/Three Coral A.path rename to src/main/deploy/pathplanner/paths/Three Coral Left 4.path index 4371b44..87a5d60 100644 --- a/src/main/deploy/pathplanner/paths/Three Coral A.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Left 4.path @@ -12,7 +12,7 @@ "y": 6.411846026490066 }, "isLocked": false, - "linkedName": "CoralTwoEnd" + "linkedName": "ThreeCoralLeftScore2" }, { "anchor": { @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": "CoralThreeEndA" + "linkedName": "ThreeCoralLeftPickup2" } ], "rotationTargets": [], @@ -59,7 +59,7 @@ "rotation": 126.6897228324759 }, "reversed": false, - "folder": null, + "folder": "Three Coral Left", "idealStartingState": { "velocity": 0, "rotation": -150.78075330951535 diff --git a/src/main/deploy/pathplanner/paths/Three Coral B.path b/src/main/deploy/pathplanner/paths/Three Coral Left 5.path similarity index 93% rename from src/main/deploy/pathplanner/paths/Three Coral B.path rename to src/main/deploy/pathplanner/paths/Three Coral Left 5.path index e5b01d9..41b8d69 100644 --- a/src/main/deploy/pathplanner/paths/Three Coral B.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Left 5.path @@ -12,7 +12,7 @@ "y": 5.9136589404007 }, "isLocked": false, - "linkedName": "CoralThreeEndA" + "linkedName": "ThreeCoralLeftPickup2" }, { "anchor": { @@ -45,7 +45,7 @@ "rotation": -90.0 }, "reversed": false, - "folder": null, + "folder": "Three Coral Left", "idealStartingState": { "velocity": 0, "rotation": 126.6897228324759 diff --git a/src/main/deploy/pathplanner/paths/Odometry Path.path b/src/main/deploy/pathplanner/paths/Three Coral Right 1.path similarity index 54% rename from src/main/deploy/pathplanner/paths/Odometry Path.path rename to src/main/deploy/pathplanner/paths/Three Coral Right 1.path index c19c88e..2325fd9 100644 --- a/src/main/deploy/pathplanner/paths/Odometry Path.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 1.path @@ -3,29 +3,29 @@ "waypoints": [ { "anchor": { - "x": 0.0, - "y": 0.0 + "x": 7.075206953642384, + "y": 0.48739652317880655 }, "prevControl": null, "nextControl": { - "x": 1.0, - "y": 0.0 + "x": 6.232574503311258, + "y": 1.3736134105960276 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.0, - "y": 0.0 + "x": 4.874192880794702, + "y": 2.3760554635761593 }, "prevControl": { - "x": 4.0, - "y": 0.0 + "x": 5.535223509933775, + "y": 1.6569122516556298 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "ThreeCoralRightScore1" } ], "rotationTargets": [], @@ -33,22 +33,22 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 2.0, - "maxAcceleration": 1.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": 29.859016164923315 }, "reversed": false, - "folder": null, + "folder": "Three Coral Right", "idealStartingState": { "velocity": 0, "rotation": 0.0 }, - "useDefaultConstraints": false + "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 2.path b/src/main/deploy/pathplanner/paths/Three Coral Right 2.path new file mode 100644 index 0000000..13c60fd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.874192880794702, + "y": 2.3760554635761593 + }, + "prevControl": null, + "nextControl": { + "x": 3.3051531456953636, + "y": 1.2937086092715224 + }, + "isLocked": false, + "linkedName": "ThreeCoralRightScore1" + }, + { + "anchor": { + "x": 1.4600786423841061, + "y": 0.8360720198675483 + }, + "prevControl": { + "x": 2.622330298013245, + "y": 1.126634933774834 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ThreeCoralRightPickup1" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9589610389610387, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -126.50144112050634 + }, + "reversed": false, + "folder": "Three Coral Right", + "idealStartingState": { + "velocity": 0, + "rotation": 29.859016164923315 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 3.path b/src/main/deploy/pathplanner/paths/Three Coral Right 3.path new file mode 100644 index 0000000..495d903 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.4600786423841061, + "y": 0.8360720198675483 + }, + "prevControl": null, + "nextControl": { + "x": 2.0189158484159204, + "y": 1.3535339349463904 + }, + "isLocked": false, + "linkedName": "ThreeCoralRightPickup1" + }, + { + "anchor": { + "x": 3.5521316225165562, + "y": 2.710202814569535 + }, + "prevControl": { + "x": 2.9327383790052464, + "y": 1.926100058626133 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ThreeCoralRightScore2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -28.474203610074287 + }, + "reversed": false, + "folder": "Three Coral Right", + "idealStartingState": { + "velocity": 0, + "rotation": -126.50144112050634 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 4.path b/src/main/deploy/pathplanner/paths/Three Coral Right 4.path new file mode 100644 index 0000000..444435f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 4.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5521316225165562, + "y": 2.710202814569535 + }, + "prevControl": null, + "nextControl": { + "x": 2.971005794701987, + "y": 2.0273799668874175 + }, + "isLocked": false, + "linkedName": "ThreeCoralRightScore2" + }, + { + "anchor": { + "x": 1.5472475165562916, + "y": 0.734375 + }, + "prevControl": { + "x": 2.1283733443708606, + "y": 1.3372930463576158 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ThreeCoralRightPickup2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9412987012986995, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -125.34010692155768 + }, + "reversed": false, + "folder": "Three Coral Right", + "idealStartingState": { + "velocity": 0, + "rotation": -28.474203610074287 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 5.path b/src/main/deploy/pathplanner/paths/Three Coral Right 5.path new file mode 100644 index 0000000..c763c8b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 5.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.5472475165562916, + "y": 0.734375 + }, + "prevControl": null, + "nextControl": { + "x": 1.9903559602649006, + "y": 1.8457781456953635 + }, + "isLocked": false, + "linkedName": "ThreeCoralRightPickup2" + }, + { + "anchor": { + "x": 2.869308774834437, + "y": 4.1630173841059595 + }, + "prevControl": { + "x": 2.622330298013245, + "y": 3.48019453642384 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.25, + "maxAcceleration": 6.0, + "maxAngularVelocity": 360.0, + "maxAngularAcceleration": 540.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -90.0 + }, + "reversed": false, + "folder": "Three Coral Right", + "idealStartingState": { + "velocity": 0, + "rotation": -125.34010692155768 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 1686d7d..79f4c4f 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,7 +2,10 @@ "robotWidth": 0.922338, "robotLength": 0.922338, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Three Coral Right", + "Three Coral Left" + ], "autoFolders": [], "defaultMaxVel": 4.25, "defaultMaxAccel": 6.0, From 0218dd83b37459b5b241194a5ba04a3622d0f6f4 Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 20:55:15 -0600 Subject: [PATCH 15/16] Working 3 coral right auto --- build.gradle | 2 +- .../paths/Three Coral Right 2.path | 19 +++++++++++------- .../paths/Three Coral Right 3.path | 8 ++++---- .../paths/Three Coral Right 4.path | 17 ++++++++++------ .../paths/Three Coral Right 5.path | 20 +++++++++---------- .../robot/commands/AlignToReefCommand.java | 4 ++-- .../subsystems/CommandSwerveDrivetrain.java | 2 +- 7 files changed, 41 insertions(+), 31 deletions(-) diff --git a/build.gradle b/build.gradle index c9b6e35..c018a43 100644 --- a/build.gradle +++ b/build.gradle @@ -34,7 +34,7 @@ deploy { frcStaticFileDeploy(getArtifactTypeClass('FileTreeArtifact')) { files = project.fileTree('src/main/deploy') directory = '/home/lvuser/deploy' - deleteOldFiles = false // Change to true to delete files on roboRIO that no + deleteOldFiles = true // Change to true to delete files on roboRIO that no // longer exist in deploy directory of this project } } diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 2.path b/src/main/deploy/pathplanner/paths/Three Coral Right 2.path index 13c60fd..93ac526 100644 --- a/src/main/deploy/pathplanner/paths/Three Coral Right 2.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 2.path @@ -8,27 +8,32 @@ }, "prevControl": null, "nextControl": { - "x": 3.3051531456953636, - "y": 1.2937086092715224 + "x": 3.174399834437086, + "y": 1.6423841059602635 }, "isLocked": false, "linkedName": "ThreeCoralRightScore1" }, { "anchor": { - "x": 1.4600786423841061, - "y": 0.8360720198675483 + "x": 1.4746067880794704, + "y": 0.7997516556291384 }, "prevControl": { - "x": 2.622330298013245, - "y": 1.126634933774834 + "x": 2.651386589403974, + "y": 1.3518211920529803 }, "nextControl": null, "isLocked": false, "linkedName": "ThreeCoralRightPickup1" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.9084181313598653, + "rotationDegrees": -125.73386147909376 + } + ], "constraintZones": [ { "name": "Constraints Zone", diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 3.path b/src/main/deploy/pathplanner/paths/Three Coral Right 3.path index 495d903..13b202f 100644 --- a/src/main/deploy/pathplanner/paths/Three Coral Right 3.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.4600786423841061, - "y": 0.8360720198675483 + "x": 1.4746067880794704, + "y": 0.7997516556291384 }, "prevControl": null, "nextControl": { - "x": 2.0189158484159204, - "y": 1.3535339349463904 + "x": 2.033443994111285, + "y": 1.3172135707079806 }, "isLocked": false, "linkedName": "ThreeCoralRightPickup1" diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 4.path b/src/main/deploy/pathplanner/paths/Three Coral Right 4.path index 444435f..94ab1d6 100644 --- a/src/main/deploy/pathplanner/paths/Three Coral Right 4.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 4.path @@ -16,23 +16,28 @@ }, { "anchor": { - "x": 1.5472475165562916, - "y": 0.734375 + "x": 1.467342715231788, + "y": 0.603621688741722 }, "prevControl": { - "x": 2.1283733443708606, - "y": 1.3372930463576158 + "x": 2.048468543046357, + "y": 1.2065397350993374 }, "nextControl": null, "isLocked": false, "linkedName": "ThreeCoralRightPickup2" } ], - "rotationTargets": [], + "rotationTargets": [ + { + "waypointRelativePos": 0.9056429232192469, + "rotationDegrees": -127.73848597026526 + } + ], "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.9412987012986995, + "minWaypointRelativePos": 0.9090909090909117, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 0.5, diff --git a/src/main/deploy/pathplanner/paths/Three Coral Right 5.path b/src/main/deploy/pathplanner/paths/Three Coral Right 5.path index c763c8b..01add35 100644 --- a/src/main/deploy/pathplanner/paths/Three Coral Right 5.path +++ b/src/main/deploy/pathplanner/paths/Three Coral Right 5.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.5472475165562916, - "y": 0.734375 + "x": 1.467342715231788, + "y": 0.603621688741722 }, "prevControl": null, "nextControl": { - "x": 1.9903559602649006, - "y": 1.8457781456953635 + "x": 1.9099209807914606, + "y": 1.591053213543426 }, "isLocked": false, "linkedName": "ThreeCoralRightPickup2" }, { "anchor": { - "x": 2.869308774834437, - "y": 4.1630173841059595 + "x": 2.8547806291390727, + "y": 3.988679635761589 }, "prevControl": { - "x": 2.622330298013245, - "y": 3.48019453642384 + "x": 2.520633278145695, + "y": 3.2840645695364232 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.25, + "maxVelocity": 3.75, "maxAcceleration": 6.0, "maxAngularVelocity": 360.0, "maxAngularAcceleration": 540.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -125.34010692155768 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/AlignToReefCommand.java b/src/main/java/frc/robot/commands/AlignToReefCommand.java index 9cb0caf..d90ab03 100644 --- a/src/main/java/frc/robot/commands/AlignToReefCommand.java +++ b/src/main/java/frc/robot/commands/AlignToReefCommand.java @@ -47,9 +47,9 @@ public class AlignToReefCommand extends Command { private static final double RIGHT_LATERAL_TARGET = 0.08; private static final Distance DISTANCE_TOLERANCE = Inches.of(0.5); - private static final Distance LATERAL_TOLERANCE = Inch.of(0.6); + private static final Distance LATERAL_TOLERANCE = Inch.of(1.0); // Theta is the difference between the two CANrange distances, in meters - private static final double THETA_TOLERANCE = 0.017; + private static final double THETA_TOLERANCE = 0.03; private final CommandSwerveDrivetrain drivetrain; private final AlignmentSubsystem alignmentSubsystem; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 4cb5864..411641f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -208,7 +208,7 @@ private void configureAutoBuilder() { // PID constants for translation new PIDConstants(8, 0, 0), // PID constants for rotation - new PIDConstants(2.75, 0, 0)), + new PIDConstants(2.5, 0, 0)), config, // Assume the path needs to be flipped for Red vs Blue, this is normally the case () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, From 83a322cfec5efada561a75307140a1259dd7d0db Mon Sep 17 00:00:00 2001 From: Andrew Gasser Date: Tue, 25 Feb 2025 21:41:31 -0600 Subject: [PATCH 16/16] Revert climb LEDs (comitted by mistake) --- src/main/java/frc/robot/RobotContainer.java | 21 +-------------------- 1 file changed, 1 insertion(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5f91aa1..5b75b0b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,10 +5,7 @@ package frc.robot; import static edu.wpi.first.units.Units.Meters; -import static edu.wpi.first.units.Units.Percent; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.Second; -import static edu.wpi.first.wpilibj.util.Color.kBlack; import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kForward; import static edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction.kReverse; import static frc.robot.Constants.TeleopDriveConstants.MAX_TELEOP_ANGULAR_VELOCITY; @@ -21,12 +18,8 @@ import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; -import edu.wpi.first.wpilibj.LEDPattern; -import edu.wpi.first.wpilibj.LEDPattern.GradientType; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.commands.AlgaeBargeCommand; @@ -151,19 +144,7 @@ private void configureBindings() { controlBindings.climb() .ifPresent( trigger -> trigger - .whileTrue( - climbSubsystem.run(() -> climbSubsystem.climb()) - .alongWith( - ledSubsystem.runPatternAsCommand( - LEDPattern - .gradient( - GradientType.kContinuous, - kBlack, - DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red ? Color.kRed - : Color.kBlue) - .reversed() - .scrollAtRelativeSpeed(Percent.per(Second).of(100)))) - .finallyDo(climbSubsystem::stop))); + .whileTrue(climbSubsystem.run(() -> climbSubsystem.climb()).finallyDo(climbSubsystem::stop))); drivetrain.registerTelemetry(drivetrainTelemetry::telemeterize);