diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c64a5aae..1a74b3df 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -474,7 +474,8 @@ public RobotVisualizer getRobotVisualizer() { } public void putShuffleboardCommands() { - + SmartDashboard.putData( + "Align", new SuperAutoScore(drivetrain, elevatorSubsystem, coralSubsystem, vision)); if (Constants.CORAL_DEBUG) { SmartDashboard.putData( "Shoot Coral", new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); diff --git a/src/main/java/frc/robot/commands/SuperAutoScore.java b/src/main/java/frc/robot/commands/SuperAutoScore.java index 251068c9..8c5dd633 100644 --- a/src/main/java/frc/robot/commands/SuperAutoScore.java +++ b/src/main/java/frc/robot/commands/SuperAutoScore.java @@ -1,6 +1,7 @@ package frc.robot.commands; import frc.robot.commands.alignment.AlignClosestBranch; +import frc.robot.commands.alignment.RoughAlignClosestBranch; import frc.robot.commands.coral.ShootCoral; import frc.robot.commands.elevator.ElevatorToStoredPosition; import frc.robot.constants.Constants; @@ -20,8 +21,10 @@ public SuperAutoScore( Vision vision) { super( new SetSuperAutoScorePosition(drivetrain, elevator, vision), - new LoggableParallelCommandGroup( - new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator)), + new LoggableSequentialCommandGroup( + new RoughAlignClosestBranch(drivetrain), + new LoggableParallelCommandGroup( + new AlignClosestBranch(drivetrain), new ElevatorToStoredPosition(elevator))), new LoggableWaitCommand(0.2), new ShootCoral(coralSubsystem, Constants.CORAL_SHOOTER_SPEED)); } diff --git a/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java b/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java new file mode 100644 index 00000000..c8db6063 --- /dev/null +++ b/src/main/java/frc/robot/commands/alignment/RoughAlignClosestBranch.java @@ -0,0 +1,42 @@ +package frc.robot.commands.alignment; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.AlignmentPosition; +import frc.robot.constants.Constants; +import frc.robot.subsystems.swervev3.SwerveDrivetrain; +import frc.robot.utils.logging.PathPlannerUtils; +import frc.robot.utils.logging.commands.LoggableCommand; +import org.littletonrobotics.junction.Logger; + +public class RoughAlignClosestBranch extends LoggableCommand { + private final SwerveDrivetrain drivetrain; + private final Timer timer = new Timer(); + private Pose2d position; + + public RoughAlignClosestBranch(SwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); + } + + @Override + public void initialize() { + timer.restart(); + AlignmentPosition alignmentTarget = + AlignmentPosition.getClosest(drivetrain.getPose().getTranslation()); + Logger.recordOutput("TargetReefRoughAlignment", alignmentTarget); + Pose2d position = alignmentTarget.getPathPlannerPosition(); + Logger.recordOutput("TargetReefRoughPose", position); + PathPlannerUtils.pathToPose(position, 0).schedule(); + } + + @Override + public boolean isFinished() { + if (drivetrain.getPose().getTranslation().getDistance(position.getTranslation()) + < Constants.ALIGNMENT_DISTANCE_THRESHOLD) { + return true; + } else { + return false; + } + } +} diff --git a/src/main/java/frc/robot/constants/AlignmentPosition.java b/src/main/java/frc/robot/constants/AlignmentPosition.java index f02f8165..2de7e33e 100644 --- a/src/main/java/frc/robot/constants/AlignmentPosition.java +++ b/src/main/java/frc/robot/constants/AlignmentPosition.java @@ -12,41 +12,48 @@ public enum AlignmentPosition { // spotless:off Although this makes it harder to read it makes it easy to copy and paste from sheets - A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + A(new Pose2d(3.2076000000000, 4.0614305050000, Rotation2d.fromDegrees(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(3.0076000000000, 4.0614305050000, Rotation2d.fromDegrees(0))), - B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN), + B(new Pose2d(3.2076000000000, 3.7328134950000, Rotation2d.fromRadians(0)), Apriltag.EIGHTEEN, Apriltag.SEVEN, new Pose2d(3.0076000000000, 3.7328134950000, Rotation2d.fromRadians(0))), - C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + C(new Pose2d(3.8176981000607, 2.9336485880936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(3.7176981000607, 2.7604435073367, Rotation2d.fromDegrees(60))), - D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT), + D(new Pose2d(4.1022887788364, 2.7693400830936, Rotation2d.fromDegrees(60)), Apriltag.SEVENTEEN, Apriltag.EIGHT, new Pose2d(4.0022887788364, 2.5961350023367, Rotation2d.fromDegrees(60))), - E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + E(new Pose2d(5.0994349400607, 2.8981180830936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.1994349400607, 2.7249130023367 , Rotation2d.fromDegrees(120))), - F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE), + F(new Pose2d(5.3840256188364, 3.0624265880936, Rotation2d.fromDegrees(120)), Apriltag.TWENTY_TWO, Apriltag.NINE, new Pose2d(5.4840256188364, 2.8892215073367, Rotation2d.fromDegrees(120))), - G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + G(new Pose2d(5.7710736800000, 3.9903694950000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(5.9710736800000, 3.9903694950000, Rotation2d.fromDegrees(180))), - H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN), + H(new Pose2d(5.7710736800000, 4.3189865050000, Rotation2d.fromDegrees(180)), Apriltag.TWENTY_ONE, Apriltag.TEN, new Pose2d(5.9710736800000, 4.3189865050000, Rotation2d.fromDegrees(180))), - I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + I(new Pose2d(5.1609755799393, 5.1181514119064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(5.2609755799393, 5.2913564926633, Rotation2d.fromDegrees(240))), - J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN), + J(new Pose2d(4.8763849011636, 5.2824599169064, Rotation2d.fromDegrees(240)), Apriltag.TWENTY, Apriltag.ELEVEN, new Pose2d(4.9763849011636, 5.4556649976633, Rotation2d.fromDegrees(240))), - K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX), + K(new Pose2d(3.8792387399393, 5.1536819169064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.7792387399393, 5.3268869976633, Rotation2d.fromDegrees(300))), - L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX); + L(new Pose2d(3.5946480611636, 4.9893734119064, Rotation2d.fromDegrees(300)), Apriltag.NINETEEN, Apriltag.SIX, new Pose2d(3.4946480611636, 5.1625784926633, Rotation2d.fromDegrees(300))); // spotless:on private final Pose2d bluePosition; private final Pose2d redPosition; private final Apriltag blueTag; private final Apriltag redTag; + private final Pose2d bluePathPlannerPosition; + private final Pose2d redPathPlannerPosition; - AlignmentPosition(Pose2d position, Apriltag blueTag, Apriltag redTag) { + AlignmentPosition( + Pose2d position, Apriltag blueTag, Apriltag redTag, Pose2d pathPlannerPosition) { this.bluePosition = position; + this.bluePathPlannerPosition = pathPlannerPosition; Rotation2d redRotation = Rotation2d.fromRadians(Math.PI + position.getRotation().getRadians()); double redX = FieldLocation.LENGTH_OF_FIELD - bluePosition.getX(); double redY = FieldLocation.HEIGHT_OF_FIELD - bluePosition.getY(); + double redPathPlannerX = FieldLocation.LENGTH_OF_FIELD - pathPlannerPosition.getX(); + double redPathPlannerY = FieldLocation.HEIGHT_OF_FIELD - pathPlannerPosition.getY(); + this.redPathPlannerPosition = new Pose2d(redPathPlannerX, redPathPlannerY, redRotation); this.redPosition = new Pose2d(redX, redY, redRotation); this.blueTag = blueTag; this.redTag = redTag; @@ -60,6 +67,14 @@ public Pose2d getRedPosition() { return redPosition; } + public Pose2d getBluePathPlannerPosition() { + return bluePathPlannerPosition; + } + + public Pose2d getRedPathPlannerPosition() { + return redPathPlannerPosition; + } + public Pose2d getPosition() { Optional allianceColor = Robot.getAllianceColor(); return allianceColor @@ -69,6 +84,17 @@ public Pose2d getPosition() { .orElse(new Pose2d()); } + public Pose2d getPathPlannerPosition() { + Optional allianceColor = Robot.getAllianceColor(); + return allianceColor + .map( + alliance -> + alliance == DriverStation.Alliance.Blue + ? getBluePathPlannerPosition() + : getRedPathPlannerPosition()) + .orElse(new Pose2d()); + } + public static AlignmentPosition getClosest(Translation2d currentPosition) { int closestIndex = 0; double closestDistance = diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index d430ec25..abefd6c5 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -90,7 +90,7 @@ public class GameConstants { // Treshholds public static final double VISION_CONSISTANCY_THRESHOLD = 0.25; - public static final double AUTO_ALIGN_THRESHOLD = 2.3; // degrees //TODO: change later + public static final double AUTO_ALIGN_THRESHOLD = 0.05; // TODO: change later public static final double ELEVATOR_ENCODER_THRESHHOLD = 1; // TODO: Change Later public static final int HIHI_EXTENDER_TICK_LIMIT = 10; diff --git a/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java b/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java index e291c0d3..4c010845 100644 --- a/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java +++ b/src/main/java/frc/robot/utils/logging/PathPlannerUtils.java @@ -15,9 +15,9 @@ public class PathPlannerUtils { private static final PathConstraints defualtPathConstraints = new PathConstraints( Constants.MAX_VELOCITY, - Constants.MAX_VELOCITY, - Math.toRadians(1000), - Math.toRadians(1000)); + Constants.MAX_PATHPLANNER_ACCEL, + Constants.MAX_ANGULAR_SPEED, + Math.toRadians(Constants.MAX_PATHPLANNER_ANGULAR_ACCEL)); public static PathPlannerPath createManualPath( Pose2d startPose, Pose2d targetPos, double endVelocity) {