diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index d0f758e7..7a1de989 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.amp_align.AmpAlignManager; import frc.robot.autos.Autos; import frc.robot.climber.AutoClimbManager; import frc.robot.climber.ClimberSubsystem; @@ -90,8 +91,8 @@ public class Robot extends LoggedRobot { private final LightsSubsystem lightsSubsystem = new LightsSubsystem( new CANdle(RobotConfig.get().lights().deviceID(), "rio"), robotManager, vision); - private final AutoClimbManager autoClimbManager = - new AutoClimbManager(localization, actions, robotManager, swerve); + private final AutoClimbManager autoClimbManager = new AutoClimbManager(localization, swerve); + private final AmpAlignManager ampAlignManager = new AmpAlignManager(localization, swerve); public Robot() { System.out.println("roboRIO serial number: " + RobotConfig.SERIAL_NUMBER); @@ -208,7 +209,9 @@ private void configureBindings() { driverController.rightBumper().onTrue(actions.outtakeCommand()).onFalse(actions.stowCommand()); operatorController.povUp().onTrue(actions.getClimberForwardCommand()); - operatorController.povDown().whileTrue(autoClimbManager.getClimbSequenceCommand()); + operatorController.povLeft().whileTrue(autoClimbManager.getClimbSequenceCommand()); + operatorController.povRight().whileTrue(ampAlignManager.getAlignForAmpCommand()); + operatorController.povDown().onTrue(actions.getClimberBackwardCommand()); operatorController.a().onTrue(actions.stowCommand()); operatorController diff --git a/src/main/java/frc/robot/amp_align/AmpAlignManager.java b/src/main/java/frc/robot/amp_align/AmpAlignManager.java new file mode 100644 index 00000000..91ae3af7 --- /dev/null +++ b/src/main/java/frc/robot/amp_align/AmpAlignManager.java @@ -0,0 +1,57 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.amp_align; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.localization.LocalizationSubsystem; +import frc.robot.swerve.SwerveSubsystem; +import frc.robot.util.scheduling.LifecycleSubsystem; +import frc.robot.util.scheduling.SubsystemPriority; +import java.util.List; +import org.littletonrobotics.junction.Logger; + +public class AmpAlignManager extends LifecycleSubsystem { + private final LocalizationSubsystem localization; + private final SwerveSubsystem swerve; + + public AmpAlignManager(LocalizationSubsystem localization, SwerveSubsystem swerve) { + super(SubsystemPriority.CLIMBER); + + this.localization = localization; + this.swerve = swerve; + } + + private AmpAlignPath getClosestTarget() { + Pose2d current = localization.getPose(); + + List paths = AmpAlignPaths.PATHS; + + AmpAlignPath closest = AmpAlignPaths.PATHS.get(0); + double currentDistance = Double.POSITIVE_INFINITY; + + for (AmpAlignPath target : paths) { + double distance = + target.behindAmpPose().getTranslation().getDistance(current.getTranslation()); + if (distance < currentDistance) { + closest = target; + currentDistance = distance; + } + } + return closest; + } + + public Command getAlignForAmpCommand() { + return swerve.driveToPoseCommand( + () -> getClosestTarget().behindAmpPose(), localization::getPose); + } + + @Override + public void robotPeriodic() { + Logger.recordOutput("AmpAlign/ClosestPose", getClosestTarget().behindAmpPose()); + Logger.recordOutput( + "AmpAlign/ClosestPathRotation", getClosestTarget().behindAmpPose().getRotation()); + } +} diff --git a/src/main/java/frc/robot/amp_align/AmpAlignPath.java b/src/main/java/frc/robot/amp_align/AmpAlignPath.java new file mode 100644 index 00000000..96a0e303 --- /dev/null +++ b/src/main/java/frc/robot/amp_align/AmpAlignPath.java @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.amp_align; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +public record AmpAlignPath(Pose2d behindAmpPose, Rotation2d robotHeading) {} diff --git a/src/main/java/frc/robot/amp_align/AmpAlignPaths.java b/src/main/java/frc/robot/amp_align/AmpAlignPaths.java new file mode 100644 index 00000000..8511e8ae --- /dev/null +++ b/src/main/java/frc/robot/amp_align/AmpAlignPaths.java @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.amp_align; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import java.util.List; + +public class AmpAlignPaths { + public static final AmpAlignPath RED_AMP = + new AmpAlignPath( + new Pose2d(14.66, 7.32, Rotation2d.fromDegrees(-90)), Rotation2d.fromDegrees(-90)); + public static final AmpAlignPath BLUE_AMP = + new AmpAlignPath( + new Pose2d(1.8, 7.19, Rotation2d.fromDegrees(-90)), Rotation2d.fromDegrees(-90)); + + public static final List PATHS = List.of(RED_AMP, BLUE_AMP); +} diff --git a/src/main/java/frc/robot/climber/AutoClimbManager.java b/src/main/java/frc/robot/climber/AutoClimbManager.java index 8f518dbc..1c969894 100644 --- a/src/main/java/frc/robot/climber/AutoClimbManager.java +++ b/src/main/java/frc/robot/climber/AutoClimbManager.java @@ -8,8 +8,6 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.fms.FmsSubsystem; import frc.robot.localization.LocalizationSubsystem; -import frc.robot.robot_manager.RobotCommands; -import frc.robot.robot_manager.RobotManager; import frc.robot.swerve.SwerveSubsystem; import frc.robot.util.scheduling.LifecycleSubsystem; import frc.robot.util.scheduling.SubsystemPriority; @@ -18,20 +16,12 @@ public class AutoClimbManager extends LifecycleSubsystem { private final LocalizationSubsystem localization; - private final RobotCommands actions; - private final RobotManager robot; private final SwerveSubsystem swerve; - public AutoClimbManager( - LocalizationSubsystem localization, - RobotCommands actions, - RobotManager robot, - SwerveSubsystem swerve) { + public AutoClimbManager(LocalizationSubsystem localization, SwerveSubsystem swerve) { super(SubsystemPriority.CLIMBER); this.localization = localization; - this.actions = actions; - this.robot = robot; this.swerve = swerve; } diff --git a/src/main/java/frc/robot/climber/ClimbAlignPaths.java b/src/main/java/frc/robot/climber/ClimbAlignPaths.java index f290f3e4..1e70d5de 100644 --- a/src/main/java/frc/robot/climber/ClimbAlignPaths.java +++ b/src/main/java/frc/robot/climber/ClimbAlignPaths.java @@ -11,37 +11,31 @@ public class ClimbAlignPaths { public static final ClimbPath RED_LEFT = new ClimbPath( - // PathPlannerPath.bezierFromPoses( new Pose2d(12.69, 2.37, Rotation2d.fromDegrees(-60)), new Pose2d(12.0, 3.5, Rotation2d.fromDegrees(-60)), Rotation2d.fromDegrees(-60)); public static final ClimbPath RED_FORWARD = new ClimbPath( - // PathPlannerPath.bezierFromPoses( new Pose2d(9.48, 3.94, Rotation2d.fromDegrees(180)), new Pose2d(10.80, 3.94, Rotation2d.fromDegrees(180)), Rotation2d.fromDegrees(180)); public static final ClimbPath RED_RIGHT = new ClimbPath( - // PathPlannerPath.bezierFromPoses( new Pose2d(12.6, 5.74, Rotation2d.fromDegrees(60)), new Pose2d(12.14, 4.95, Rotation2d.fromDegrees(60)), Rotation2d.fromDegrees(60)); public static final ClimbPath BLUE_LEFT = new ClimbPath( - // PathPlannerPath.bezierFromPoses( new Pose2d(5.64, 5.5, Rotation2d.fromDegrees(0)), new Pose2d(5.00, 4.75, Rotation2d.fromDegrees(0)), Rotation2d.fromDegrees(120)); public static final ClimbPath BLUE_FORWARD = new ClimbPath( - // PathPlannerPath.bezierFromPoses( new Pose2d(6.32, 4.11, Rotation2d.fromDegrees(0)), new Pose2d(5.55, 4.11, Rotation2d.fromDegrees(0)), Rotation2d.fromDegrees(240)); public static final ClimbPath BLUE_RIGHT = new ClimbPath( - // PathPlannerPath.bezierFromPoses( new Pose2d(3.85, 2.49, Rotation2d.fromDegrees(-120)), new Pose2d(4.36, 3.26, Rotation2d.fromDegrees(-120)), Rotation2d.fromDegrees(-120));