diff --git a/src/main/java/frc/trigon/robot/RobotContainer.java b/src/main/java/frc/trigon/robot/RobotContainer.java index 37fda07c..98975d77 100644 --- a/src/main/java/frc/trigon/robot/RobotContainer.java +++ b/src/main/java/frc/trigon/robot/RobotContainer.java @@ -20,21 +20,17 @@ import frc.trigon.robot.misc.simulatedfield.SimulatedGamePieceConstants; import frc.trigon.robot.poseestimation.poseestimator.PoseEstimator; import frc.trigon.robot.subsystems.MotorSubsystem; - -import frc.trigon.robot.subsystems.elevator.Elevator; -import frc.trigon.robot.subsystems.elevator.ElevatorCommands; -import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.arm.Arm; import frc.trigon.robot.subsystems.arm.ArmCommands; -import frc.trigon.robot.subsystems.arm.ArmConstants; - import frc.trigon.robot.subsystems.climber.Climber; import frc.trigon.robot.subsystems.climber.ClimberCommands; import frc.trigon.robot.subsystems.climber.ClimberConstants; +import frc.trigon.robot.subsystems.elevator.Elevator; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.intake.Intake; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; - import frc.trigon.robot.subsystems.swerve.Swerve; import frc.trigon.robot.subsystems.transporter.Transporter; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -79,7 +75,7 @@ private void configureBindings() { private void bindDefaultCommands() { SWERVE.setDefaultCommand(GeneralCommands.getFieldRelativeDriveCommand()); - ARM.setDefaultCommand(ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.REST)); + ARM.setDefaultCommand(ArmCommands.getRestCommand()); CLIMBER.setDefaultCommand(ClimberCommands.getSetTargetStateCommand(ClimberConstants.ClimberState.REST)); ELEVATOR.setDefaultCommand(ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.REST)); INTAKE.setDefaultCommand(IntakeCommands.getSetTargetStateCommand(IntakeConstants.IntakeState.REST)); diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java index e3786616..ce9fc10d 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralCollectionCommands.java @@ -8,6 +8,10 @@ import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; import frc.trigon.robot.constants.LEDConstants; import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; import frc.trigon.robot.subsystems.intake.IntakeCommands; import frc.trigon.robot.subsystems.intake.IntakeConstants; import frc.trigon.robot.subsystems.transporter.TransporterCommands; @@ -15,6 +19,13 @@ import lib.hardware.misc.leds.LEDCommands; public class CoralCollectionCommands { + public static Command getLoadCoralCommand() { + return new ParallelCommandGroup( + ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.LOAD_CORAL), + ElevatorCommands.getSetTargetStateCommand(ElevatorConstants.ElevatorState.LOAD_CORAL) + ).until(RobotContainer.ARM::hasGamePiece); + } + public static Command getCoralCollectionCommand() { return new ParallelCommandGroup( getIntakeSequenceCommand(), diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java index cb52f00d..6ca495b4 100644 --- a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralEjectionCommands.java @@ -29,7 +29,7 @@ private static Command getEjectCoralFromIntakeCommand() { private static Command getEjectCoralFromArmCommand() { return new SequentialCommandGroup( - ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atTargetAngle), + ArmCommands.getPrepareForStateCommand(ArmConstants.ArmState.EJECT).until(RobotContainer.ARM::atPrepareAngle), ArmCommands.getSetTargetStateCommand(ArmConstants.ArmState.EJECT) ); } diff --git a/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java new file mode 100644 index 00000000..a366475f --- /dev/null +++ b/src/main/java/frc/trigon/robot/commands/commandfactories/CoralPlacingCommands.java @@ -0,0 +1,203 @@ +package frc.trigon.robot.commands.commandfactories; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.trigon.robot.RobotContainer; +import frc.trigon.robot.constants.AutonomousConstants; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.misc.ReefChooser; +import frc.trigon.robot.subsystems.arm.ArmCommands; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorCommands; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.swerve.SwerveCommands; +import lib.utilities.flippable.FlippablePose2d; +import lib.utilities.flippable.FlippableTranslation2d; + +public class CoralPlacingCommands { + public static boolean SHOULD_SCORE_AUTONOMOUSLY = true; + private static final ReefChooser REEF_CHOOSER = OperatorConstants.REEF_CHOOSER; + + public static Command getScoreInReefCommand(boolean shouldScoreRight) { + return new ConditionalCommand( + getAutonomouslyScoreCommand(shouldScoreRight), + getScoreCommand(shouldScoreRight), + () -> SHOULD_SCORE_AUTONOMOUSLY + ); + } + + private static Command getAutonomouslyScoreCommand(boolean shouldScoreRight) { + return new SequentialCommandGroup( + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(() -> isArmAndElevatorAtPrepareState(shouldScoreRight)), + new ParallelCommandGroup( + ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), + ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) + ) + ); + } + + private static Command getScoreCommand(boolean shouldScoreRight) { + return new SequentialCommandGroup( + getAutonomouslyPrepareScoreCommand(shouldScoreRight).until(OperatorConstants.CONTINUE_TRIGGER), + new ParallelCommandGroup( + ElevatorCommands.getSetTargetStateCommand(REEF_CHOOSER::getElevatorState), + ArmCommands.getSetTargetStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore) + ) + ); + } + + private static Command getAutonomouslyPrepareScoreCommand(boolean shouldScoreRight) { + return new ParallelCommandGroup( + ElevatorCommands.getPrepareStateCommand(REEF_CHOOSER::getElevatorState), + ArmCommands.getPrepareForStateCommand(REEF_CHOOSER::getArmState, CoralPlacingCommands::shouldReverseScore), + getAutonomousDriveToReefThenManualDriveCommand(shouldScoreRight).asProxy() + ); + } + + private static Command getAutonomousDriveToReefThenManualDriveCommand(boolean shouldScoreRight) { + return new SequentialCommandGroup( + SwerveCommands.getDriveToPoseCommand( + () -> CoralPlacingCommands.calculateClosestScoringPose(shouldScoreRight), + AutonomousConstants.DRIVE_TO_REEF_CONSTRAINTS + ), + GeneralCommands.getFieldRelativeDriveCommand() + ); + } + + private static double calculateDistanceToTargetScoringPose(boolean shouldScoreRight) { + final Translation2d currentTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d targetTranslation = calculateClosestScoringPose(shouldScoreRight).get().getTranslation(); + return currentTranslation.getDistance(targetTranslation); + } + + public static FlippablePose2d calculateClosestScoringPose(boolean shouldScoreRight) { + final Translation2d robotPositionOnField = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d reefCenterPosition = new FlippableTranslation2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, true).get(); + final Rotation2d[] reefClockAngles = FieldConstants.REEF_CLOCK_ANGLES; + final Transform2d reefCenterToScoringPose = new Transform2d(FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS, 0, new Rotation2d()); + + double distanceFromClosestScoringPoseMeters = Double.POSITIVE_INFINITY; + Pose2d closestScoringPose = new Pose2d(); + for (final Rotation2d targetRotation : reefClockAngles) { + final Pose2d reefCenterAtTargetRotation = new Pose2d(reefCenterPosition, targetRotation); + + + final Pose2d currentScoringPose = reefCenterAtTargetRotation.transformBy(reefCenterToScoringPose); + final double distanceFromCurrentScoringPoseMeters = currentScoringPose.getTranslation().getDistance(robotPositionOnField); + if (distanceFromCurrentScoringPoseMeters < distanceFromClosestScoringPoseMeters) { + distanceFromClosestScoringPoseMeters = distanceFromCurrentScoringPoseMeters; + closestScoringPose = currentScoringPose; + } + } + + final Transform2d scoringPoseToBranch = new Transform2d( + 0, + shouldScoreRight ? FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS : -FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS, + shouldReverseScore() ? Rotation2d.k180deg : new Rotation2d() + ); + + return new FlippablePose2d(closestScoringPose.transformBy(scoringPoseToBranch), false); + } + + private static boolean isArmAndElevatorAtPrepareState(boolean shouldScoreRight) { + return RobotContainer.ELEVATOR.atPreparedTargetState() + && RobotContainer.ARM.atPrepareAngle() + && RobotContainer.SWERVE.atPose(calculateClosestScoringPose(shouldScoreRight)); + } + + private static boolean shouldReverseScore() { + final Rotation2d robotRotation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getRotation(); + final Translation2d robotTranslation = RobotContainer.ROBOT_POSE_ESTIMATOR.getEstimatedRobotPose().getTranslation(); + final Translation2d reefCenterTranslation = FieldConstants.FLIPPABLE_REEF_CENTER_TRANSLATION.get(); + final Translation2d difference = reefCenterTranslation.minus(robotTranslation); + final Rotation2d robotRotationRelativeToReef = difference.getAngle(); + final Rotation2d robotRotationFacingReef = robotRotation.minus(robotRotationRelativeToReef); + return robotRotationFacingReef.getDegrees() > Rotation2d.kCW_90deg.getDegrees() && robotRotationFacingReef.getDegrees() < Rotation2d.kCCW_90deg.getDegrees(); + } + + /** + * An enum that represents the different levels of scoring in the reef. + * Each level has a different x and y transform from the reef center, + * as well as a different elevator and gripper state. + * The x and y transform are used to calculate the target placing position from the middle of the reef. + */ + public enum ScoringLevel { + L1(Double.NaN, Double.NaN, null), + L2(FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS, FieldConstants.REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS, Rotation2d.fromDegrees(0)), + L3(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)), + L4(L2.xTransformMeters, L2.positiveYTransformMeters, Rotation2d.fromDegrees(0)); + + public final ElevatorConstants.ElevatorState elevatorState; + public final ArmConstants.ArmState armState; + public final int level = calculateLevel(); + final double xTransformMeters, positiveYTransformMeters; + final Rotation2d rotationTransform; + + /** + * Constructs a scoring level with the given x and y transform. + * The elevator and gripper state are determined automatically based on the scoring level. + * + * @param xTransformMeters the x transform from the middle of the reef to the target placing position + * @param positiveYTransformMeters the y transform from the middle of the reef to the target placing position. + * This must be positive (to account for flipping later on), and might be flipped depending on operator input (left or right reef side) + * @param rotationTransform the angle to be facing the reef with the robot. Might change when scoring from the coral intake + */ + ScoringLevel(double xTransformMeters, double positiveYTransformMeters, Rotation2d rotationTransform) { + this.xTransformMeters = xTransformMeters; + this.positiveYTransformMeters = positiveYTransformMeters; + this.rotationTransform = rotationTransform; + this.elevatorState = determineElevatorState(); + this.armState = determineArmState(); + } + + /** + * Calculates the target placing position using the clock position and the target reef side. + * The reef side transform will be flipped depending on operator input. + * To make it more intuitive for the operator to input the reef side, + * left will always correspond to the physical left side in the driver station, + * as opposed to "reef relative" left. + * + * @param reefClockPosition the desired clock position of the reef + * @param reefSide the desired side of the reef, left or right (as seen from the driver station) + * @return the target placing position + */ + public FlippablePose2d calculateTargetPlacingPosition(FieldConstants.ReefClockPosition reefClockPosition, FieldConstants.ReefSide reefSide) { + final Pose2d reefCenterPose = new Pose2d(FieldConstants.BLUE_REEF_CENTER_TRANSLATION, reefClockPosition.clockAngle); + final double yTransform = reefSide.shouldFlipYTransform(reefClockPosition) ? -positiveYTransformMeters : positiveYTransformMeters; + final Transform2d transform = new Transform2d(xTransformMeters, yTransform, rotationTransform); + + return new FlippablePose2d(reefCenterPose.plus(transform), true); + } + + private ElevatorConstants.ElevatorState determineElevatorState() { + return switch (level) { + case 1 -> ElevatorConstants.ElevatorState.SCORE_L1; + case 2 -> ElevatorConstants.ElevatorState.SCORE_L2; + case 3 -> ElevatorConstants.ElevatorState.SCORE_L3; + case 4 -> ElevatorConstants.ElevatorState.SCORE_L4; + default -> throw new IllegalStateException("Unexpected value: " + ordinal()); + }; + } + + private ArmConstants.ArmState determineArmState() { + return switch (level) { + case 1 -> ArmConstants.ArmState.SCORE_L1; + case 2 -> ArmConstants.ArmState.SCORE_L2; + case 3 -> ArmConstants.ArmState.SCORE_L3; + case 4 -> ArmConstants.ArmState.SCORE_L4; + default -> throw new IllegalStateException("Unexpected value: " + ordinal()); + }; + } + + private int calculateLevel() { + return ordinal() + 1; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java new file mode 100644 index 00000000..996d9ff2 --- /dev/null +++ b/src/main/java/frc/trigon/robot/constants/AutonomousConstants.java @@ -0,0 +1,105 @@ +package frc.trigon.robot.constants; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathfindingCommand; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.pathfinding.Pathfinding; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import frc.trigon.robot.RobotContainer; +import lib.hardware.RobotHardwareStats; +import lib.utilities.LocalADStarAK; +import lib.utilities.flippable.Flippable; +import lib.utilities.flippable.FlippablePose2d; +import org.json.simple.parser.ParseException; + +import java.io.IOException; + +/** + * A class that contains the constants and configurations for everything related to the 15-second autonomous period at the start of the match. + */ +public class AutonomousConstants { + public static final RobotConfig ROBOT_CONFIG = getRobotConfig(); + public static final PathConstraints DRIVE_TO_REEF_CONSTRAINTS = new PathConstraints(2.5, 4, Units.degreesToRadians(450), Units.degreesToRadians(900)); + public static final double FEEDFORWARD_SCALAR = 0.5;//TODO: Calibrate + public static final double MINIMUM_DISTANCE_FROM_REEF_TO_OPEN_ELEVATOR = 2.2; + public static final double + REEF_RELATIVE_X_TOLERANCE_METERS = 0.085, + REEF_RELATIVE_Y_TOLERANCE_METERS = 0.03; + + public static final String DEFAULT_AUTO_NAME = "DefaultAutoName"; + + private static final double + AUTO_FIND_CORAL_POSE_X = 3.3, + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; + private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + public static final FlippablePose2d + AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + public static final double + AUTO_FIND_CORAL_END_VELOCITY_METERS_PER_SECOND = 2.5, + AUTO_FIND_CORAL_ROTATION_POWER = 0.2; + + private static final PIDConstants + AUTO_TRANSLATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(0, 0, 0) : + new PIDConstants(0, 0, 0), + AUTO_ROTATION_PID_CONSTANTS = RobotHardwareStats.isSimulation() ? + new PIDConstants(0, 0, 0) : + new PIDConstants(0, 0, 0); + + + public static final PIDController GAME_PIECE_AUTO_DRIVE_Y_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new PIDController(0.5, 0, 0) : + new PIDController(0.3, 0, 0.03); + public static final ProfiledPIDController GAME_PIECE_AUTO_DRIVE_X_PID_CONTROLLER = RobotHardwareStats.isSimulation() ? + new ProfiledPIDController(0.5, 0, 0, new TrapezoidProfile.Constraints(2.8, 5)) : + new ProfiledPIDController(2.4, 0, 0, new TrapezoidProfile.Constraints(2.65, 5.5)); + public static final double AUTO_COLLECTION_INTAKE_OPEN_CHECK_DISTANCE_METERS = 2; + + private static final PPHolonomicDriveController AUTO_PATH_FOLLOWING_CONTROLLER = new PPHolonomicDriveController( + AUTO_TRANSLATION_PID_CONSTANTS, + AUTO_ROTATION_PID_CONSTANTS + ); + + /** + * Initializes PathPlanner. This needs to be called before any PathPlanner function can be used. + */ + public static void init() { + Pathfinding.setPathfinder(new LocalADStarAK()); + PathfindingCommand.warmupCommand().schedule(); + configureAutoBuilder(); + registerCommands(); + } + + private static void configureAutoBuilder() { + AutoBuilder.configure( + RobotContainer.ROBOT_POSE_ESTIMATOR::getEstimatedRobotPose, + RobotContainer.ROBOT_POSE_ESTIMATOR::resetPose, + RobotContainer.SWERVE::getSelfRelativeVelocity, + (chassisSpeeds -> RobotContainer.SWERVE.drivePathPlanner(chassisSpeeds, true)), + AUTO_PATH_FOLLOWING_CONTROLLER, + ROBOT_CONFIG, + Flippable::isRedAlliance, + RobotContainer.SWERVE + ); + } + + private static RobotConfig getRobotConfig() { + try { + return RobotConfig.fromGUISettings(); + } catch (IOException | ParseException e) { + throw new RuntimeException(e); + } + } + + private static void registerCommands() { + //TODO: Implement + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/FieldConstants.java b/src/main/java/frc/trigon/robot/constants/FieldConstants.java index 9061418c..1ea770ae 100644 --- a/src/main/java/frc/trigon/robot/constants/FieldConstants.java +++ b/src/main/java/frc/trigon/robot/constants/FieldConstants.java @@ -4,10 +4,11 @@ import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.*; +import lib.utilities.Conversions; import lib.utilities.FilesHandler; +import lib.utilities.flippable.FlippablePose2d; +import lib.utilities.flippable.FlippableTranslation2d; import java.io.IOException; import java.util.HashMap; @@ -18,13 +19,33 @@ public class FieldConstants { FIELD_WIDTH_METERS = FlippingUtil.fieldSizeY, FIELD_LENGTH_METERS = FlippingUtil.fieldSizeX; private static final List I_HATE_YOU = List.of( - //Tags to ignore + 13, 12, 16, 15, 14, 4, 5, 3, 2, 1 ); private static final boolean SHOULD_USE_HOME_TAG_LAYOUT = false; public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT = createAprilTagFieldLayout(); private static final Transform3d TAG_OFFSET = new Transform3d(0, 0, 0, new Rotation3d(0, 0, 0)); - public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIDToPoseMap(); + public static final HashMap TAG_ID_TO_POSE = fieldLayoutToTagIdToPoseMap(); + + private static final double + AUTO_FIND_CORAL_POSE_X = 3.3, + AUTO_FIND_CORAL_POSE_LEFT_Y = 5.5; + private static final Rotation2d AUTO_FIND_CORAL_POSE_LEFT_ROTATION = Rotation2d.fromDegrees(130); + public static final FlippablePose2d + AUTO_FIND_CORAL_POSE_LEFT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION, true), + AUTO_FIND_CORAL_POSE_RIGHT = new FlippablePose2d(AUTO_FIND_CORAL_POSE_X, FieldConstants.FIELD_WIDTH_METERS - AUTO_FIND_CORAL_POSE_LEFT_Y, AUTO_FIND_CORAL_POSE_LEFT_ROTATION.unaryMinus(), true); + + public static final Rotation2d LEFT_FEEDER_ANGLE = Rotation2d.fromDegrees(54); + + public static final int REEF_CLOCK_POSITIONS = 6; + public static final Rotation2d REEF_CLOCK_POSITION_DIFFERENCE = Rotation2d.fromDegrees(Conversions.DEGREES_PER_ROTATIONS / REEF_CLOCK_POSITIONS); + public static final Rotation2d[] REEF_CLOCK_ANGLES = ReefClockPosition.getClockAngles(); + public static final Translation2d BLUE_REEF_CENTER_TRANSLATION = new Translation2d(4.48945, FIELD_WIDTH_METERS / 2); + public static final FlippableTranslation2d FLIPPABLE_REEF_CENTER_TRANSLATION = new FlippableTranslation2d(BLUE_REEF_CENTER_TRANSLATION, true); + public static final double + REEF_CENTER_TO_TARGET_SCORING_POSITION_X_TRANSFORM_METERS = 1.3, + REEF_CENTER_TO_TARGET_SCORING_POSITION_Y_TRANSFORM_METERS = 0.17, + REEF_CENTER_TO_TARGET_ALGAE_COLLECTION_POSITION_X_TRANSFORM_METERS = 1.6; private static AprilTagFieldLayout createAprilTagFieldLayout() { try { @@ -36,12 +57,59 @@ private static AprilTagFieldLayout createAprilTagFieldLayout() { } } - private static HashMap fieldLayoutToTagIDToPoseMap() { - final HashMap tagIDToPose = new HashMap<>(); - for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) + private static HashMap fieldLayoutToTagIdToPoseMap() { + final HashMap tagIdToPose = new HashMap<>(); + for (AprilTag aprilTag : APRIL_TAG_FIELD_LAYOUT.getTags()) { if (!I_HATE_YOU.contains(aprilTag.ID)) - tagIDToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); + tagIdToPose.put(aprilTag.ID, aprilTag.pose.transformBy(TAG_OFFSET)); + } + + return tagIdToPose; + } + + public enum ReefSide { + RIGHT(true), + LEFT(false); + + public final boolean doesFlipYTransformWhenFacingDriverStation; + + ReefSide(boolean doesFlipYTransformWhenFacingDriverStation) { + this.doesFlipYTransformWhenFacingDriverStation = doesFlipYTransformWhenFacingDriverStation; + } - return tagIDToPose; + public boolean shouldFlipYTransform(ReefClockPosition reefClockPosition) { + return doesFlipYTransformWhenFacingDriverStation ^ reefClockPosition.isFacingDriverStation; // In Java, ^ acts as an XOR (exclusive OR) operator, which fits in this case + } + } + + public enum ReefClockPosition { + REEF_12_OCLOCK(true), + REEF_2_OCLOCK(true), + REEF_4_OCLOCK(true), + REEF_6_OCLOCK(true), + REEF_8_OCLOCK(true), + REEF_10_OCLOCK(true); + + public final Rotation2d clockAngle; + public final boolean isFacingDriverStation; + public final int clockPosition; + + ReefClockPosition(boolean isFacingDriverStation) { + this.clockAngle = calculateClockAngle(); + this.isFacingDriverStation = isFacingDriverStation; + this.clockPosition = ordinal() == 0 ? 12 : ordinal() * 2; + } + + public static Rotation2d[] getClockAngles() { + final Rotation2d[] clockAngles = new Rotation2d[ReefClockPosition.values().length]; + for (int i = 0; i < clockAngles.length; i++) + clockAngles[i] = ReefClockPosition.values()[i].clockAngle; + + return clockAngles; + } + + private Rotation2d calculateClockAngle() { + return REEF_CLOCK_POSITION_DIFFERENCE.times(-ordinal()); + } } -} +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java index f7506110..70e4f652 100644 --- a/src/main/java/frc/trigon/robot/constants/OperatorConstants.java +++ b/src/main/java/frc/trigon/robot/constants/OperatorConstants.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.trigon.robot.commands.commandclasses.IntakeAssistCommand; +import frc.trigon.robot.misc.ReefChooser; import lib.hardware.misc.KeyboardController; import lib.hardware.misc.XboxController; @@ -11,7 +12,9 @@ public class OperatorConstants { public static final double RUMBLE_DURATION_SECONDS = 0.7, RUMBLE_POWER = 1; - private static final int DRIVER_CONTROLLER_PORT = 0; + private static final int + DRIVER_CONTROLLER_PORT = 0, + REEF_CHOOSER_PORT = 1; private static final int DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT = 1, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT = 2; @@ -19,6 +22,7 @@ public class OperatorConstants { DRIVER_CONTROLLER_PORT, DRIVER_CONTROLLER_RIGHT_STICK_EXPONENT, DRIVER_CONTROLLER_LEFT_STICK_EXPONENT, DRIVER_CONTROLLER_DEADBAND ); public static final KeyboardController OPERATOR_CONTROLLER = new KeyboardController(); + public static final ReefChooser REEF_CHOOSER = new ReefChooser(REEF_CHOOSER_PORT); public static final double POV_DIVIDER = 2, @@ -39,4 +43,18 @@ public class OperatorConstants { FORWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.up(), BACKWARD_DYNAMIC_CHARACTERIZATION_TRIGGER = OPERATOR_CONTROLLER.down(); public static final Trigger CORAL_COLLECTION_TRIGGER = DRIVER_CONTROLLER.leftTrigger().or(OPERATOR_CONTROLLER.c()); + + public static final Trigger + SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER = OPERATOR_CONTROLLER.numpad0().or(DRIVER_CONTROLLER.povDown()), + SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER = OPERATOR_CONTROLLER.numpad1().or(DRIVER_CONTROLLER.povRight()), + SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER = OPERATOR_CONTROLLER.numpad2().or(DRIVER_CONTROLLER.povLeft()), + SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER = OPERATOR_CONTROLLER.numpad3().or(DRIVER_CONTROLLER.povUp()), + SET_TARGET_SCORING_REEF_CLOCK_POSITION_2_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad9(), + SET_TARGET_SCORING_REEF_CLOCK_POSITION_4_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad6(), + SET_TARGET_SCORING_REEF_CLOCK_POSITION_6_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad5(), + SET_TARGET_SCORING_REEF_CLOCK_POSITION_8_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad4(), + SET_TARGET_SCORING_REEF_CLOCK_POSITION_10_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad7(), + SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER = OPERATOR_CONTROLLER.numpad8(), + SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER = OPERATOR_CONTROLLER.left(), + SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER = OPERATOR_CONTROLLER.right(); } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/misc/ReefChooser.java b/src/main/java/frc/trigon/robot/misc/ReefChooser.java new file mode 100644 index 00000000..9a185914 --- /dev/null +++ b/src/main/java/frc/trigon/robot/misc/ReefChooser.java @@ -0,0 +1,130 @@ +package frc.trigon.robot.misc; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; +import frc.trigon.robot.commands.commandfactories.CoralPlacingCommands; +import frc.trigon.robot.constants.FieldConstants; +import frc.trigon.robot.constants.OperatorConstants; +import frc.trigon.robot.subsystems.elevator.ElevatorConstants; +import frc.trigon.robot.subsystems.arm.ArmConstants; +import lib.utilities.flippable.FlippablePose2d; + +import java.util.function.Supplier; + +public class ReefChooser { + private final CommandGenericHID hid; + private FieldConstants.ReefClockPosition clockPosition = FieldConstants.ReefClockPosition.REEF_6_OCLOCK; + private CoralPlacingCommands.ScoringLevel scoringLevel = CoralPlacingCommands.ScoringLevel.L4; + private FieldConstants.ReefSide reefSide = FieldConstants.ReefSide.LEFT; + + public ReefChooser(int port) { + hid = new CommandGenericHID(port); + + new WaitCommand(3).andThen(this::configureBindings).ignoringDisable(true).schedule(); + } + + public CoralPlacingCommands.ScoringLevel getScoringLevel() { + return scoringLevel; + } + + public void switchReefSide() { + reefSide = reefSide == FieldConstants.ReefSide.LEFT ? FieldConstants.ReefSide.RIGHT : FieldConstants.ReefSide.LEFT; + } + + public FieldConstants.ReefClockPosition getClockPosition() { + return clockPosition; + } + + public FieldConstants.ReefSide getReefSide() { + return reefSide; + } + + public FlippablePose2d calculateTargetScoringPose() { + return scoringLevel.calculateTargetPlacingPosition(clockPosition, reefSide); + } + + public ArmConstants.ArmState getArmState() { + return scoringLevel.armState; + } + + public ElevatorConstants.ElevatorState getElevatorState() { + return scoringLevel.elevatorState; + } + + private void configureBindings() { + configureScoringLevelBindings(); + configureReefHIDBindings(); + configureFallbackBindings(); + } + + private void configureScoringLevelBindings() { + OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L1_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L1)); + OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L2_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L2)); + OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L3_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L3)); + OperatorConstants.SET_TARGET_SCORING_REEF_LEVEL_L4_TRIGGER.onTrue(getSetTargetLevelCommand(() -> CoralPlacingCommands.ScoringLevel.L4)); + } + + private Command getSetTargetLevelCommand(Supplier scoringLevel) { + return new InstantCommand( + () -> this.scoringLevel = scoringLevel.get() + ).ignoringDisable(true); + } + + private void configureReefHIDBindings() { + for (int i = 0; i < 12; i++) + hid.button(i + 1).onTrue(getSetFaceFromIndexCommand(i)); + } + + private Command getSetFaceFromIndexCommand(int index) { + return new InstantCommand( + () -> setFaceFromIndex(index) + ).ignoringDisable(true); + } + + private void setFaceFromIndex(int index) { + clockPosition = FieldConstants.ReefClockPosition.values()[index / 2]; + reefSide = FieldConstants.ReefSide.values()[index % 2]; + } + + private void configureFallbackBindings() { + OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_2_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_2_OCLOCK)); + OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_4_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_4_OCLOCK)); + OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_6_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_6_OCLOCK)); + OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_8_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_8_OCLOCK)); + OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_10_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_10_OCLOCK)); + OperatorConstants.SET_TARGET_SCORING_REEF_CLOCK_POSITION_12_OCLOCK_TRIGGER.onTrue(getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition.REEF_12_OCLOCK)); + + OperatorConstants.SET_TARGET_REEF_SCORING_SIDE_LEFT_TRIGGER.onTrue(getSetTargetReefSideCommand(FieldConstants.ReefSide.LEFT)); + OperatorConstants.SET_TARGET_REEF_SCORING_SIDE_RIGHT_TRIGGER.onTrue(getSetTargetReefSideCommand(FieldConstants.ReefSide.RIGHT)); + } + + private Command getSetTargetClockPositionCommand(FieldConstants.ReefClockPosition clockPosition) { + return new InstantCommand( + () -> this.clockPosition = clockPosition + ).ignoringDisable(true); + } + + private Command getSetTargetReefSideCommand(FieldConstants.ReefSide reefSide) { + return new InstantCommand( + () -> this.reefSide = reefSide + ).ignoringDisable(true); + } + + private CoralPlacingCommands.ScoringLevel getIncrementedScoringLevel() { + return switch (scoringLevel) { + case L2 -> CoralPlacingCommands.ScoringLevel.L3; + case L1 -> CoralPlacingCommands.ScoringLevel.L2; + default -> CoralPlacingCommands.ScoringLevel.L4; + }; + } + + private CoralPlacingCommands.ScoringLevel getDecrementedScoringLevel() { + return switch (scoringLevel) { + case L4 -> CoralPlacingCommands.ScoringLevel.L3; + case L3 -> CoralPlacingCommands.ScoringLevel.L2; + default -> CoralPlacingCommands.ScoringLevel.L1; + }; + } +} \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java index 2e768d9f..354dd371 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/Arm.java @@ -13,6 +13,7 @@ import lib.hardware.phoenix6.cancoder.CANcoderSignal; import lib.hardware.phoenix6.talonfx.TalonFXMotor; import lib.hardware.phoenix6.talonfx.TalonFXSignal; +import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; public class Arm extends MotorSubsystem { @@ -104,6 +105,18 @@ public boolean atTargetAngle() { return atAngle(targetState.targetAngle); } + public boolean atPrepareAngle() { + if (isStateReversed) { + return atAngle(subtractFrom360Degrees(targetState.prepareAngle)); + } + return atAngle(targetState.prepareAngle); + } + + @AutoLogOutput(key = "Arm/HasCoral") + public boolean hasGamePiece() { + return ArmConstants.COLLECTION_DETECTION_BOOLEAN_EVENT.getAsBoolean(); + } + public Rotation2d getAngle() { return Rotation2d.fromRotations(angleEncoder.getSignal(CANcoderSignal.POSITION)); } @@ -135,6 +148,21 @@ void setTargetState(Rotation2d targetAngle, double targetVoltage) { setEndEffectorTargetVoltage(targetVoltage); } + void setPrepareState(ArmConstants.ArmState targetState) { + setPrepareState(targetState, false); + } + + void setPrepareState(ArmConstants.ArmState targetState, boolean isStateReversed) { + this.isStateReversed = isStateReversed; + this.targetState = targetState; + + if (isStateReversed) { + setTargetAngle(subtractFrom360Degrees(targetState.prepareAngle)); + return; + } + setTargetAngle(targetState.prepareAngle); + } + void setArmState(ArmConstants.ArmState targetState) { setArmState(targetState, false); } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java index 7b96ab63..6bb7d384 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmCommands.java @@ -2,6 +2,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import frc.trigon.robot.RobotContainer; import lib.commands.ExecuteEndCommand; @@ -9,6 +10,7 @@ import lib.commands.NetworkTablesCommand; import java.util.Set; +import java.util.function.Supplier; public class ArmCommands { public static Command getDebuggingCommand() { @@ -33,6 +35,15 @@ public static Command getGearRatioCalulationCommand() { ); } + public static Command getSetTargetStateCommand(Supplier targetState, Supplier isStateReversed) { + return new FunctionalCommand( + () -> RobotContainer.ARM.setEndEffectorState(targetState.get()), + () -> RobotContainer.ARM.setArmState(targetState.get(), isStateReversed.get()), + interrupted -> RobotContainer.ARM.stop(), + () -> false, + RobotContainer.ARM + ); } + public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState) { return getSetTargetStateCommand(targetState, false); } @@ -47,15 +58,31 @@ public static Command getSetTargetStateCommand(ArmConstants.ArmState targetState ); } + public static Command getPrepareForStateCommand(Supplier targetState, Supplier isStateReversed) { + return new ExecuteEndCommand( + () -> RobotContainer.ARM.setPrepareState(targetState.get(), isStateReversed.get()), + RobotContainer.ARM::stop, + RobotContainer.ARM + ); + } + public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState) { return getPrepareForStateCommand(targetState, false); } public static Command getPrepareForStateCommand(ArmConstants.ArmState targetState, boolean isStateReversed) { return new ExecuteEndCommand( - () -> RobotContainer.ARM.setArmState(targetState, isStateReversed), + () -> RobotContainer.ARM.setPrepareState(targetState, isStateReversed), RobotContainer.ARM::stop, RobotContainer.ARM ); } + + public static Command getRestCommand() { + return new ConditionalCommand( + getSetTargetStateCommand(ArmConstants.ArmState.REST_WITH_CORAL), + getSetTargetStateCommand(ArmConstants.ArmState.REST), + RobotContainer.ARM::hasGamePiece + ); + } } diff --git a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java index ab2c8de8..07893783 100644 --- a/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/arm/ArmConstants.java @@ -117,7 +117,7 @@ public class ArmConstants { * The highest point of the arms angular zone where the safety logic applies. */ static final Rotation2d MAXIMUM_ARM_SAFE_ANGLE = Rotation2d.fromDegrees(90); - static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(0); + static final Rotation2d ANGLE_TOLERANCE = Rotation2d.fromDegrees(5); private static final double COLLECTION_DETECTION_DEBOUNCE_TIME_SECONDS = 0.2; static final BooleanEvent COLLECTION_DETECTION_BOOLEAN_EVENT = new BooleanEvent( CommandScheduler.getInstance().getActiveButtonLoop(), @@ -232,30 +232,28 @@ private static void configureDistanceSensor() { } public enum ArmState { - REST(Rotation2d.fromDegrees(0), 0), - REST_FOR_CLIMB(Rotation2d.fromDegrees(0), 0), - HOLD_ALGAE(Rotation2d.fromDegrees(90), -4), - EJECT(Rotation2d.fromDegrees(60), 4), - PREPARE_SCORE_L1(Rotation2d.fromDegrees(80), 0), - SCORE_L1(Rotation2d.fromDegrees(75), 4), - PREPARE_SCORE_L2(Rotation2d.fromDegrees(95), 0), - SCORE_L2(Rotation2d.fromDegrees(180), 4), - PREPARE_SCORE_L3(Rotation2d.fromDegrees(95), 0), - SCORE_L3(Rotation2d.fromDegrees(90), 4), - PREPARE_SCORE_L4(Rotation2d.fromDegrees(95), 0), - SCORE_L4(Rotation2d.fromDegrees(90), 4), - PREPARE_NET_SCORE(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), - SCORE_NET(Rotation2d.fromDegrees(160), 4), - PREPARE_PROCESSOR_SCORE(Rotation2d.fromDegrees(90), HOLD_ALGAE.targetEndEffectorVoltage), - SCORE_PROCESSOR(Rotation2d.fromDegrees(90), 4), - COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), -4), - COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), -4); + REST(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), + REST_WITH_CORAL(Rotation2d.fromDegrees(180), Rotation2d.fromDegrees(180), 0), + REST_FOR_CLIMB(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), 0), + LOAD_CORAL(Rotation2d.fromDegrees(0), Rotation2d.fromDegrees(0), -4), + HOLD_ALGAE(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), + EJECT(Rotation2d.fromDegrees(60), Rotation2d.fromDegrees(60), 4), + SCORE_L1(Rotation2d.fromDegrees(110), Rotation2d.fromDegrees(110), 4), + SCORE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), + SCORE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), + SCORE_L4(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(100), 4), + SCORE_NET(Rotation2d.fromDegrees(160), Rotation2d.fromDegrees(160), 4), + SCORE_PROCESSOR(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), 4), + COLLECT_ALGAE_L2(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4), + COLLECT_ALGAE_L3(Rotation2d.fromDegrees(90), Rotation2d.fromDegrees(90), -4); public final Rotation2d targetAngle; + public final Rotation2d prepareAngle; public final double targetEndEffectorVoltage; - ArmState(Rotation2d targetAngle, double targetEndEffectorVoltage) { + ArmState(Rotation2d targetAngle, Rotation2d prepareAngle, double targetEndEffectorVoltage) { this.targetAngle = targetAngle; + this.prepareAngle = prepareAngle; this.targetEndEffectorVoltage = targetEndEffectorVoltage; } } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java index bdbed2b9..080739d1 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/Elevator.java @@ -77,6 +77,20 @@ public void updatePeriodically() { Logger.recordOutput("Elevator/CurrentPositionMeters", getPositionMeters()); } + public boolean atState(ElevatorConstants.ElevatorState targetState) { + return targetState == this.targetState && atTargetState(); + } + + public boolean atTargetState() { + final double currentToTargetStateDifferenceMeters = Math.abs(targetState.targetPositionMeters - getPositionMeters()); + return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; + } + + public boolean atPreparedTargetState() { + final double currentToTargetStateDifferenceMeters = Math.abs(targetState.prepareStatePositionMeters - getPositionMeters()); + return currentToTargetStateDifferenceMeters < ElevatorConstants.HEIGHT_TOLERANCE_METERS; + } + public double getPositionMeters() { return rotationsToMeters(getPositionRotations()); } @@ -99,6 +113,12 @@ void setTargetState(ElevatorConstants.ElevatorState targetState) { setTargetPositionRotations(metersToRotations(targetState.targetPositionMeters)); } + void prepareState(ElevatorConstants.ElevatorState targetState) { + this.targetState = targetState; + scalePositionRequestSpeed(targetState.speedScalar); + setTargetPositionRotations(metersToRotations(targetState.prepareStatePositionMeters)); + } + void setTargetPositionRotations(double targetPositionRotations) { masterMotor.setControl(positionRequest.withPosition(Math.max(targetPositionRotations, calculateMinimumSafeElevatorHeightRotations()))); } diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java index 0957aa70..988476fb 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorCommands.java @@ -1,13 +1,12 @@ package frc.trigon.robot.subsystems.elevator; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.trigon.robot.RobotContainer; -import frc.trigon.robot.subsystems.arm.ArmConstants; import lib.commands.ExecuteEndCommand; import lib.commands.NetworkTablesCommand; import java.util.Set; +import java.util.function.Supplier; public class ElevatorCommands { public static Command getDebbugingCommand() { @@ -19,6 +18,14 @@ public static Command getDebbugingCommand() { ); } + public static Command getSetTargetStateCommand(Supplier targetState) { + return new ExecuteEndCommand( + () -> RobotContainer.ELEVATOR.setTargetState(targetState.get()), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR + ); + } + public static Command getSetTargetStateCommand(ElevatorConstants.ElevatorState targetState) { return new ExecuteEndCommand( () -> RobotContainer.ELEVATOR.setTargetState(targetState), @@ -34,4 +41,19 @@ public static Command getSetTargetStateCommand(double targetPositionRotations) { RobotContainer.ELEVATOR ); } + + public static Command getPrepareStateCommand(Supplier targetState) { + return new ExecuteEndCommand( + () -> RobotContainer.ELEVATOR.prepareState(targetState.get()), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR + ); + } + + public static Command getPrepareStateCommand(ElevatorConstants.ElevatorState targetState) { + return new ExecuteEndCommand( + () -> RobotContainer.ELEVATOR.prepareState(targetState), + RobotContainer.ELEVATOR::stop, + RobotContainer.ELEVATOR); + } } \ No newline at end of file diff --git a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java index 05758a80..ca527cf9 100644 --- a/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/trigon/robot/subsystems/elevator/ElevatorConstants.java @@ -84,6 +84,7 @@ public class ElevatorConstants { MINIMUM_ELEVATOR_HEIGHT_METERS, Color.kYellow ); + static final double HEIGHT_TOLERANCE_METERS = 0.01; static final double SECOND_ELEVATOR_COMPONENT_EXTENDED_LENGTH_METERS = 0.603; static final double DRUM_DIAMETER_METERS = DRUM_RADIUS_METERS * 2; private static final double REVERSE_LIMIT_SENSOR_DEBOUNCE_TIME_SECONDS = 0.1; @@ -171,27 +172,25 @@ private static void configureReverseLimitSensor() { } public enum ElevatorState { - REST(0.603, 0.7), - LOAD_CORAL(0.5519, 0.7), - SCORE_L1(0.203, 1), - SCORE_L2(0.203, 1), - SCORE_L3(1.003, 1), - SCORE_L4(1.382, 1), - PREPARE_L1(0.603, 1), - PREPARE_L2(0.603, 1), - PREPARE_L3(1.003, 1), - PREPARE_L4(1.382, 1), - COLLECT_ALGAE_FROM_L2(0.603, 1), - COLLECT_ALGAE_FROM_L3(0.953, 1), - COLLECT_ALGAE_FROM_GROUND(0, 0.7), - REST_WITH_ALGAE(0.603, 0.3), - SCORE_NET(1.382, 0.3); + REST(0.603, 0.603, 0.7), + LOAD_CORAL(0.5519, 0.5519, 0.7), + SCORE_L1(0.1, 0.1, 1), + SCORE_L2(0.3, 0.4, 1), + SCORE_L3(0.7, 0.8, 1), + SCORE_L4(1.2, 1.3, 1), + COLLECT_ALGAE_FROM_L2(0.603, 0.603, 1), + COLLECT_ALGAE_FROM_L3(0.953, 0.953, 1), + COLLECT_ALGAE_FROM_GROUND(0, 0, 0.7), + REST_WITH_ALGAE(0.603, 0.603, 0.3), + SCORE_NET(1.382, 1.382, 0.3); public final double targetPositionMeters; + public final double prepareStatePositionMeters; final double speedScalar; - ElevatorState(double targetPositionMeters, double speedScalar) { + ElevatorState(double targetPositionMeters, double prepareStatePositionMeters, double speedScalar) { this.targetPositionMeters = targetPositionMeters; + this.prepareStatePositionMeters = prepareStatePositionMeters; this.speedScalar = speedScalar; } }