From bd3bd3de74a422cda41e7947655781b6521c97e6 Mon Sep 17 00:00:00 2001 From: NandaGuntupalli <70027620+NandaGuntupalli@users.noreply.github.com> Date: Wed, 24 Jan 2024 09:10:35 -0500 Subject: [PATCH 1/5] Created AutoClimber Commands --- src/main/java/frc/robot/Constants.java | 56 ++++++ src/main/java/frc/robot/RobotContainer.java | 21 +++ .../commands/AutoClimb/AutoClimbCenter.java | 54 ++++++ .../commands/AutoClimb/AutoClimbLeft.java | 53 ++++++ .../commands/AutoClimb/AutoClimbRight.java | 52 ++++++ .../java/frc/robot/subsystems/Climber.java | 2 +- .../java/frc/robot/subsystems/Intake.java | 1 - .../java/frc/robot/subsystems/Shooter.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 168 ++++++++++++++++++ 9 files changed, 406 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java create mode 100644 src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java create mode 100644 src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6f786ac..9f340a1 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -62,6 +62,62 @@ public static final class FieldConstants { public static Pose2d driverStationRedAlliance = new Pose2d(); } + public static final class StationCoordinateConstants { + public static final Translation2d tag11Coordinates = new Translation2d(12.41, 2.87); + public static final double tag11LeftXCoordinate = 11.89; + public static final double tag11LeftYCoordinate = 2.59; + public static final double tag11RightXCoordinate = 12.94; + public static final double tag11RightYCoordinate = 3.18; + public static final double tag11CenterXCoordinate = 12.41; + public static final double tag11CenterYCoordinate = 2.87; + public static final double tag11AngleHeading = -59.30; + + public static final Translation2d tag12Coordinates = new Translation2d(12.39, 5.32); + public static final double tag12LeftXCoordinate = 13.07; + public static final double tag12LeftYCoordinate = 5; + public static final double tag12RightXCoordinate = 11.98; + public static final double tag12RightYCoordinate = 5.6; + public static final double tag12CenterXCoordinate = 12.39; + public static final double tag12CenterYCoordinate = 5.32; + public static final double tag12AngleHeading = 60; + + public static final Translation2d tag13Coordinates = new Translation2d(10.23, 4.06); + public static final double tag13LeftXCoordinate = 10.27; + public static final double tag13LeftYCoordinate = 4.69; + public static final double tag13RightXCoordinate = 10.27; + public static final double tag13RightYCoordinate = 3.41; + public static final double tag13CenterXCoordinate = 10.23; + public static final double tag13CenterYCoordinate = 4.06; + public static final double tag13AngleHeading = -180; + + public static final Translation2d tag14Coordinates = new Translation2d(6.32, 3.99); + public static final double tag14LeftXCoordinate = 6.31; + public static final double tag14LeftYCoordinate = 3.57; + public static final double tag14RightXCoordinate = 6.31; + public static final double tag14RightYCoordinate = 4.63; + public static final double tag14CenterXCoordinate = 6.32; + public static final double tag14CenterYCoordinate = 3.99; + public static final double tag14AngleHeading = -2.01; + + public static final Translation2d tag15Coordinates = new Translation2d(4.14, 5.29); + public static final double tag15LeftXCoordinate = 4.72; + public static final double tag15LeftYCoordinate = 5.70; + public static final double tag15RightXCoordinate = 3.60; + public static final double tag15RightYCoordinate = 5.05; + public static final double tag15CenterXCoordinate = 4.14; + public static final double tag15CenterYCoordinate = 5.29; + public static final double tag15AngleHeading = 120; + + public static final Translation2d tag16Coordinates = new Translation2d(4.06, 2.89); + public static final double tag16LeftXCoordinate = 3.61; + public static final double tag16LeftYCoordinate = 3.16; + public static final double tagl6RightXCoordinate = 4.69; + public static final double tag16RightYCoordinate = 2.60; + public static final double tag16CenterXCoordinate = 4.06; + public static final double tag16CenterYCoordinate = 2.89; + public static final double tag16AngleHeading = -123.52; + } + public static final class VisionConstants { public static final String limelightName = "limelight"; public static final String arducamName = "Arducam_OV9281"; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 70d1943..f7ed3e3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,9 @@ import frc.robot.Constants.OperatorConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.commands.TeleopSwerve; +import frc.robot.commands.AutoClimb.AutoClimbCenter; +import frc.robot.commands.AutoClimb.AutoClimbLeft; +import frc.robot.commands.AutoClimb.AutoClimbRight; import frc.robot.commands.arm.ArmHold; import frc.robot.commands.arm.AutoShoot; import frc.robot.subsystems.Arm; @@ -236,6 +239,24 @@ private void configureDriveBindings() { SwerveConstants.turboMaxTranslationalSpeed, SwerveConstants.maxAngularSpeed, swerve)); + + driverController + .x() + .whileTrue( + new AutoClimbLeft() + ); + + driverController + .y() + .whileTrue( + new AutoClimbCenter() + ); + + driverController + .b() + .whileTrue( + new AutoClimbRight() + ); } private void configureIntakeBindings() { diff --git a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java new file mode 100644 index 0000000..04bec92 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java @@ -0,0 +1,54 @@ +// 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.commands.AutoClimb; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Swerve; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoClimbCenter extends SequentialCommandGroup { + /** Creates a new AutoClimbCenter. */ + + Swerve swerve; + Climber climber; + + public AutoClimbCenter() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + double[] coordinates = swerve.getCenterChainPose(); + double xCoordinates = coordinates[0]; + double yCoordinates = coordinates[1]; + Rotation2d angleHeading = Rotation2d.fromDegrees(coordinates[2]); + + addCommands( + Commands.runOnce( + () -> climber.setClimberUp()), + + AutoBuilder.pathfindToPose( + new Pose2d( + xCoordinates, + yCoordinates, + angleHeading), + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + Units.degreesToRadians(180.0), 180.0))); + + Commands.runOnce( + () -> climber.setClimberDown()); + } +} diff --git a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java new file mode 100644 index 0000000..39e484d --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java @@ -0,0 +1,53 @@ +// 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.commands.AutoClimb; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Swerve; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoClimbLeft extends SequentialCommandGroup { + /** Creates a new AutoClimbLeft. */ + Swerve swerve; + Climber climber; + + public AutoClimbLeft() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + double[] coordinates = swerve.getLeftChainPose(); + double xCoordinates = coordinates[0]; + double yCoordinates = coordinates[1]; + Rotation2d angleHeading = Rotation2d.fromDegrees(coordinates[2]); + + addCommands( + Commands.runOnce( + () -> climber.setClimberUp()), + + AutoBuilder.pathfindToPose( + new Pose2d( + xCoordinates, + yCoordinates, + angleHeading), + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + Units.degreesToRadians(180.0), 180.0))); + + Commands.runOnce( + () -> climber.setClimberDown()); + } +} diff --git a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java new file mode 100644 index 0000000..58cb10e --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java @@ -0,0 +1,52 @@ +// 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.commands.AutoClimb; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathConstraints; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.Constants.SwerveConstants; +import frc.robot.subsystems.Climber; +import frc.robot.subsystems.Swerve; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoClimbRight extends SequentialCommandGroup { + /** Creates a new AutoClimbRight. */ + Swerve swerve; + Climber climber; + public AutoClimbRight() { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + + double[] coordinates = swerve.getRightChainPose(); + double xCoordinates = coordinates[0]; + double yCoordinates = coordinates[1]; + Rotation2d angleHeading = Rotation2d.fromDegrees(coordinates[2]); + + addCommands( + Commands.runOnce( + () -> climber.setClimberUp()), + + AutoBuilder.pathfindToPose( + new Pose2d( + xCoordinates, + yCoordinates, + angleHeading), + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + Units.degreesToRadians(180.0), 180.0))); + + Commands.runOnce( + () -> climber.setClimberDown()); + } +} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index 54ac872..8f8fcbe 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -32,7 +32,7 @@ public void setClimberDown() { public void stopClimberMotors() { mainMotor.set(0.0); - } + }n @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 2b96ba5..1658e8d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import com.revrobotics.RelativeEncoder; -import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.lib.controllers.VirtualJoystick; import frc.lib.controllers.VirtualXboxController; import frc.lib.subsystem.VirtualSubsystem; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index ad9d4f8..c298d6b 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -35,7 +35,7 @@ public Shooter() { shooterPID.setP(ShooterConstants.shooterP); } - + b public void setMotorSpeed(double velocity) { double feedForward = shooterFeedforward.calculate(velocity); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 920730b..2c63c23 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -48,6 +48,7 @@ import frc.robot.Constants.FieldConstants; import frc.robot.Constants.FrontLeftModule; import frc.robot.Constants.FrontRightModule; +import frc.robot.Constants.StationCoordinateConstants; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.VisionConstants.ArducamConstants; @@ -609,6 +610,173 @@ private void updatePhotonVisionPoses() { } } + public Translation2d getNearestChain() { + List chainCoordinates = List.of(StationCoordinateConstants.tag11Coordinates, + StationCoordinateConstants.tag12Coordinates, + StationCoordinateConstants.tag13Coordinates, + StationCoordinateConstants.tag14Coordinates, + StationCoordinateConstants.tag15Coordinates, + StationCoordinateConstants.tag16Coordinates); + + Translation2d nearestChainPose = new Translation2d(); + + return nearestChainPose.nearest(chainCoordinates); + } + + public double[] getCenterChainPose() { + double[] coordinates = new double[2]; + + if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { + coordinates[0] = StationCoordinateConstants.tag11CenterXCoordinate; + coordinates[1] = StationCoordinateConstants.tag11CenterYCoordinate; + coordinates[2] = StationCoordinateConstants.tag11AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { + coordinates[0] = StationCoordinateConstants.tag12CenterXCoordinate; + coordinates[1] = StationCoordinateConstants.tag12CenterYCoordinate; + coordinates[2] = StationCoordinateConstants.tag12AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { + coordinates[0] = StationCoordinateConstants.tag13CenterXCoordinate; + coordinates[1] = StationCoordinateConstants.tag13CenterYCoordinate; + coordinates[2] = StationCoordinateConstants.tag13AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { + coordinates[0] = StationCoordinateConstants.tag14CenterXCoordinate; + coordinates[1] = StationCoordinateConstants.tag14CenterYCoordinate; + coordinates[2] = StationCoordinateConstants.tag14AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { + coordinates[0] = StationCoordinateConstants.tag15CenterXCoordinate; + coordinates[1] = StationCoordinateConstants.tag15CenterYCoordinate; + coordinates[2] = StationCoordinateConstants.tag15AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { + coordinates[0] = StationCoordinateConstants.tag16CenterXCoordinate; + coordinates[1] = StationCoordinateConstants.tag16CenterYCoordinate; + coordinates[2] = StationCoordinateConstants.tag16AngleHeading; + return coordinates; + } else { + coordinates[0] = 1000; + coordinates[1] = 1000; + coordinates[2] = 1000; + return coordinates; + } + } + + public double[] getRightChainPose() { + double[] coordinates = new double[2]; + + if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { + coordinates[0] = StationCoordinateConstants.tag11RightXCoordinate; + coordinates[1] = StationCoordinateConstants.tag11RightYCoordinate; + coordinates[2] = StationCoordinateConstants.tag11AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { + coordinates[0] = StationCoordinateConstants.tag12RightXCoordinate; + coordinates[1] = StationCoordinateConstants.tag12RightYCoordinate; + coordinates[2] = StationCoordinateConstants.tag12AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { + coordinates[0] = StationCoordinateConstants.tag13RightXCoordinate; + coordinates[1] = StationCoordinateConstants.tag13RightYCoordinate; + coordinates[2] = StationCoordinateConstants.tag13AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { + coordinates[0] = StationCoordinateConstants.tag14RightXCoordinate; + coordinates[1] = StationCoordinateConstants.tag14RightYCoordinate; + coordinates[2] = StationCoordinateConstants.tag14AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { + coordinates[0] = StationCoordinateConstants.tag15RightXCoordinate; + coordinates[1] = StationCoordinateConstants.tag15RightYCoordinate; + coordinates[2] = StationCoordinateConstants.tag15AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { + coordinates[0] = StationCoordinateConstants.tagl6RightXCoordinate; + coordinates[1] = StationCoordinateConstants.tag16RightYCoordinate; + coordinates[2] = StationCoordinateConstants.tag16AngleHeading; + return coordinates; + } else { + coordinates[0] = 1000; + coordinates[1] = 1000; + coordinates[2] = 1000; + return coordinates; + } + } + + public double[] getLeftChainPose() { + double[] coordinates = new double[2]; + + if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { + coordinates[0] = StationCoordinateConstants.tag11LeftXCoordinate; + coordinates[1] = StationCoordinateConstants.tag11LeftYCoordinate; + coordinates[2] = StationCoordinateConstants.tag11AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { + coordinates[0] = StationCoordinateConstants.tag12LeftXCoordinate; + coordinates[1] = StationCoordinateConstants.tag12LeftYCoordinate; + coordinates[2] = StationCoordinateConstants.tag12AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { + coordinates[0] = StationCoordinateConstants.tag13LeftXCoordinate; + coordinates[1] = StationCoordinateConstants.tag13LeftYCoordinate; + coordinates[2] = StationCoordinateConstants.tag13AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { + coordinates[0] = StationCoordinateConstants.tag14LeftXCoordinate; + coordinates[1] = StationCoordinateConstants.tag14LeftYCoordinate; + coordinates[2] = StationCoordinateConstants.tag14AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { + coordinates[0] = StationCoordinateConstants.tag15LeftXCoordinate; + coordinates[1] = StationCoordinateConstants.tag15LeftYCoordinate; + coordinates[2] = StationCoordinateConstants.tag15AngleHeading; + return coordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { + coordinates[0] = StationCoordinateConstants.tag16LeftXCoordinate; + coordinates[1] = StationCoordinateConstants.tag16LeftYCoordinate; + coordinates[2] = StationCoordinateConstants.tag16AngleHeading; + return coordinates; + } else{ + coordinates[0] = 1000; + coordinates[1] = 1000; + coordinates[2] = 1000; + return coordinates; + } + } + + public void updateVisionPoseEstimates() { detectedTargets.clear(); updateLimelightPoses(VisionConstants.limelightName); From a9d4d1bbb41baa9edc65553fec1342d9f438d7d9 Mon Sep 17 00:00:00 2001 From: NandaGuntupalli <70027620+NandaGuntupalli@users.noreply.github.com> Date: Wed, 24 Jan 2024 19:25:18 -0500 Subject: [PATCH 2/5] Updated Autoclimber to not suck --- src/main/java/frc/robot/Constants.java | 74 ++++----- src/main/java/frc/robot/RobotContainer.java | 46 ++++-- .../commands/AutoClimb/AutoClimbCenter.java | 54 ------ .../commands/AutoClimb/AutoClimbLeft.java | 53 ------ .../commands/AutoClimb/AutoClimbRight.java | 52 ------ .../java/frc/robot/subsystems/Climber.java | 2 +- .../java/frc/robot/subsystems/Intake.java | 1 - .../java/frc/robot/subsystems/Shooter.java | 2 +- .../java/frc/robot/subsystems/Swerve.java | 156 ++++++------------ 9 files changed, 121 insertions(+), 319 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java delete mode 100644 src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java delete mode 100644 src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 8eca697..dcc3cbc 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -17,6 +17,7 @@ import frc.robot.util.LinearInterpolation; import frc.robot.util.PolynomialRegression; import java.awt.geom.Point2D; +import java.util.List; /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean @@ -64,59 +65,46 @@ public static final class FieldConstants { } public static final class StationCoordinateConstants { + public static final List chainCoordinates = List.of( + StationCoordinateConstants.tag11Coordinates, + StationCoordinateConstants.tag12Coordinates, + StationCoordinateConstants.tag13Coordinates, + StationCoordinateConstants.tag14Coordinates, + StationCoordinateConstants.tag15Coordinates, + StationCoordinateConstants.tag16Coordinates); + public static final Translation2d tag11Coordinates = new Translation2d(12.41, 2.87); - public static final double tag11LeftXCoordinate = 11.89; - public static final double tag11LeftYCoordinate = 2.59; - public static final double tag11RightXCoordinate = 12.94; - public static final double tag11RightYCoordinate = 3.18; - public static final double tag11CenterXCoordinate = 12.41; - public static final double tag11CenterYCoordinate = 2.87; - public static final double tag11AngleHeading = -59.30; + public static final Translation2d tag11LeftCoordinates = new Translation2d(11.89, 2.59); + public static final Translation2d tag11RightCoordinates = new Translation2d(12.94, 3.18); + public static final Rotation2d tag11AngleHeading = Rotation2d.fromDegrees(-59.30); public static final Translation2d tag12Coordinates = new Translation2d(12.39, 5.32); - public static final double tag12LeftXCoordinate = 13.07; - public static final double tag12LeftYCoordinate = 5; - public static final double tag12RightXCoordinate = 11.98; - public static final double tag12RightYCoordinate = 5.6; - public static final double tag12CenterXCoordinate = 12.39; - public static final double tag12CenterYCoordinate = 5.32; - public static final double tag12AngleHeading = 60; - + public static final Translation2d tag12LeftCoordinates = new Translation2d(13.07, 5); + public static final Translation2d tag12RightCoordinates = new Translation2d(11.98, 5.6); + public static final Rotation2d tag12AngleHeading = Rotation2d.fromDegrees(60); + public static final Translation2d tag13Coordinates = new Translation2d(10.23, 4.06); - public static final double tag13LeftXCoordinate = 10.27; - public static final double tag13LeftYCoordinate = 4.69; - public static final double tag13RightXCoordinate = 10.27; - public static final double tag13RightYCoordinate = 3.41; - public static final double tag13CenterXCoordinate = 10.23; - public static final double tag13CenterYCoordinate = 4.06; - public static final double tag13AngleHeading = -180; + public static final Translation2d tag13LeftCoordinates = new Translation2d(10.27, 4.69); + public static final Translation2d tag13RightCoordinates = new Translation2d(10.27, 3.41); + public static final Rotation2d tag13AngleHeading = Rotation2d.fromDegrees(-180); public static final Translation2d tag14Coordinates = new Translation2d(6.32, 3.99); - public static final double tag14LeftXCoordinate = 6.31; - public static final double tag14LeftYCoordinate = 3.57; - public static final double tag14RightXCoordinate = 6.31; - public static final double tag14RightYCoordinate = 4.63; - public static final double tag14CenterXCoordinate = 6.32; - public static final double tag14CenterYCoordinate = 3.99; - public static final double tag14AngleHeading = -2.01; + public static final Translation2d tag14LeftCoordinates = new Translation2d(6.31, 3.57); + public static final Translation2d tag14RightCoordinates = new Translation2d(6.31, 4.63); + public static final Rotation2d tag14AngleHeading = Rotation2d.fromDegrees(-2.01); public static final Translation2d tag15Coordinates = new Translation2d(4.14, 5.29); - public static final double tag15LeftXCoordinate = 4.72; - public static final double tag15LeftYCoordinate = 5.70; - public static final double tag15RightXCoordinate = 3.60; - public static final double tag15RightYCoordinate = 5.05; - public static final double tag15CenterXCoordinate = 4.14; - public static final double tag15CenterYCoordinate = 5.29; - public static final double tag15AngleHeading = 120; + public static final Translation2d tag15LeftCoordinates = new Translation2d(4.72, 5.70); + public static final Translation2d tag15RightCoordinates = new Translation2d(3.60, 5.05); + public static final Rotation2d tag15AngleHeading = Rotation2d.fromDegrees(120); public static final Translation2d tag16Coordinates = new Translation2d(4.06, 2.89); - public static final double tag16LeftXCoordinate = 3.61; - public static final double tag16LeftYCoordinate = 3.16; - public static final double tagl6RightXCoordinate = 4.69; - public static final double tag16RightYCoordinate = 2.60; - public static final double tag16CenterXCoordinate = 4.06; - public static final double tag16CenterYCoordinate = 2.89; - public static final double tag16AngleHeading = -123.52; + public static final Translation2d tag16LeftCoordinates = new Translation2d(3.61, 3.16); + public static final Translation2d tag16RightCoordinates = new Translation2d(4.69, 2.60); + public static final Rotation2d tag16AngleHeading = Rotation2d.fromDegrees(-123.52); + + public static final Translation2d placeHolderCoordinates = new Translation2d(353, 353); + public static final Rotation2d placeHolderAngleHeading = Rotation2d.fromDegrees(353); } public static final class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bea4e2c..7e33b6b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,9 +30,6 @@ import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.VisionConstants; import frc.robot.commands.TeleopSwerve; -import frc.robot.commands.AutoClimb.AutoClimbCenter; -import frc.robot.commands.AutoClimb.AutoClimbLeft; -import frc.robot.commands.AutoClimb.AutoClimbRight; import frc.robot.commands.arm.ArmHold; import frc.robot.commands.arm.AutoShoot; import frc.robot.subsystems.Arm; @@ -217,7 +214,7 @@ private void configureDriveBindings() { 180.0)); } })); - + driverController .leftTrigger() .whileTrue( @@ -247,20 +244,47 @@ private void configureDriveBindings() { driverController .x() .whileTrue( - new AutoClimbLeft() - ); + new ProxyCommand( + AutoBuilder.pathfindToPose( + new Pose2d( + swerve.getLeftChainPose().getX(), + swerve.getLeftChainPose().getY(), + swerve.getAngleHeading()), + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + Units.degreesToRadians(180.0), + 180.0)))); driverController .y() .whileTrue( - new AutoClimbCenter() - ); - + new ProxyCommand( + AutoBuilder.pathfindToPose( + new Pose2d( + swerve.getCenterChainPose().getX(), + swerve.getCenterChainPose().getY(), + swerve.getAngleHeading()), + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + Units.degreesToRadians(180.0), + 180.0)))); + driverController .b() .whileTrue( - new AutoClimbRight() - ); + new ProxyCommand( + AutoBuilder.pathfindToPose( + new Pose2d( + swerve.getRightChainPose().getX(), + swerve.getRightChainPose().getY(), + swerve.getAngleHeading()), + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + Units.degreesToRadians(180.0), + 180.0)))); } private void configureIntakeBindings() { diff --git a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java deleted file mode 100644 index 04bec92..0000000 --- a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbCenter.java +++ /dev/null @@ -1,54 +0,0 @@ -// 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.commands.AutoClimb; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.SwerveConstants; -import frc.robot.subsystems.Climber; -import frc.robot.subsystems.Swerve; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoClimbCenter extends SequentialCommandGroup { - /** Creates a new AutoClimbCenter. */ - - Swerve swerve; - Climber climber; - - public AutoClimbCenter() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - - double[] coordinates = swerve.getCenterChainPose(); - double xCoordinates = coordinates[0]; - double yCoordinates = coordinates[1]; - Rotation2d angleHeading = Rotation2d.fromDegrees(coordinates[2]); - - addCommands( - Commands.runOnce( - () -> climber.setClimberUp()), - - AutoBuilder.pathfindToPose( - new Pose2d( - xCoordinates, - yCoordinates, - angleHeading), - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - Units.degreesToRadians(180.0), 180.0))); - - Commands.runOnce( - () -> climber.setClimberDown()); - } -} diff --git a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java deleted file mode 100644 index 39e484d..0000000 --- a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbLeft.java +++ /dev/null @@ -1,53 +0,0 @@ -// 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.commands.AutoClimb; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.SwerveConstants; -import frc.robot.subsystems.Climber; -import frc.robot.subsystems.Swerve; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoClimbLeft extends SequentialCommandGroup { - /** Creates a new AutoClimbLeft. */ - Swerve swerve; - Climber climber; - - public AutoClimbLeft() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - - double[] coordinates = swerve.getLeftChainPose(); - double xCoordinates = coordinates[0]; - double yCoordinates = coordinates[1]; - Rotation2d angleHeading = Rotation2d.fromDegrees(coordinates[2]); - - addCommands( - Commands.runOnce( - () -> climber.setClimberUp()), - - AutoBuilder.pathfindToPose( - new Pose2d( - xCoordinates, - yCoordinates, - angleHeading), - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - Units.degreesToRadians(180.0), 180.0))); - - Commands.runOnce( - () -> climber.setClimberDown()); - } -} diff --git a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java b/src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java deleted file mode 100644 index 58cb10e..0000000 --- a/src/main/java/frc/robot/commands/AutoClimb/AutoClimbRight.java +++ /dev/null @@ -1,52 +0,0 @@ -// 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.commands.AutoClimb; - -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.SwerveConstants; -import frc.robot.subsystems.Climber; -import frc.robot.subsystems.Swerve; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html -public class AutoClimbRight extends SequentialCommandGroup { - /** Creates a new AutoClimbRight. */ - Swerve swerve; - Climber climber; - public AutoClimbRight() { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - - double[] coordinates = swerve.getRightChainPose(); - double xCoordinates = coordinates[0]; - double yCoordinates = coordinates[1]; - Rotation2d angleHeading = Rotation2d.fromDegrees(coordinates[2]); - - addCommands( - Commands.runOnce( - () -> climber.setClimberUp()), - - AutoBuilder.pathfindToPose( - new Pose2d( - xCoordinates, - yCoordinates, - angleHeading), - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - Units.degreesToRadians(180.0), 180.0))); - - Commands.runOnce( - () -> climber.setClimberDown()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index c0fe40d..27596d9 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -43,7 +43,7 @@ public void climb() { public void stopClimberMotors() { mainMotor.set(0.0); - }n + } @Override public void periodic() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index c5eeea9..41928ec 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -12,7 +12,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import com.revrobotics.RelativeEncoder; import frc.lib.controllers.VirtualJoystick; import frc.lib.controllers.VirtualXboxController; import frc.lib.subsystem.VirtualSubsystem; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 9feb9dd..353455c 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -54,7 +54,7 @@ public Shooter() { SparkMaxUtil.configureFollower(shooterFollower); shooterPID.setP(ShooterConstants.shooterP); } - b + public void setMotorSpeed(double velocity) { double feedForward = shooterFeedforward.calculate(velocity); shooterPID.setReference(velocity, ControlType.kVelocity, 0, feedForward, ArbFFUnits.kVoltage); diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index e2eb079..ed0e417 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -617,171 +617,121 @@ private void updatePhotonVisionPoses() { } public Translation2d getNearestChain() { - List chainCoordinates = List.of(StationCoordinateConstants.tag11Coordinates, - StationCoordinateConstants.tag12Coordinates, - StationCoordinateConstants.tag13Coordinates, - StationCoordinateConstants.tag14Coordinates, - StationCoordinateConstants.tag15Coordinates, - StationCoordinateConstants.tag16Coordinates); - Translation2d nearestChainPose = new Translation2d(); - - return nearestChainPose.nearest(chainCoordinates); + return nearestChainPose.nearest(StationCoordinateConstants.chainCoordinates); } - public double[] getCenterChainPose() { - double[] coordinates = new double[2]; - + public Rotation2d getAngleHeading() { if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - coordinates[0] = StationCoordinateConstants.tag11CenterXCoordinate; - coordinates[1] = StationCoordinateConstants.tag11CenterYCoordinate; - coordinates[2] = StationCoordinateConstants.tag11AngleHeading; - return coordinates; + return StationCoordinateConstants.tag11AngleHeading; } if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - coordinates[0] = StationCoordinateConstants.tag12CenterXCoordinate; - coordinates[1] = StationCoordinateConstants.tag12CenterYCoordinate; - coordinates[2] = StationCoordinateConstants.tag12AngleHeading; - return coordinates; + return StationCoordinateConstants.tag12AngleHeading; } if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - coordinates[0] = StationCoordinateConstants.tag13CenterXCoordinate; - coordinates[1] = StationCoordinateConstants.tag13CenterYCoordinate; - coordinates[2] = StationCoordinateConstants.tag13AngleHeading; - return coordinates; + return StationCoordinateConstants.tag13AngleHeading; } if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - coordinates[0] = StationCoordinateConstants.tag14CenterXCoordinate; - coordinates[1] = StationCoordinateConstants.tag14CenterYCoordinate; - coordinates[2] = StationCoordinateConstants.tag14AngleHeading; - return coordinates; + return StationCoordinateConstants.tag14AngleHeading; } if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - coordinates[0] = StationCoordinateConstants.tag15CenterXCoordinate; - coordinates[1] = StationCoordinateConstants.tag15CenterYCoordinate; - coordinates[2] = StationCoordinateConstants.tag15AngleHeading; - return coordinates; + return StationCoordinateConstants.tag15AngleHeading; } if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - coordinates[0] = StationCoordinateConstants.tag16CenterXCoordinate; - coordinates[1] = StationCoordinateConstants.tag16CenterYCoordinate; - coordinates[2] = StationCoordinateConstants.tag16AngleHeading; - return coordinates; + return StationCoordinateConstants.tag16AngleHeading; } else { - coordinates[0] = 1000; - coordinates[1] = 1000; - coordinates[2] = 1000; - return coordinates; + return StationCoordinateConstants.placeHolderAngleHeading; } } - public double[] getRightChainPose() { - double[] coordinates = new double[2]; - + public Translation2d getCenterChainPose() { if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - coordinates[0] = StationCoordinateConstants.tag11RightXCoordinate; - coordinates[1] = StationCoordinateConstants.tag11RightYCoordinate; - coordinates[2] = StationCoordinateConstants.tag11AngleHeading; - return coordinates; + return StationCoordinateConstants.tag11Coordinates; } if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - coordinates[0] = StationCoordinateConstants.tag12RightXCoordinate; - coordinates[1] = StationCoordinateConstants.tag12RightYCoordinate; - coordinates[2] = StationCoordinateConstants.tag12AngleHeading; - return coordinates; + return StationCoordinateConstants.tag12Coordinates; } if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - coordinates[0] = StationCoordinateConstants.tag13RightXCoordinate; - coordinates[1] = StationCoordinateConstants.tag13RightYCoordinate; - coordinates[2] = StationCoordinateConstants.tag13AngleHeading; - return coordinates; + return StationCoordinateConstants.tag13Coordinates; } if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - coordinates[0] = StationCoordinateConstants.tag14RightXCoordinate; - coordinates[1] = StationCoordinateConstants.tag14RightYCoordinate; - coordinates[2] = StationCoordinateConstants.tag14AngleHeading; - return coordinates; + return StationCoordinateConstants.tag14Coordinates; } if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - coordinates[0] = StationCoordinateConstants.tag15RightXCoordinate; - coordinates[1] = StationCoordinateConstants.tag15RightYCoordinate; - coordinates[2] = StationCoordinateConstants.tag15AngleHeading; - return coordinates; + return StationCoordinateConstants.tag15Coordinates; } if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - coordinates[0] = StationCoordinateConstants.tagl6RightXCoordinate; - coordinates[1] = StationCoordinateConstants.tag16RightYCoordinate; - coordinates[2] = StationCoordinateConstants.tag16AngleHeading; - return coordinates; + return StationCoordinateConstants.tag16Coordinates; } else { - coordinates[0] = 1000; - coordinates[1] = 1000; - coordinates[2] = 1000; - return coordinates; + return StationCoordinateConstants.placeHolderCoordinates; } } - public double[] getLeftChainPose() { - double[] coordinates = new double[2]; + public Translation2d getRightChainPose() { + if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { + return StationCoordinateConstants.tag11RightCoordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { + return StationCoordinateConstants.tag12RightCoordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { + return StationCoordinateConstants.tag13RightCoordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { + return StationCoordinateConstants.tag14RightCoordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { + return StationCoordinateConstants.tag15RightCoordinates; + } + + if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { + return StationCoordinateConstants.tag16RightCoordinates; + } else { + return StationCoordinateConstants.placeHolderCoordinates; + } + } + public Translation2d getLeftChainPose() { if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - coordinates[0] = StationCoordinateConstants.tag11LeftXCoordinate; - coordinates[1] = StationCoordinateConstants.tag11LeftYCoordinate; - coordinates[2] = StationCoordinateConstants.tag11AngleHeading; - return coordinates; + return StationCoordinateConstants.tag11LeftCoordinates; } if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - coordinates[0] = StationCoordinateConstants.tag12LeftXCoordinate; - coordinates[1] = StationCoordinateConstants.tag12LeftYCoordinate; - coordinates[2] = StationCoordinateConstants.tag12AngleHeading; - return coordinates; + return StationCoordinateConstants.tag12LeftCoordinates; } if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - coordinates[0] = StationCoordinateConstants.tag13LeftXCoordinate; - coordinates[1] = StationCoordinateConstants.tag13LeftYCoordinate; - coordinates[2] = StationCoordinateConstants.tag13AngleHeading; - return coordinates; + return StationCoordinateConstants.tag13LeftCoordinates; } if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - coordinates[0] = StationCoordinateConstants.tag14LeftXCoordinate; - coordinates[1] = StationCoordinateConstants.tag14LeftYCoordinate; - coordinates[2] = StationCoordinateConstants.tag14AngleHeading; - return coordinates; + return StationCoordinateConstants.tag14LeftCoordinates; } if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - coordinates[0] = StationCoordinateConstants.tag15LeftXCoordinate; - coordinates[1] = StationCoordinateConstants.tag15LeftYCoordinate; - coordinates[2] = StationCoordinateConstants.tag15AngleHeading; - return coordinates; + return StationCoordinateConstants.tag15LeftCoordinates; } if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - coordinates[0] = StationCoordinateConstants.tag16LeftXCoordinate; - coordinates[1] = StationCoordinateConstants.tag16LeftYCoordinate; - coordinates[2] = StationCoordinateConstants.tag16AngleHeading; - return coordinates; - } else{ - coordinates[0] = 1000; - coordinates[1] = 1000; - coordinates[2] = 1000; - return coordinates; + return StationCoordinateConstants.tag16LeftCoordinates; + } else { + return StationCoordinateConstants.placeHolderCoordinates; } } - public void updateVisionPoseEstimates() { detectedTargets.clear(); From f3811a8d34fac13f29c86d587e347e350c510243 Mon Sep 17 00:00:00 2001 From: NandaGuntupalli <70027620+NandaGuntupalli@users.noreply.github.com> Date: Thu, 25 Jan 2024 17:36:23 -0500 Subject: [PATCH 3/5] Updated constants to be Pose2d --- src/main/java/frc/robot/Constants.java | 74 +++++++++---------- src/main/java/frc/robot/RobotContainer.java | 6 +- .../java/frc/robot/subsystems/Swerve.java | 44 ++--------- 3 files changed, 45 insertions(+), 79 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b9d1e60..2351ded 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -68,46 +68,40 @@ public static final class FieldConstants { } public static final class StationCoordinateConstants { - public static final List chainCoordinates = List.of( - StationCoordinateConstants.tag11Coordinates, - StationCoordinateConstants.tag12Coordinates, - StationCoordinateConstants.tag13Coordinates, - StationCoordinateConstants.tag14Coordinates, - StationCoordinateConstants.tag15Coordinates, - StationCoordinateConstants.tag16Coordinates); - - public static final Translation2d tag11Coordinates = new Translation2d(12.41, 2.87); - public static final Translation2d tag11LeftCoordinates = new Translation2d(11.89, 2.59); - public static final Translation2d tag11RightCoordinates = new Translation2d(12.94, 3.18); - public static final Rotation2d tag11AngleHeading = Rotation2d.fromDegrees(-59.30); - - public static final Translation2d tag12Coordinates = new Translation2d(12.39, 5.32); - public static final Translation2d tag12LeftCoordinates = new Translation2d(13.07, 5); - public static final Translation2d tag12RightCoordinates = new Translation2d(11.98, 5.6); - public static final Rotation2d tag12AngleHeading = Rotation2d.fromDegrees(60); - - public static final Translation2d tag13Coordinates = new Translation2d(10.23, 4.06); - public static final Translation2d tag13LeftCoordinates = new Translation2d(10.27, 4.69); - public static final Translation2d tag13RightCoordinates = new Translation2d(10.27, 3.41); - public static final Rotation2d tag13AngleHeading = Rotation2d.fromDegrees(-180); - - public static final Translation2d tag14Coordinates = new Translation2d(6.32, 3.99); - public static final Translation2d tag14LeftCoordinates = new Translation2d(6.31, 3.57); - public static final Translation2d tag14RightCoordinates = new Translation2d(6.31, 4.63); - public static final Rotation2d tag14AngleHeading = Rotation2d.fromDegrees(-2.01); - - public static final Translation2d tag15Coordinates = new Translation2d(4.14, 5.29); - public static final Translation2d tag15LeftCoordinates = new Translation2d(4.72, 5.70); - public static final Translation2d tag15RightCoordinates = new Translation2d(3.60, 5.05); - public static final Rotation2d tag15AngleHeading = Rotation2d.fromDegrees(120); - - public static final Translation2d tag16Coordinates = new Translation2d(4.06, 2.89); - public static final Translation2d tag16LeftCoordinates = new Translation2d(3.61, 3.16); - public static final Translation2d tag16RightCoordinates = new Translation2d(4.69, 2.60); - public static final Rotation2d tag16AngleHeading = Rotation2d.fromDegrees(-123.52); - - public static final Translation2d placeHolderCoordinates = new Translation2d(353, 353); - public static final Rotation2d placeHolderAngleHeading = Rotation2d.fromDegrees(353); + + public static final Pose2d tag11Coordinates = new Pose2d(new Translation2d(12.41, 2.87), new Rotation2d(-59.30)); + public static final Pose2d tag11LeftCoordinates = new Pose2d(new Translation2d(11.89, 2.59), new Rotation2d(-59.30)); + public static final Pose2d tag11RightCoordinates = new Pose2d(new Translation2d(12.94, 3.18), new Rotation2d(-59.30)); + + public static final Pose2d tag12Coordinates = new Pose2d(new Translation2d(12.39, 5.32), new Rotation2d(60)); + public static final Pose2d tag12LeftCoordinates = new Pose2d(new Translation2d(13.07, 5), new Rotation2d(60)); + public static final Pose2d tag12RightCoordinates = new Pose2d(new Translation2d(11.98, 5.6), new Rotation2d(60)); + + public static final Pose2d tag13Coordinates = new Pose2d(new Translation2d(10.23, 4.06), new Rotation2d(-180)); + public static final Pose2d tag13LeftCoordinates = new Pose2d(new Translation2d(10.27, 4.69), new Rotation2d(-180)); + public static final Pose2d tag13RightCoordinates = new Pose2d(new Translation2d(10.27, 3.41), new Rotation2d(-180)); + + public static final Pose2d tag14Coordinates = new Pose2d(new Translation2d(6.32, 3.99), new Rotation2d(-2.01)); + public static final Pose2d tag14LeftCoordinates = new Pose2d(new Translation2d(6.31, 3.57), new Rotation2d(-2.01)); + public static final Pose2d tag14RightCoordinates = new Pose2d(new Translation2d(6.31, 4.63), new Rotation2d(-2.01)); + + public static final Pose2d tag15Coordinates = new Pose2d(new Translation2d(4.14, 5.29), new Rotation2d(120)); + public static final Pose2d tag15LeftCoordinates = new Pose2d(new Translation2d(4.72, 5.70), new Rotation2d(120)); + public static final Pose2d tag15RightCoordinates = new Pose2d(new Translation2d(3.60, 5.05), new Rotation2d(120)); + + public static final Pose2d tag16Coordinates = new Pose2d(new Translation2d(4.06, 2.89), new Rotation2d(-123.52)); + public static final Pose2d tag16LeftCoordinates = new Pose2d(new Translation2d(3.61, 3.16), new Rotation2d(-123.52)); + public static final Pose2d tag16RightCoordinates = new Pose2d(new Translation2d(4.69, 2.60), new Rotation2d(-123.52)); + + public static final Pose2d placeHolderPose = new Pose2d(new Translation2d(353, 353), new Rotation2d(353)); + + public static final List chainCoordinates = List.of( + tag11Coordinates, + tag12Coordinates, + tag13Coordinates, + tag14Coordinates, + tag15Coordinates, + tag16Coordinates); } public static final class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59eca95..06a3ca0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -268,7 +268,7 @@ private void configureDriveBindings() { new Pose2d( swerve.getLeftChainPose().getX(), swerve.getLeftChainPose().getY(), - swerve.getAngleHeading()), + swerve.getLeftChainPose().getRotation()), new PathConstraints( SwerveConstants.maxTranslationalSpeed, SwerveConstants.maxTranslationalAcceleration, @@ -283,7 +283,7 @@ private void configureDriveBindings() { new Pose2d( swerve.getCenterChainPose().getX(), swerve.getCenterChainPose().getY(), - swerve.getAngleHeading()), + swerve.getCenterChainPose().getRotation()), new PathConstraints( SwerveConstants.maxTranslationalSpeed, SwerveConstants.maxTranslationalAcceleration, @@ -298,7 +298,7 @@ private void configureDriveBindings() { new Pose2d( swerve.getRightChainPose().getX(), swerve.getRightChainPose().getY(), - swerve.getAngleHeading()), + swerve.getRightChainPose().getRotation()), new PathConstraints( SwerveConstants.maxTranslationalSpeed, SwerveConstants.maxTranslationalAcceleration, diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index ed0e417..82bebbb 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -616,40 +616,12 @@ private void updatePhotonVisionPoses() { } } - public Translation2d getNearestChain() { - Translation2d nearestChainPose = new Translation2d(); + public Pose2d getNearestChain() { + Pose2d nearestChainPose = new Pose2d(); return nearestChainPose.nearest(StationCoordinateConstants.chainCoordinates); } - public Rotation2d getAngleHeading() { - if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - return StationCoordinateConstants.tag11AngleHeading; - } - - if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - return StationCoordinateConstants.tag12AngleHeading; - } - - if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - return StationCoordinateConstants.tag13AngleHeading; - } - - if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - return StationCoordinateConstants.tag14AngleHeading; - } - - if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - return StationCoordinateConstants.tag15AngleHeading; - } - - if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - return StationCoordinateConstants.tag16AngleHeading; - } else { - return StationCoordinateConstants.placeHolderAngleHeading; - } - } - - public Translation2d getCenterChainPose() { + public Pose2d getCenterChainPose() { if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { return StationCoordinateConstants.tag11Coordinates; } @@ -673,11 +645,11 @@ public Translation2d getCenterChainPose() { if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { return StationCoordinateConstants.tag16Coordinates; } else { - return StationCoordinateConstants.placeHolderCoordinates; + return StationCoordinateConstants.placeHolderPose; } } - public Translation2d getRightChainPose() { + public Pose2d getRightChainPose() { if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { return StationCoordinateConstants.tag11RightCoordinates; } @@ -701,11 +673,11 @@ public Translation2d getRightChainPose() { if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { return StationCoordinateConstants.tag16RightCoordinates; } else { - return StationCoordinateConstants.placeHolderCoordinates; + return StationCoordinateConstants.placeHolderPose; } } - public Translation2d getLeftChainPose() { + public Pose2d getLeftChainPose() { if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { return StationCoordinateConstants.tag11LeftCoordinates; } @@ -729,7 +701,7 @@ public Translation2d getLeftChainPose() { if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { return StationCoordinateConstants.tag16LeftCoordinates; } else { - return StationCoordinateConstants.placeHolderCoordinates; + return StationCoordinateConstants.placeHolderPose; } } From ac09b5ef6a52988f907796b863a90af5305cba27 Mon Sep 17 00:00:00 2001 From: NandaGuntupalli <70027620+NandaGuntupalli@users.noreply.github.com> Date: Thu, 25 Jan 2024 18:58:38 -0500 Subject: [PATCH 4/5] Updated Naming --- src/main/java/frc/robot/Constants.java | 89 ++++++++++++------- src/main/java/frc/robot/RobotContainer.java | 64 +++++++------ .../java/frc/robot/subsystems/Swerve.java | 84 +++++++++-------- 3 files changed, 133 insertions(+), 104 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 2351ded..10857a6 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -69,39 +69,62 @@ public static final class FieldConstants { public static final class StationCoordinateConstants { - public static final Pose2d tag11Coordinates = new Pose2d(new Translation2d(12.41, 2.87), new Rotation2d(-59.30)); - public static final Pose2d tag11LeftCoordinates = new Pose2d(new Translation2d(11.89, 2.59), new Rotation2d(-59.30)); - public static final Pose2d tag11RightCoordinates = new Pose2d(new Translation2d(12.94, 3.18), new Rotation2d(-59.30)); - - public static final Pose2d tag12Coordinates = new Pose2d(new Translation2d(12.39, 5.32), new Rotation2d(60)); - public static final Pose2d tag12LeftCoordinates = new Pose2d(new Translation2d(13.07, 5), new Rotation2d(60)); - public static final Pose2d tag12RightCoordinates = new Pose2d(new Translation2d(11.98, 5.6), new Rotation2d(60)); - - public static final Pose2d tag13Coordinates = new Pose2d(new Translation2d(10.23, 4.06), new Rotation2d(-180)); - public static final Pose2d tag13LeftCoordinates = new Pose2d(new Translation2d(10.27, 4.69), new Rotation2d(-180)); - public static final Pose2d tag13RightCoordinates = new Pose2d(new Translation2d(10.27, 3.41), new Rotation2d(-180)); - - public static final Pose2d tag14Coordinates = new Pose2d(new Translation2d(6.32, 3.99), new Rotation2d(-2.01)); - public static final Pose2d tag14LeftCoordinates = new Pose2d(new Translation2d(6.31, 3.57), new Rotation2d(-2.01)); - public static final Pose2d tag14RightCoordinates = new Pose2d(new Translation2d(6.31, 4.63), new Rotation2d(-2.01)); - - public static final Pose2d tag15Coordinates = new Pose2d(new Translation2d(4.14, 5.29), new Rotation2d(120)); - public static final Pose2d tag15LeftCoordinates = new Pose2d(new Translation2d(4.72, 5.70), new Rotation2d(120)); - public static final Pose2d tag15RightCoordinates = new Pose2d(new Translation2d(3.60, 5.05), new Rotation2d(120)); - - public static final Pose2d tag16Coordinates = new Pose2d(new Translation2d(4.06, 2.89), new Rotation2d(-123.52)); - public static final Pose2d tag16LeftCoordinates = new Pose2d(new Translation2d(3.61, 3.16), new Rotation2d(-123.52)); - public static final Pose2d tag16RightCoordinates = new Pose2d(new Translation2d(4.69, 2.60), new Rotation2d(-123.52)); - - public static final Pose2d placeHolderPose = new Pose2d(new Translation2d(353, 353), new Rotation2d(353)); - - public static final List chainCoordinates = List.of( - tag11Coordinates, - tag12Coordinates, - tag13Coordinates, - tag14Coordinates, - tag15Coordinates, - tag16Coordinates); + public static final class CenterChainPoses { + public static final Pose2d bottomRightChainRedStage = + new Pose2d(new Translation2d(12.41, 2.87), new Rotation2d(-59.30)); + public static final Pose2d topRightChainRedStage = + new Pose2d(new Translation2d(12.39, 5.32), new Rotation2d(60)); + public static final Pose2d leftChainRedStage = + new Pose2d(new Translation2d(10.23, 4.06), new Rotation2d(-180)); + public static final Pose2d rightChainBlueStage = + new Pose2d(new Translation2d(6.32, 3.99), new Rotation2d(-2.01)); + public static final Pose2d topLeftChainBlueStage = + new Pose2d(new Translation2d(4.14, 5.29), new Rotation2d(120)); + public static final Pose2d bottomLeftChainBlueStage = + new Pose2d(new Translation2d(4.06, 2.89), new Rotation2d(-123.52)); + } + + public static final class LeftChainPoses { + public static final Pose2d bottomRightChainRedStage = + new Pose2d(new Translation2d(11.89, 2.59), new Rotation2d(-59.30)); + public static final Pose2d topRightChainRedStage = + new Pose2d(new Translation2d(13.07, 5), new Rotation2d(60)); + public static final Pose2d leftChainRedStage = + new Pose2d(new Translation2d(10.27, 4.69), new Rotation2d(-180)); + public static final Pose2d rightChainBlueStage = + new Pose2d(new Translation2d(6.31, 3.57), new Rotation2d(-2.01)); + public static final Pose2d topLeftChainBlueStage = + new Pose2d(new Translation2d(4.72, 5.70), new Rotation2d(120)); + public static final Pose2d bottomLeftChainBlueStage = + new Pose2d(new Translation2d(3.61, 3.16), new Rotation2d(-123.52)); + } + + public static final class RightChainPoses { + public static final Pose2d bottomRightChainRedStage = + new Pose2d(new Translation2d(12.94, 3.18), new Rotation2d(-59.30)); + public static final Pose2d topRightChainRedStage = + new Pose2d(new Translation2d(11.98, 5.6), new Rotation2d(60)); + public static final Pose2d leftChainRedStage = + new Pose2d(new Translation2d(10.27, 3.41), new Rotation2d(-180)); + public static final Pose2d rightChainBlueStage = + new Pose2d(new Translation2d(6.31, 4.63), new Rotation2d(-2.01)); + public static final Pose2d topLeftChainBlueStage = + new Pose2d(new Translation2d(3.60, 5.05), new Rotation2d(120)); + public static final Pose2d bottomLeftChainBlueStage = + new Pose2d(new Translation2d(4.69, 2.60), new Rotation2d(-123.52)); + } + + public static final Pose2d placeHolderPose = + new Pose2d(new Translation2d(353, 353), new Rotation2d(353)); + + public static final List chainCoordinates = + List.of( + CenterChainPoses.bottomRightChainRedStage, + CenterChainPoses.topRightChainRedStage, + CenterChainPoses.leftChainRedStage, + CenterChainPoses.rightChainBlueStage, + CenterChainPoses.topLeftChainBlueStage, + CenterChainPoses.bottomLeftChainBlueStage); } public static final class VisionConstants { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 06a3ca0..aba3270 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,6 +67,10 @@ public class RobotContainer implements Logged { private Climber climber = new Climber(); private LEDs leds = new LEDs(); + private Pose2d leftChainPose = swerve.getLeftChainPose(); + private Pose2d rightChainPose = swerve.getRightChainPose(); + private Pose2d centerChainPose = swerve.getCenterChainPose(); + // Replace with CommandPS4Controller or CommandJoystick if needed private final VirtualXboxController driverController = new VirtualXboxController(OperatorConstants.driverControllerPort); @@ -233,7 +237,7 @@ private void configureDriveBindings() { 180.0)); } })); - + driverController .leftTrigger() .whileTrue( @@ -264,46 +268,40 @@ private void configureDriveBindings() { .x() .whileTrue( new ProxyCommand( - AutoBuilder.pathfindToPose( - new Pose2d( - swerve.getLeftChainPose().getX(), - swerve.getLeftChainPose().getY(), - swerve.getLeftChainPose().getRotation()), - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - Units.degreesToRadians(180.0), - 180.0)))); - + () -> + AutoBuilder.pathfindToPose( + leftChainPose, + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + SwerveConstants.maxAngularSpeed, + SwerveConstants.maxAngularAcceleration)))); + driverController .y() .whileTrue( new ProxyCommand( - AutoBuilder.pathfindToPose( - new Pose2d( - swerve.getCenterChainPose().getX(), - swerve.getCenterChainPose().getY(), - swerve.getCenterChainPose().getRotation()), - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - Units.degreesToRadians(180.0), - 180.0)))); - + () -> + AutoBuilder.pathfindToPose( + centerChainPose, + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + SwerveConstants.maxAngularSpeed, + SwerveConstants.maxAngularAcceleration)))); + driverController .b() .whileTrue( new ProxyCommand( - AutoBuilder.pathfindToPose( - new Pose2d( - swerve.getRightChainPose().getX(), - swerve.getRightChainPose().getY(), - swerve.getRightChainPose().getRotation()), - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - Units.degreesToRadians(180.0), - 180.0)))); + () -> + AutoBuilder.pathfindToPose( + rightChainPose, + new PathConstraints( + SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + SwerveConstants.maxAngularSpeed, + SwerveConstants.maxAngularAcceleration)))); } private void configureIntakeBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index 82bebbb..e2b9a18 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -50,6 +50,9 @@ import frc.robot.Constants.FrontLeftModule; import frc.robot.Constants.FrontRightModule; import frc.robot.Constants.StationCoordinateConstants; +import frc.robot.Constants.StationCoordinateConstants.CenterChainPoses; +import frc.robot.Constants.StationCoordinateConstants.LeftChainPoses; +import frc.robot.Constants.StationCoordinateConstants.RightChainPoses; import frc.robot.Constants.SwerveConstants; import frc.robot.Constants.VisionConstants; import frc.robot.Constants.VisionConstants.ArducamConstants; @@ -617,89 +620,94 @@ private void updatePhotonVisionPoses() { } public Pose2d getNearestChain() { - Pose2d nearestChainPose = new Pose2d(); - return nearestChainPose.nearest(StationCoordinateConstants.chainCoordinates); + return getPose().nearest(StationCoordinateConstants.chainCoordinates); } public Pose2d getCenterChainPose() { - if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - return StationCoordinateConstants.tag11Coordinates; + Pose2d nearestChain = getNearestChain(); + + if (nearestChain == CenterChainPoses.bottomRightChainRedStage) { + return CenterChainPoses.bottomRightChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - return StationCoordinateConstants.tag12Coordinates; + if (nearestChain == CenterChainPoses.topRightChainRedStage) { + return CenterChainPoses.topRightChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - return StationCoordinateConstants.tag13Coordinates; + if (nearestChain == CenterChainPoses.leftChainRedStage) { + return CenterChainPoses.leftChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - return StationCoordinateConstants.tag14Coordinates; + if (nearestChain == CenterChainPoses.rightChainBlueStage) { + return CenterChainPoses.rightChainBlueStage; } - if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - return StationCoordinateConstants.tag15Coordinates; + if (nearestChain == CenterChainPoses.topLeftChainBlueStage) { + return CenterChainPoses.topLeftChainBlueStage; } - if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - return StationCoordinateConstants.tag16Coordinates; + if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) { + return CenterChainPoses.bottomLeftChainBlueStage; } else { return StationCoordinateConstants.placeHolderPose; } } public Pose2d getRightChainPose() { - if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - return StationCoordinateConstants.tag11RightCoordinates; + Pose2d nearestChain = getNearestChain(); + + if (nearestChain == CenterChainPoses.bottomRightChainRedStage) { + return RightChainPoses.bottomRightChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - return StationCoordinateConstants.tag12RightCoordinates; + if (nearestChain == CenterChainPoses.topRightChainRedStage) { + return RightChainPoses.topRightChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - return StationCoordinateConstants.tag13RightCoordinates; + if (nearestChain == CenterChainPoses.leftChainRedStage) { + return RightChainPoses.leftChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - return StationCoordinateConstants.tag14RightCoordinates; + if (nearestChain == CenterChainPoses.rightChainBlueStage) { + return RightChainPoses.rightChainBlueStage; } - if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - return StationCoordinateConstants.tag15RightCoordinates; + if (nearestChain == CenterChainPoses.topLeftChainBlueStage) { + return RightChainPoses.topLeftChainBlueStage; } - if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - return StationCoordinateConstants.tag16RightCoordinates; + if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) { + return RightChainPoses.bottomLeftChainBlueStage; } else { return StationCoordinateConstants.placeHolderPose; } } public Pose2d getLeftChainPose() { - if (getNearestChain() == StationCoordinateConstants.tag11Coordinates) { - return StationCoordinateConstants.tag11LeftCoordinates; + Pose2d nearestChain = getNearestChain(); + + if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) { + return LeftChainPoses.bottomRightChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag12Coordinates) { - return StationCoordinateConstants.tag12LeftCoordinates; + if (nearestChain == CenterChainPoses.topRightChainRedStage) { + return LeftChainPoses.topRightChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag13Coordinates) { - return StationCoordinateConstants.tag13LeftCoordinates; + if (nearestChain == CenterChainPoses.leftChainRedStage) { + return LeftChainPoses.leftChainRedStage; } - if (getNearestChain() == StationCoordinateConstants.tag14Coordinates) { - return StationCoordinateConstants.tag14LeftCoordinates; + if (nearestChain == CenterChainPoses.rightChainBlueStage) { + return LeftChainPoses.rightChainBlueStage; } - if (getNearestChain() == StationCoordinateConstants.tag15Coordinates) { - return StationCoordinateConstants.tag15LeftCoordinates; + if (nearestChain == CenterChainPoses.topLeftChainBlueStage) { + return LeftChainPoses.topLeftChainBlueStage; } - if (getNearestChain() == StationCoordinateConstants.tag16Coordinates) { - return StationCoordinateConstants.tag16LeftCoordinates; + if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) { + return LeftChainPoses.bottomLeftChainBlueStage; } else { return StationCoordinateConstants.placeHolderPose; } From a27f0cf351fab9e88d52c60664dc222d95058436 Mon Sep 17 00:00:00 2001 From: NandaGuntupalli <70027620+NandaGuntupalli@users.noreply.github.com> Date: Fri, 26 Jan 2024 15:30:08 -0500 Subject: [PATCH 5/5] Updated Center Chain Returns and RobotContainer Pose & pathConstraints --- src/main/java/frc/robot/RobotContainer.java | 32 +++++++------------ .../java/frc/robot/subsystems/Swerve.java | 12 +++---- 2 files changed, 17 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index aba3270..f2fa44c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -67,9 +67,11 @@ public class RobotContainer implements Logged { private Climber climber = new Climber(); private LEDs leds = new LEDs(); - private Pose2d leftChainPose = swerve.getLeftChainPose(); - private Pose2d rightChainPose = swerve.getRightChainPose(); - private Pose2d centerChainPose = swerve.getCenterChainPose(); + private PathConstraints pathConstraints = + new PathConstraints(SwerveConstants.maxTranslationalSpeed, + SwerveConstants.maxTranslationalAcceleration, + SwerveConstants.maxAngularSpeed, + SwerveConstants.maxAngularAcceleration); // Replace with CommandPS4Controller or CommandJoystick if needed private final VirtualXboxController driverController = @@ -270,12 +272,8 @@ private void configureDriveBindings() { new ProxyCommand( () -> AutoBuilder.pathfindToPose( - leftChainPose, - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - SwerveConstants.maxAngularSpeed, - SwerveConstants.maxAngularAcceleration)))); + swerve.getLeftChainPose(), + pathConstraints))); driverController .y() @@ -283,12 +281,8 @@ private void configureDriveBindings() { new ProxyCommand( () -> AutoBuilder.pathfindToPose( - centerChainPose, - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - SwerveConstants.maxAngularSpeed, - SwerveConstants.maxAngularAcceleration)))); + swerve.getCenterChainPose(), + pathConstraints))); driverController .b() @@ -296,12 +290,8 @@ private void configureDriveBindings() { new ProxyCommand( () -> AutoBuilder.pathfindToPose( - rightChainPose, - new PathConstraints( - SwerveConstants.maxTranslationalSpeed, - SwerveConstants.maxTranslationalAcceleration, - SwerveConstants.maxAngularSpeed, - SwerveConstants.maxAngularAcceleration)))); + swerve.getRightChainPose(), + pathConstraints))); } private void configureIntakeBindings() { diff --git a/src/main/java/frc/robot/subsystems/Swerve.java b/src/main/java/frc/robot/subsystems/Swerve.java index e2b9a18..f4574a7 100644 --- a/src/main/java/frc/robot/subsystems/Swerve.java +++ b/src/main/java/frc/robot/subsystems/Swerve.java @@ -627,27 +627,27 @@ public Pose2d getCenterChainPose() { Pose2d nearestChain = getNearestChain(); if (nearestChain == CenterChainPoses.bottomRightChainRedStage) { - return CenterChainPoses.bottomRightChainRedStage; + return nearestChain; } if (nearestChain == CenterChainPoses.topRightChainRedStage) { - return CenterChainPoses.topRightChainRedStage; + return nearestChain; } if (nearestChain == CenterChainPoses.leftChainRedStage) { - return CenterChainPoses.leftChainRedStage; + return nearestChain; } if (nearestChain == CenterChainPoses.rightChainBlueStage) { - return CenterChainPoses.rightChainBlueStage; + return nearestChain; } if (nearestChain == CenterChainPoses.topLeftChainBlueStage) { - return CenterChainPoses.topLeftChainBlueStage; + return nearestChain; } if (nearestChain == CenterChainPoses.bottomLeftChainBlueStage) { - return CenterChainPoses.bottomLeftChainBlueStage; + return nearestChain; } else { return StationCoordinateConstants.placeHolderPose; }