From cf9b835da699b7e84eae9fa26cff146f7fa3f6dc Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Tue, 11 Feb 2025 17:52:54 -0500 Subject: [PATCH 01/18] Small integrations. --- src/main/java/frc/robot/DriverJoystick.java | 3 + .../java/frc/robot/autons/FollowPath.java | 108 ++++++++ .../frc/robot/commands/DriveToWaypoint.java | 238 ++++++++++++++++++ .../frc/robot/constants/AutoConstants.java | 2 + .../java/frc/robot/subsystems/Dashboard.java | 9 +- .../java/frc/robot/subsystems/DriveTrain.java | 26 +- 6 files changed, 383 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/autons/FollowPath.java create mode 100644 src/main/java/frc/robot/commands/DriveToWaypoint.java diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index 773cf6d..d5150c1 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -1,5 +1,7 @@ package frc.robot; +import frc.robot.autons.FollowPath; +import frc.robot.commands.DriveToWaypoint; import frc.robot.constants.DriveConstants; import frc.robot.constants.IOConstants; import frc.robot.libraries.XboxController1038; @@ -82,6 +84,7 @@ private DriverJoystick() { // Lock the wheels into an X formation super.xButton.whileTrue(this.driveTrain.setX()); + super.aButton.whileTrue(new FollowPath(FollowPath.Position.TEST)); } /** diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java new file mode 100644 index 0000000..f70dab9 --- /dev/null +++ b/src/main/java/frc/robot/autons/FollowPath.java @@ -0,0 +1,108 @@ +package frc.robot.autons; + +import java.util.Optional; +import java.util.function.BiConsumer; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.controllers.PathFollowingController; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.IdealStartingState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.commands.DriveToWaypoint; +import frc.robot.constants.AutoConstants; +import frc.robot.constants.DriveConstants; +import frc.robot.subsystems.Dashboard; +import frc.robot.subsystems.DriveTrain; + +public class FollowPath extends Auton { + private final DriveTrain driveTrain = DriveTrain.getInstance(); + private final Dashboard dashboard = Dashboard.getInstance(); + + public enum Position { + TEST(3.165, 3.947, new Rotation2d(0)); + + private final double x; + private final double y; + private final Rotation2d rotation; + + private Position(double x, double y, Rotation2d rotation) { + this.x = x; + this.y = y; + this.rotation = rotation; + } + + public Pose2d getPose() { + return new Pose2d(x, y, rotation); + } + } + + private Pose2d startingPose; + private final PathConstraints constraints = new PathConstraints( + DriveConstants.MaxSpeed, + AutoConstants.kMaxAccelerationMetersPerSecondSquared, + AutoConstants.kMaxAngularSpeedRadiansPerSecond, + AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared); + private final IdealStartingState idealStartingState = new IdealStartingState( + driveTrain.getState().Speeds.vxMetersPerSecond, + driveTrain.getState().Pose.getRotation()); + private final GoalEndState goalEndState = new GoalEndState(0, Rotation2d.kZero); + + public FollowPath(Position position) { + super(Optional.empty()); + + Pose2d endingPose = position.getPose(); + PathPlannerPath path = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(endingPose, endingPose), + this.constraints, + this.idealStartingState, this.goalEndState); + + PathPlannerLogging.setLogActivePathCallback((activePath) -> { + if (activePath.size() >= 2) { + PathPlannerPath dashboardPath = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(activePath), + constraints, + idealStartingState, goalEndState); + this.dashboard.addPath(dashboardPath); + } + }); + + PathFollowingController driveController = new PPHolonomicDriveController( + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, + 0.0), + new PIDConstants(AutoConstants.kPThetaController, + AutoConstants.kIThetaController, 0.0)); + super.addCommands( + AutoBuilder.pathfindToPose(endingPose, this.constraints), + new DriveToWaypoint( + endingPose, + this.constraints, + this.idealStartingState, + this.goalEndState, + () -> this.driveTrain.getState().Pose, + () -> this.driveTrain.getState().Speeds, + (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> { + this.driveTrain.setControl( + new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()) + ); + }, + new PPHolonomicDriveController( + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, AutoConstants.kDController), + new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, AutoConstants.kDThetaController)), + AutoConstants.kRobotConfig.get(), + () -> DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red, + this.driveTrain)); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveToWaypoint.java b/src/main/java/frc/robot/commands/DriveToWaypoint.java new file mode 100644 index 0000000..fcf55cf --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveToWaypoint.java @@ -0,0 +1,238 @@ +package frc.robot.commands; + +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.config.ModuleConfig; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.controllers.PathFollowingController; +import com.pathplanner.lib.events.EventScheduler; +import com.pathplanner.lib.path.*; +import com.pathplanner.lib.trajectory.PathPlannerTrajectory; +import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.PPLibTelemetry; +import com.pathplanner.lib.util.PathPlannerLogging; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.Subsystem; +import java.util.*; +import java.util.function.BiConsumer; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +/** Base command for following a path */ +public class DriveToWaypoint extends Command { + private final Timer timer = new Timer(); + private final Supplier poseSupplier; + private final Supplier speedsSupplier; + private final BiConsumer output; + private final PathFollowingController controller; + private final RobotConfig robotConfig; + private final BooleanSupplier shouldFlipPath; + private final EventScheduler eventScheduler; + + private PathPlannerPath path; + private final Pose2d endingPose; + private final PathConstraints constraints; + private final IdealStartingState idealStartingState; + private final GoalEndState goalEndState; + private PathPlannerTrajectory trajectory; + + /** + * Construct a base path following command + * + * @param endingPose The pose to go to + * @param poseSupplier Function that supplies the current field-relative pose + * of the robot + * @param speedsSupplier Function that supplies the current robot-relative + * chassis speeds + * @param output Output function that accepts robot-relative + * ChassisSpeeds and feedforwards for + * each drive motor. If using swerve, these feedforwards + * will be in FL, FR, BL, BR order. If + * using a differential drive, they will be in L, R order. + *

+ * NOTE: These feedforwards are assuming unoptimized + * module states. When you optimize your + * module states, you will need to reverse the + * feedforwards for modules that have been flipped + * @param controller Path following controller that will be used to follow + * the path + * @param robotConfig The robot configuration + * @param shouldFlipPath Should the path be flipped to the other side of the + * field? This will + * maintain a global blue alliance origin. + * @param requirements Subsystems required by this command, usually just the + * drive subsystem + */ + public DriveToWaypoint( + Pose2d endingPose, + PathConstraints constraints, + IdealStartingState idealStartingState, + GoalEndState goalEndState, + Supplier poseSupplier, + Supplier speedsSupplier, + BiConsumer output, + PathFollowingController controller, + RobotConfig robotConfig, + BooleanSupplier shouldFlipPath, + Subsystem... requirements) { + this.endingPose = endingPose; + this.constraints = constraints; + this.idealStartingState = idealStartingState; + this.goalEndState = goalEndState; + this.poseSupplier = poseSupplier; + this.speedsSupplier = speedsSupplier; + this.output = output; + this.controller = controller; + this.robotConfig = robotConfig; + this.shouldFlipPath = shouldFlipPath; + this.eventScheduler = new EventScheduler(); + + addRequirements(requirements); + + } + + @Override + public void initialize() { + Pose2d currentPose = poseSupplier.get(); + List poses = Arrays.asList(currentPose, this.endingPose); + this.path = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(poses), this.constraints, + this.idealStartingState, + this.goalEndState); + + Optional idealTrajectory = this.path.getIdealTrajectory(this.robotConfig); + idealTrajectory.ifPresent(traj -> this.trajectory = traj); + + if (shouldFlipPath.getAsBoolean() && !path.preventFlipping) { + path = path.flipPath(); + } + ChassisSpeeds currentSpeeds = speedsSupplier.get(); + + controller.reset(currentPose, currentSpeeds); + + double linearVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + + if (path.getIdealStartingState() != null) { + // Check if we match the ideal starting state + boolean idealVelocity = Math.abs(linearVel - path.getIdealStartingState().velocityMPS()) <= 0.25; + boolean idealRotation = !robotConfig.isHolonomic + || Math.abs( + currentPose + .getRotation() + .minus(path.getIdealStartingState().rotation()) + .getDegrees()) <= 30.0; + if (idealVelocity && idealRotation) { + // We can use the ideal trajectory + trajectory = path.getIdealTrajectory(robotConfig).orElseThrow(); + } else { + // We need to regenerate + trajectory = path.generateTrajectory(currentSpeeds, currentPose.getRotation(), robotConfig); + } + } else { + // No ideal starting state, generate the trajectory + trajectory = path.generateTrajectory(currentSpeeds, currentPose.getRotation(), robotConfig); + } + + PathPlannerAuto.setCurrentTrajectory(trajectory); + PathPlannerAuto.currentPathName = path.name; + + PathPlannerLogging.logActivePath(path); + PPLibTelemetry.setCurrentPath(path); + + eventScheduler.initialize(trajectory); + + timer.reset(); + timer.start(); + } + + @Override + public void execute() { + double currentTime = timer.get(); + PathPlannerTrajectoryState targetState = trajectory.sample(currentTime); + if (!controller.isHolonomic() && path.isReversed()) { + targetState = targetState.reverse(); + } + + Pose2d currentPose = poseSupplier.get(); + ChassisSpeeds currentSpeeds = speedsSupplier.get(); + + ChassisSpeeds targetSpeeds = controller.calculateRobotRelativeSpeeds(currentPose, targetState); + + double currentVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); + + PPLibTelemetry.setCurrentPose(currentPose); + PathPlannerLogging.logCurrentPose(currentPose); + + PPLibTelemetry.setTargetPose(targetState.pose); + PathPlannerLogging.logTargetPose(targetState.pose); + + PPLibTelemetry.setVelocities( + currentVel, + targetState.linearVelocity, + currentSpeeds.omegaRadiansPerSecond, + targetSpeeds.omegaRadiansPerSecond); + + output.accept(targetSpeeds, targetState.feedforwards); + + eventScheduler.execute(currentTime); + } + + @Override + public boolean isFinished() { + return timer.hasElapsed(trajectory.getTotalTimeSeconds()); + } + + @Override + public void end(boolean interrupted) { + timer.stop(); + PathPlannerAuto.currentPathName = ""; + PathPlannerAuto.setCurrentTrajectory(null); + + // Only output 0 speeds when ending a path that is supposed to stop, this allows + // interrupting + // the command to smoothly transition into some auto-alignment routine + if (!interrupted && path.getGoalEndState().velocityMPS() < 0.1) { + output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); + } + + PathPlannerLogging.logActivePath(null); + + eventScheduler.end(); + } + + /** + * Create a command to warmup on-the-fly generation, re-planning, and the path + * following command + * + * @return Path following warmup command + */ + public static Command warmupCommand() { + return new DriveToWaypoint( + new Pose2d(0.0, 0.0, Rotation2d.kZero), + new PathConstraints(4.0, 4.0, 4.0, 4.0), + new IdealStartingState(0.0, Rotation2d.kZero), + new GoalEndState(0.0, Rotation2d.kCW_90deg), + () -> Pose2d.kZero, + ChassisSpeeds::new, + (speeds, feedforwards) -> { + }, + new PPHolonomicDriveController( + new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), + new RobotConfig( + 75, + 6.8, + new ModuleConfig( + 0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0, 1), + 0.55), + () -> true) + .andThen(Commands.print("[PathPlanner] FollowPoseCommand finished warmup")) + .ignoringDisable(true); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index bcc8fb4..8da73b5 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -13,8 +13,10 @@ public final class AutoConstants { public static final double kPXController = 1.0; public static final double kIXController = 0.35; + public static final double kDController = 0.0; public static final double kPThetaController = 1.0; public static final double kIThetaController = 0.05; + public static final double kDThetaController = 0.0; // Constraint for the motion profiled robot angle controller public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints( diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index 190e433..df033d7 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -13,13 +13,14 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.autons.AutonSelector.AutonChoices; +import com.pathplanner.lib.path.PathPlannerPath; public class Dashboard extends SubsystemBase { // Inputs - private DriveTrain driveTrain = DriveTrain.getInstance(); + private final DriveTrain driveTrain = DriveTrain.getInstance(); // Choosers - private SendableChooser autoChooser = new SendableChooser<>(); + private final SendableChooser autoChooser = new SendableChooser<>(); private SendableChooser delayChooser = new SendableChooser<>(); // Tabs @@ -100,6 +101,10 @@ public void periodic() { field.setRobotPose(driveTrain.getState().Pose); } + public void addPath(PathPlannerPath path) { + this.field.getObject("poses").setPoses(path.getPathPoses()); + } + /** * Puts the given {@link Trajectory} on the dashboard * diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index caaea61..435eb05 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -12,9 +12,14 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.DriveFeedforwards; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj.DriverStation; @@ -24,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.SwerveConstants; @@ -142,6 +148,24 @@ private DriveTrain() { SwerveConstants.FrontRight, SwerveConstants.BackLeft, SwerveConstants.FrontRight); + AutoBuilder.configure( + () -> this.getState().Pose, + this::resetPose, + () -> this.getState().Speeds, + (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> { + this.setControl( + new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()) + ); + }, + new PPHolonomicDriveController( + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, AutoConstants.kDController), + new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, AutoConstants.kDThetaController)), + AutoConstants.kRobotConfig.get(), + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this + ); if (Utils.isSimulation()) { startSimThread(); } @@ -181,7 +205,7 @@ public Command setX() { * Returns a command that applies the specified control request to this swerve * drivetrain. * - * @param request Function returning the request to apply + * @param requestSupplier Function returning the request to apply * @return Command to run */ public Command applyRequest(Supplier requestSupplier) { From 2f5cf9e16cc770ae239b5f24784b8c4b8f7c19ed Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Tue, 11 Feb 2025 18:23:47 -0500 Subject: [PATCH 02/18] Remove method from Auton since AutoBuilder is neater --- src/main/java/frc/robot/autons/Auton.java | 32 +---------------------- 1 file changed, 1 insertion(+), 31 deletions(-) diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java index 7e9f6b2..39ce5ed 100644 --- a/src/main/java/frc/robot/autons/Auton.java +++ b/src/main/java/frc/robot/autons/Auton.java @@ -3,6 +3,7 @@ import java.util.Optional; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -60,35 +61,4 @@ protected void setInitialPose(PathPlannerTrajectory initialTrajectory, Rotation2 public Pose2d getInitialPose() { return initialPose; } - - public Command followPathCommand(PathPlannerPath path) { - try { - if (!AutoConstants.kRobotConfig.isPresent()) { - throw new Error("PP Robot Config is Missing"); - } - return new FollowPathCommand( - path, - // Robot pose supplier - () -> this.driveTrain.getState().Pose, - // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE - () -> this.driveTrain.getState().Speeds, - // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds - (speeds, feedforwards) -> this.driveTrain.setControl( - new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), - new PPHolonomicDriveController( - // Translation PIDconstants - new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, 0.0), - // Rotation PID constants - new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, 0.0)), - AutoConstants.kRobotConfig.get(), - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this.driveTrain // Reference to this subsystem to set requirements - ); - } catch (Exception e) { - DriverStation.reportError("Failed to create auton: " + e.getMessage(), e.getStackTrace()); - return Commands.none(); - } - } } \ No newline at end of file From 9b21ef700e510c9f8e92a24df583382f5ed6709c Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Tue, 11 Feb 2025 19:15:47 -0500 Subject: [PATCH 03/18] Add configure --- .../java/frc/robot/autons/FollowPath.java | 25 +--- .../frc/robot/commands/DriveToWaypoint.java | 109 +++++------------- .../java/frc/robot/subsystems/DriveTrain.java | 18 +++ 3 files changed, 50 insertions(+), 102 deletions(-) diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java index f70dab9..b50e17e 100644 --- a/src/main/java/frc/robot/autons/FollowPath.java +++ b/src/main/java/frc/robot/autons/FollowPath.java @@ -64,9 +64,6 @@ public FollowPath(Position position) { super(Optional.empty()); Pose2d endingPose = position.getPose(); - PathPlannerPath path = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(endingPose, endingPose), - this.constraints, - this.idealStartingState, this.goalEndState); PathPlannerLogging.setLogActivePathCallback((activePath) -> { if (activePath.size() >= 2) { @@ -77,32 +74,12 @@ public FollowPath(Position position) { } }); - PathFollowingController driveController = new PPHolonomicDriveController( - new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, - 0.0), - new PIDConstants(AutoConstants.kPThetaController, - AutoConstants.kIThetaController, 0.0)); super.addCommands( AutoBuilder.pathfindToPose(endingPose, this.constraints), new DriveToWaypoint( endingPose, this.constraints, this.idealStartingState, - this.goalEndState, - () -> this.driveTrain.getState().Pose, - () -> this.driveTrain.getState().Speeds, - (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> { - this.driveTrain.setControl( - new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()) - ); - }, - new PPHolonomicDriveController( - new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, AutoConstants.kDController), - new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, AutoConstants.kDThetaController)), - AutoConstants.kRobotConfig.get(), - () -> DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red, - this.driveTrain)); + this.goalEndState)); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveToWaypoint.java b/src/main/java/frc/robot/commands/DriveToWaypoint.java index fcf55cf..3834c8e 100644 --- a/src/main/java/frc/robot/commands/DriveToWaypoint.java +++ b/src/main/java/frc/robot/commands/DriveToWaypoint.java @@ -1,10 +1,7 @@ package frc.robot.commands; import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.config.ModuleConfig; -import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; import com.pathplanner.lib.controllers.PathFollowingController; import com.pathplanner.lib.events.EventScheduler; import com.pathplanner.lib.path.*; @@ -14,27 +11,28 @@ import com.pathplanner.lib.util.PPLibTelemetry; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; + import java.util.*; import java.util.function.BiConsumer; import java.util.function.BooleanSupplier; import java.util.function.Supplier; -/** Base command for following a path */ +/** + * Base command for following a path + */ public class DriveToWaypoint extends Command { private final Timer timer = new Timer(); - private final Supplier poseSupplier; - private final Supplier speedsSupplier; - private final BiConsumer output; - private final PathFollowingController controller; - private final RobotConfig robotConfig; - private final BooleanSupplier shouldFlipPath; + private static Supplier poseSupplier; + private static Supplier speedsSupplier; + private static BiConsumer output; + private static PathFollowingController controller; + private static RobotConfig robotConfig; + private static BooleanSupplier shouldFlipPath; + private static Subsystem[] requirements; private final EventScheduler eventScheduler; private PathPlannerPath path; @@ -47,52 +45,20 @@ public class DriveToWaypoint extends Command { /** * Construct a base path following command * - * @param endingPose The pose to go to - * @param poseSupplier Function that supplies the current field-relative pose - * of the robot - * @param speedsSupplier Function that supplies the current robot-relative - * chassis speeds - * @param output Output function that accepts robot-relative - * ChassisSpeeds and feedforwards for - * each drive motor. If using swerve, these feedforwards - * will be in FL, FR, BL, BR order. If - * using a differential drive, they will be in L, R order. - *

- * NOTE: These feedforwards are assuming unoptimized - * module states. When you optimize your - * module states, you will need to reverse the - * feedforwards for modules that have been flipped - * @param controller Path following controller that will be used to follow - * the path - * @param robotConfig The robot configuration - * @param shouldFlipPath Should the path be flipped to the other side of the - * field? This will - * maintain a global blue alliance origin. - * @param requirements Subsystems required by this command, usually just the - * drive subsystem + * @param endingPose The pose to go to + * @param constraints The constraints of the path + * @param idealStartingState The ideal starting state of the path + * @param goalEndState The ending state of the path */ public DriveToWaypoint( Pose2d endingPose, PathConstraints constraints, IdealStartingState idealStartingState, - GoalEndState goalEndState, - Supplier poseSupplier, - Supplier speedsSupplier, - BiConsumer output, - PathFollowingController controller, - RobotConfig robotConfig, - BooleanSupplier shouldFlipPath, - Subsystem... requirements) { + GoalEndState goalEndState) { this.endingPose = endingPose; this.constraints = constraints; this.idealStartingState = idealStartingState; this.goalEndState = goalEndState; - this.poseSupplier = poseSupplier; - this.speedsSupplier = speedsSupplier; - this.output = output; - this.controller = controller; - this.robotConfig = robotConfig; - this.shouldFlipPath = shouldFlipPath; this.eventScheduler = new EventScheduler(); addRequirements(requirements); @@ -107,7 +73,7 @@ public void initialize() { this.idealStartingState, this.goalEndState); - Optional idealTrajectory = this.path.getIdealTrajectory(this.robotConfig); + Optional idealTrajectory = this.path.getIdealTrajectory(DriveToWaypoint.robotConfig); idealTrajectory.ifPresent(traj -> this.trajectory = traj); if (shouldFlipPath.getAsBoolean() && !path.preventFlipping) { @@ -207,32 +173,19 @@ public void end(boolean interrupted) { eventScheduler.end(); } - /** - * Create a command to warmup on-the-fly generation, re-planning, and the path - * following command - * - * @return Path following warmup command - */ - public static Command warmupCommand() { - return new DriveToWaypoint( - new Pose2d(0.0, 0.0, Rotation2d.kZero), - new PathConstraints(4.0, 4.0, 4.0, 4.0), - new IdealStartingState(0.0, Rotation2d.kZero), - new GoalEndState(0.0, Rotation2d.kCW_90deg), - () -> Pose2d.kZero, - ChassisSpeeds::new, - (speeds, feedforwards) -> { - }, - new PPHolonomicDriveController( - new PIDConstants(5.0, 0.0, 0.0), new PIDConstants(5.0, 0.0, 0.0)), - new RobotConfig( - 75, - 6.8, - new ModuleConfig( - 0.048, 5.0, 1.2, DCMotor.getKrakenX60(1).withReduction(6.14), 60.0, 1), - 0.55), - () -> true) - .andThen(Commands.print("[PathPlanner] FollowPoseCommand finished warmup")) - .ignoringDisable(true); + public static void configure(Supplier poseSupplier, + Supplier speedsSupplier, + BiConsumer output, + PathFollowingController controller, + RobotConfig robotConfig, + BooleanSupplier shouldFlipPath, + Subsystem... requirements) { + DriveToWaypoint.poseSupplier = poseSupplier; + DriveToWaypoint.speedsSupplier = speedsSupplier; + DriveToWaypoint.output = output; + DriveToWaypoint.controller = controller; + DriveToWaypoint.robotConfig = robotConfig; + DriveToWaypoint.shouldFlipPath = shouldFlipPath; + DriveToWaypoint.requirements = requirements; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 435eb05..094ad71 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.DriveToWaypoint; import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.SwerveConstants; @@ -166,6 +167,23 @@ private DriveTrain() { () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this ); + DriveToWaypoint.configure( + () -> this.getState().Pose, + () -> this.getState().Speeds, + (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> { + this.setControl( + new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()) + ); + }, + new PPHolonomicDriveController( + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, AutoConstants.kDController), + new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, AutoConstants.kDThetaController)), + AutoConstants.kRobotConfig.get(), + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this + ); if (Utils.isSimulation()) { startSimThread(); } From 7f511a92745aac5538a9d07ef75acc348719e496 Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Tue, 11 Feb 2025 19:35:45 -0500 Subject: [PATCH 04/18] Add vision --- .../frc/robot/constants/VisionConstants.java | 16 ++ .../java/frc/robot/subsystems/Vision.java | 153 ++++++++++++++++++ 2 files changed, 169 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/Vision.java diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 148184f..be169d2 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -1,5 +1,15 @@ package frc.robot.constants; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; + public final class VisionConstants { public static final String kTableName = "Vision"; public static final String kValuesTopic = "values"; @@ -20,4 +30,10 @@ public final class VisionConstants { public static final double spinD = 0.0002; public static final double spinSetpoint = 0.0; public static final double aprilTagArea = 28908; + + public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); + public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0.1, 0.0, 0.1), + new Rotation3d(0, 0, 0)); + public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); + public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java new file mode 100644 index 0000000..937bcc9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -0,0 +1,153 @@ +/* + * MIT License + * + * Copyright (c) PhotonVision + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package frc.robot.subsystems; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.VisionConstants; + +import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonTrackedTarget; + +public class Vision extends SubsystemBase { + private final PhotonCamera frontCam; + private final PhotonCamera backCam; + private final PhotonPoseEstimator photonEstimator; + private Matrix curStdDevs; + + private static Vision instance; + + public static Vision getInstance() { + if (instance == null) { + instance = new Vision(); + } + return instance; + } + + private Vision() { + frontCam = new PhotonCamera("frontCam"); + backCam = new PhotonCamera("backCam"); + + photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + VisionConstants.kRobotToCam); + photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS); + } + + /** + * The latest estimated robot pose on the field from vision data. This may be + * empty. This should + * only be called once per loop. + * + * @return An {@link EstimatedRobotPose} with an estimated pose, estimate + * timestamp, and targets + * used for estimation. + */ + public Optional getEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + + for (var change : frontCam.getAllUnreadResults()) { + visionEst = photonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + } + return visionEst; + } + + /** + * Calculates new standard deviations This algorithm is a heuristic that creates + * dynamic standard + * deviations based on number of tags, estimation strategy, and distance from + * the tags. + * + * @param estimatedPose The estimated pose to guess standard deviations for. + * @param targets All targets in this camera frame + */ + private void updateEstimationStdDevs( + Optional estimatedPose, List targets) { + if (estimatedPose.isEmpty()) { + // No pose input. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + + } else { + // Pose present. Start running Heuristic + var estStdDevs = VisionConstants.kSingleTagStdDevs; + int numTags = 0; + double avgDist = 0; + + // Precalculation - see how many tags we found, and calculate an + // average-distance metric + for (var tgt : targets) { + var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + if (tagPose.isEmpty()) + continue; + numTags++; + avgDist += tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance(estimatedPose.get().estimatedPose.toPose2d().getTranslation()); + } + + if (numTags == 0) { + // No tags visible. Default to single-tag std devs + curStdDevs = VisionConstants.kSingleTagStdDevs; + } else { + // One or more tags visible, run the full heuristic. + avgDist /= numTags; + // Decrease std devs if multiple targets are visible + if (numTags > 1) + estStdDevs = VisionConstants.kMultiTagStdDevs; + // Increase std devs based on (average) distance + if (numTags == 1 && avgDist > 4) + estStdDevs = VecBuilder.fill(Double.MAX_VALUE, Double.MAX_VALUE, Double.MAX_VALUE); + else + estStdDevs = estStdDevs.times(1 + (avgDist * avgDist / 30)); + curStdDevs = estStdDevs; + } + } + } + + /** + * Returns the latest standard deviations of the estimated pose from {@link + * #getEstimatedGlobalPose()}, for use with {@link + * edu.wpi.first.math.estimator.SwerveDrivePoseEstimator + * SwerveDrivePoseEstimator}. This should + * only be used when there are targets visible. + */ + public Matrix getEstimationStdDevs() { + return curStdDevs; + } + + // public int getBestTarget() { + // return frontCam.getAllUnreadResults().get(1).getBestTarget().getFiducialId(); + // } +} \ No newline at end of file From 6a898a8121a132b9f343734c08739b5b25e9fbac Mon Sep 17 00:00:00 2001 From: Griffin Date: Wed, 12 Feb 2025 17:55:48 -0500 Subject: [PATCH 05/18] changed to 800x600 camera width/height --- src/main/java/frc/robot/constants/VisionConstants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index be169d2..8cf0ebd 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -18,8 +18,8 @@ public final class VisionConstants { public static final String kEnabled0Topic = "enable0"; public static final String kEnabled1Topic = "enable1"; - public static final double width = 1280; - public static final double height = 720; + public static final double width = 800; + public static final double height = 600; public static final double fov = 100; public static final double driveP = 0.005; From 4758ca90347a899f14961b1fb22e88867002ada3 Mon Sep 17 00:00:00 2001 From: Rachit Gupta Date: Wed, 12 Feb 2025 18:11:00 -0500 Subject: [PATCH 06/18] Adding enum --- src/main/java/frc/robot/autons/AutonSelector.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/autons/AutonSelector.java b/src/main/java/frc/robot/autons/AutonSelector.java index 5f63edd..47a3129 100644 --- a/src/main/java/frc/robot/autons/AutonSelector.java +++ b/src/main/java/frc/robot/autons/AutonSelector.java @@ -32,7 +32,8 @@ private AutonSelector() { this.autoChooser = Dashboard.getInstance().getAutoChooser(); this.autoChooser.setDefaultOption("No Auto", AutonChoices.NoAuto); - // this.autoChooser.addOption("Score 2 In Amp Position 1", AutonChoices.AmpAuto); + // this.autoChooser.addOption("Score 2 In Amp Position 1", + // AutonChoices.AmpAuto); this.delayChooser = Dashboard.getInstance().getDelayChooser(); @@ -57,7 +58,7 @@ public Auton chooseAuton() { Optional alliance = DriverStation.getAlliance(); switch (this.autoChooser.getSelected()) { // case AmpAuto: - // return new ScoreInAmp(alliance); + // return new ScoreInAmp(alliance); default: return null; } From 61264e863ea813d7f41aa74d62222a9528ffd765 Mon Sep 17 00:00:00 2001 From: Rachit Gupta Date: Wed, 12 Feb 2025 18:12:21 -0500 Subject: [PATCH 07/18] Working on DriveToWaypoints Enum --- .../java/frc/robot/autons/FollowPath.java | 3 +- .../frc/robot/commands/DriveToWaypoint.java | 77 +++++++++++++++---- 2 files changed, 63 insertions(+), 17 deletions(-) diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java index b50e17e..d5b1275 100644 --- a/src/main/java/frc/robot/autons/FollowPath.java +++ b/src/main/java/frc/robot/autons/FollowPath.java @@ -22,6 +22,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import frc.robot.commands.DriveToWaypoint; +import frc.robot.commands.DriveToWaypoint.DriveWaypoints; import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.Dashboard; @@ -77,7 +78,7 @@ public FollowPath(Position position) { super.addCommands( AutoBuilder.pathfindToPose(endingPose, this.constraints), new DriveToWaypoint( - endingPose, + DriveWaypoints.Algae23, this.constraints, this.idealStartingState, this.goalEndState)); diff --git a/src/main/java/frc/robot/commands/DriveToWaypoint.java b/src/main/java/frc/robot/commands/DriveToWaypoint.java index 3834c8e..a55465b 100644 --- a/src/main/java/frc/robot/commands/DriveToWaypoint.java +++ b/src/main/java/frc/robot/commands/DriveToWaypoint.java @@ -11,6 +11,8 @@ import com.pathplanner.lib.util.PPLibTelemetry; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -36,26 +38,69 @@ public class DriveToWaypoint extends Command { private final EventScheduler eventScheduler; private PathPlannerPath path; - private final Pose2d endingPose; + private final DriveWaypoints driveWaypoint; private final PathConstraints constraints; private final IdealStartingState idealStartingState; private final GoalEndState goalEndState; private PathPlannerTrajectory trajectory; + public enum DriveWaypoints { + LeftCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftoCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Algae23(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Algae34(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Processor(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))); + + private Pose2d pose; + + private DriveWaypoints(Pose2d pose) { + this.pose = pose; + } + + public Pose2d getPose() { + return pose; + } + + } + /** * Construct a base path following command * - * @param endingPose The pose to go to + * @param driveWaypoint The waypoint to reach * @param constraints The constraints of the path * @param idealStartingState The ideal starting state of the path * @param goalEndState The ending state of the path */ public DriveToWaypoint( - Pose2d endingPose, + DriveWaypoints driveWaypoint, PathConstraints constraints, IdealStartingState idealStartingState, GoalEndState goalEndState) { - this.endingPose = endingPose; + this.driveWaypoint = driveWaypoint; this.constraints = constraints; this.idealStartingState = idealStartingState; this.goalEndState = goalEndState; @@ -68,7 +113,7 @@ public DriveToWaypoint( @Override public void initialize() { Pose2d currentPose = poseSupplier.get(); - List poses = Arrays.asList(currentPose, this.endingPose); + List poses = Arrays.asList(currentPose, this.driveWaypoint.getPose()); this.path = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(poses), this.constraints, this.idealStartingState, this.goalEndState); @@ -90,10 +135,10 @@ public void initialize() { boolean idealVelocity = Math.abs(linearVel - path.getIdealStartingState().velocityMPS()) <= 0.25; boolean idealRotation = !robotConfig.isHolonomic || Math.abs( - currentPose - .getRotation() - .minus(path.getIdealStartingState().rotation()) - .getDegrees()) <= 30.0; + currentPose + .getRotation() + .minus(path.getIdealStartingState().rotation()) + .getDegrees()) <= 30.0; if (idealVelocity && idealRotation) { // We can use the ideal trajectory trajectory = path.getIdealTrajectory(robotConfig).orElseThrow(); @@ -174,12 +219,12 @@ public void end(boolean interrupted) { } public static void configure(Supplier poseSupplier, - Supplier speedsSupplier, - BiConsumer output, - PathFollowingController controller, - RobotConfig robotConfig, - BooleanSupplier shouldFlipPath, - Subsystem... requirements) { + Supplier speedsSupplier, + BiConsumer output, + PathFollowingController controller, + RobotConfig robotConfig, + BooleanSupplier shouldFlipPath, + Subsystem... requirements) { DriveToWaypoint.poseSupplier = poseSupplier; DriveToWaypoint.speedsSupplier = speedsSupplier; DriveToWaypoint.output = output; @@ -188,4 +233,4 @@ public static void configure(Supplier poseSupplier, DriveToWaypoint.shouldFlipPath = shouldFlipPath; DriveToWaypoint.requirements = requirements; } -} \ No newline at end of file +} From f717fa86074897ac8ff03797341f56d148eb40fd Mon Sep 17 00:00:00 2001 From: Rachit Gupta Date: Wed, 12 Feb 2025 18:18:40 -0500 Subject: [PATCH 08/18] Renamed to DriveToWaypointCommand versus DriveToWaypoint --- src/main/java/frc/robot/DriverJoystick.java | 2 +- .../java/frc/robot/autons/FollowPath.java | 6 ++-- ...point.java => DriveToWaypointCommand.java} | 21 +++++++------- .../java/frc/robot/subsystems/DriveTrain.java | 28 +++++++++---------- 4 files changed, 29 insertions(+), 28 deletions(-) rename src/main/java/frc/robot/commands/{DriveToWaypoint.java => DriveToWaypointCommand.java} (94%) diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index d5150c1..fa1f4fa 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -1,7 +1,7 @@ package frc.robot; import frc.robot.autons.FollowPath; -import frc.robot.commands.DriveToWaypoint; +import frc.robot.commands.DriveToWaypointCommand; import frc.robot.constants.DriveConstants; import frc.robot.constants.IOConstants; import frc.robot.libraries.XboxController1038; diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java index d5b1275..33c324a 100644 --- a/src/main/java/frc/robot/autons/FollowPath.java +++ b/src/main/java/frc/robot/autons/FollowPath.java @@ -21,8 +21,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.commands.DriveToWaypoint; -import frc.robot.commands.DriveToWaypoint.DriveWaypoints; +import frc.robot.commands.DriveToWaypointCommand; +import frc.robot.commands.DriveToWaypointCommand.DriveWaypoints; import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.subsystems.Dashboard; @@ -77,7 +77,7 @@ public FollowPath(Position position) { super.addCommands( AutoBuilder.pathfindToPose(endingPose, this.constraints), - new DriveToWaypoint( + new DriveToWaypointCommand( DriveWaypoints.Algae23, this.constraints, this.idealStartingState, diff --git a/src/main/java/frc/robot/commands/DriveToWaypoint.java b/src/main/java/frc/robot/commands/DriveToWaypointCommand.java similarity index 94% rename from src/main/java/frc/robot/commands/DriveToWaypoint.java rename to src/main/java/frc/robot/commands/DriveToWaypointCommand.java index a55465b..2623d6b 100644 --- a/src/main/java/frc/robot/commands/DriveToWaypoint.java +++ b/src/main/java/frc/robot/commands/DriveToWaypointCommand.java @@ -26,7 +26,7 @@ /** * Base command for following a path */ -public class DriveToWaypoint extends Command { +public class DriveToWaypointCommand extends Command { private final Timer timer = new Timer(); private static Supplier poseSupplier; private static Supplier speedsSupplier; @@ -95,7 +95,7 @@ public Pose2d getPose() { * @param idealStartingState The ideal starting state of the path * @param goalEndState The ending state of the path */ - public DriveToWaypoint( + public DriveToWaypointCommand( DriveWaypoints driveWaypoint, PathConstraints constraints, IdealStartingState idealStartingState, @@ -118,7 +118,8 @@ public void initialize() { this.idealStartingState, this.goalEndState); - Optional idealTrajectory = this.path.getIdealTrajectory(DriveToWaypoint.robotConfig); + Optional idealTrajectory = this.path + .getIdealTrajectory(DriveToWaypointCommand.robotConfig); idealTrajectory.ifPresent(traj -> this.trajectory = traj); if (shouldFlipPath.getAsBoolean() && !path.preventFlipping) { @@ -225,12 +226,12 @@ public static void configure(Supplier poseSupplier, RobotConfig robotConfig, BooleanSupplier shouldFlipPath, Subsystem... requirements) { - DriveToWaypoint.poseSupplier = poseSupplier; - DriveToWaypoint.speedsSupplier = speedsSupplier; - DriveToWaypoint.output = output; - DriveToWaypoint.controller = controller; - DriveToWaypoint.robotConfig = robotConfig; - DriveToWaypoint.shouldFlipPath = shouldFlipPath; - DriveToWaypoint.requirements = requirements; + DriveToWaypointCommand.poseSupplier = poseSupplier; + DriveToWaypointCommand.speedsSupplier = speedsSupplier; + DriveToWaypointCommand.output = output; + DriveToWaypointCommand.controller = controller; + DriveToWaypointCommand.robotConfig = robotConfig; + DriveToWaypointCommand.shouldFlipPath = shouldFlipPath; + DriveToWaypointCommand.requirements = requirements; } } diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 094ad71..4883470 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -29,7 +29,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.commands.DriveToWaypoint; +import frc.robot.commands.DriveToWaypointCommand; import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.SwerveConstants; @@ -157,33 +157,33 @@ private DriveTrain() { this.setControl( new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()) - ); + .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons())); }, new PPHolonomicDriveController( - new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, AutoConstants.kDController), - new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, AutoConstants.kDThetaController)), + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, + AutoConstants.kDController), + new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, + AutoConstants.kDThetaController)), AutoConstants.kRobotConfig.get(), () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this - ); - DriveToWaypoint.configure( + this); + DriveToWaypointCommand.configure( () -> this.getState().Pose, () -> this.getState().Speeds, (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> { this.setControl( new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons()) - ); + .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons())); }, new PPHolonomicDriveController( - new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, AutoConstants.kDController), - new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, AutoConstants.kDThetaController)), + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, + AutoConstants.kDController), + new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, + AutoConstants.kDThetaController)), AutoConstants.kRobotConfig.get(), () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this - ); + this); if (Utils.isSimulation()) { startSimThread(); } From 0c1379a7b1528ef24d0f5ac40d45cdcff4bb5000 Mon Sep 17 00:00:00 2001 From: Wesley Reed <24629474+reediculous456@users.noreply.github.com> Date: Thu, 13 Feb 2025 16:46:32 -0500 Subject: [PATCH 09/18] Discard changes to src/main/java/frc/robot/DriverJoystick.java --- src/main/java/frc/robot/DriverJoystick.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index fa1f4fa..773cf6d 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -1,7 +1,5 @@ package frc.robot; -import frc.robot.autons.FollowPath; -import frc.robot.commands.DriveToWaypointCommand; import frc.robot.constants.DriveConstants; import frc.robot.constants.IOConstants; import frc.robot.libraries.XboxController1038; @@ -84,7 +82,6 @@ private DriverJoystick() { // Lock the wheels into an X formation super.xButton.whileTrue(this.driveTrain.setX()); - super.aButton.whileTrue(new FollowPath(FollowPath.Position.TEST)); } /** From 22e3cd009bda12dfcd16ec7bf92d49e9a6ed5af5 Mon Sep 17 00:00:00 2001 From: Wesley Reed <24629474+reediculous456@users.noreply.github.com> Date: Thu, 13 Feb 2025 16:46:45 -0500 Subject: [PATCH 10/18] Discard changes to src/main/java/frc/robot/autons/Auton.java --- src/main/java/frc/robot/autons/Auton.java | 32 ++++++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/autons/Auton.java b/src/main/java/frc/robot/autons/Auton.java index 39ce5ed..7e9f6b2 100644 --- a/src/main/java/frc/robot/autons/Auton.java +++ b/src/main/java/frc/robot/autons/Auton.java @@ -3,7 +3,6 @@ import java.util.Optional; import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.FollowPathCommand; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -61,4 +60,35 @@ protected void setInitialPose(PathPlannerTrajectory initialTrajectory, Rotation2 public Pose2d getInitialPose() { return initialPose; } + + public Command followPathCommand(PathPlannerPath path) { + try { + if (!AutoConstants.kRobotConfig.isPresent()) { + throw new Error("PP Robot Config is Missing"); + } + return new FollowPathCommand( + path, + // Robot pose supplier + () -> this.driveTrain.getState().Pose, + // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + () -> this.driveTrain.getState().Speeds, + // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds + (speeds, feedforwards) -> this.driveTrain.setControl( + new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + // Translation PIDconstants + new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, 0.0), + // Rotation PID constants + new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, 0.0)), + AutoConstants.kRobotConfig.get(), + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this.driveTrain // Reference to this subsystem to set requirements + ); + } catch (Exception e) { + DriverStation.reportError("Failed to create auton: " + e.getMessage(), e.getStackTrace()); + return Commands.none(); + } + } } \ No newline at end of file From 0212d8c75492df67714c25994d512d9da284f5ed Mon Sep 17 00:00:00 2001 From: Wesley Reed <24629474+reediculous456@users.noreply.github.com> Date: Thu, 13 Feb 2025 16:47:26 -0500 Subject: [PATCH 11/18] Discard changes to src/main/java/frc/robot/autons/AutonSelector.java --- src/main/java/frc/robot/autons/AutonSelector.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/autons/AutonSelector.java b/src/main/java/frc/robot/autons/AutonSelector.java index 47a3129..5f63edd 100644 --- a/src/main/java/frc/robot/autons/AutonSelector.java +++ b/src/main/java/frc/robot/autons/AutonSelector.java @@ -32,8 +32,7 @@ private AutonSelector() { this.autoChooser = Dashboard.getInstance().getAutoChooser(); this.autoChooser.setDefaultOption("No Auto", AutonChoices.NoAuto); - // this.autoChooser.addOption("Score 2 In Amp Position 1", - // AutonChoices.AmpAuto); + // this.autoChooser.addOption("Score 2 In Amp Position 1", AutonChoices.AmpAuto); this.delayChooser = Dashboard.getInstance().getDelayChooser(); @@ -58,7 +57,7 @@ public Auton chooseAuton() { Optional alliance = DriverStation.getAlliance(); switch (this.autoChooser.getSelected()) { // case AmpAuto: - // return new ScoreInAmp(alliance); + // return new ScoreInAmp(alliance); default: return null; } From bf4a85af569499a86ffd1a5d990a944fdc5e2a73 Mon Sep 17 00:00:00 2001 From: Rachit Gupta Date: Thu, 13 Feb 2025 18:20:43 -0500 Subject: [PATCH 12/18] Renamed enum variables --- .../frc/robot/commands/DriveToWaypointCommand.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/commands/DriveToWaypointCommand.java b/src/main/java/frc/robot/commands/DriveToWaypointCommand.java index 2623d6b..0c3a971 100644 --- a/src/main/java/frc/robot/commands/DriveToWaypointCommand.java +++ b/src/main/java/frc/robot/commands/DriveToWaypointCommand.java @@ -75,14 +75,14 @@ public enum DriveWaypoints { RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))); - private Pose2d pose; + private Pose2d endpoint; - private DriveWaypoints(Pose2d pose) { - this.pose = pose; + private DriveWaypoints(Pose2d endpoint) { + this.endpoint = endpoint; } - public Pose2d getPose() { - return pose; + public Pose2d getEndpoint() { + return endpoint; } } @@ -113,7 +113,7 @@ public DriveToWaypointCommand( @Override public void initialize() { Pose2d currentPose = poseSupplier.get(); - List poses = Arrays.asList(currentPose, this.driveWaypoint.getPose()); + List poses = Arrays.asList(currentPose, this.driveWaypoint.getEndpoint()); this.path = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(poses), this.constraints, this.idealStartingState, this.goalEndState); From 96f620cd149206afff7189bec047c52e82282d66 Mon Sep 17 00:00:00 2001 From: Wesley Reed <24629474+reediculous456@users.noreply.github.com> Date: Thu, 13 Feb 2025 18:39:10 -0500 Subject: [PATCH 13/18] Delete src/main/java/frc/robot/autons/FollowPath.java --- .../java/frc/robot/autons/FollowPath.java | 86 ------------------- 1 file changed, 86 deletions(-) delete mode 100644 src/main/java/frc/robot/autons/FollowPath.java diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java deleted file mode 100644 index 33c324a..0000000 --- a/src/main/java/frc/robot/autons/FollowPath.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.autons; - -import java.util.Optional; -import java.util.function.BiConsumer; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.controllers.PathFollowingController; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.IdealStartingState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.PathPlannerLogging; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.commands.DriveToWaypointCommand; -import frc.robot.commands.DriveToWaypointCommand.DriveWaypoints; -import frc.robot.constants.AutoConstants; -import frc.robot.constants.DriveConstants; -import frc.robot.subsystems.Dashboard; -import frc.robot.subsystems.DriveTrain; - -public class FollowPath extends Auton { - private final DriveTrain driveTrain = DriveTrain.getInstance(); - private final Dashboard dashboard = Dashboard.getInstance(); - - public enum Position { - TEST(3.165, 3.947, new Rotation2d(0)); - - private final double x; - private final double y; - private final Rotation2d rotation; - - private Position(double x, double y, Rotation2d rotation) { - this.x = x; - this.y = y; - this.rotation = rotation; - } - - public Pose2d getPose() { - return new Pose2d(x, y, rotation); - } - } - - private Pose2d startingPose; - private final PathConstraints constraints = new PathConstraints( - DriveConstants.MaxSpeed, - AutoConstants.kMaxAccelerationMetersPerSecondSquared, - AutoConstants.kMaxAngularSpeedRadiansPerSecond, - AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared); - private final IdealStartingState idealStartingState = new IdealStartingState( - driveTrain.getState().Speeds.vxMetersPerSecond, - driveTrain.getState().Pose.getRotation()); - private final GoalEndState goalEndState = new GoalEndState(0, Rotation2d.kZero); - - public FollowPath(Position position) { - super(Optional.empty()); - - Pose2d endingPose = position.getPose(); - - PathPlannerLogging.setLogActivePathCallback((activePath) -> { - if (activePath.size() >= 2) { - PathPlannerPath dashboardPath = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(activePath), - constraints, - idealStartingState, goalEndState); - this.dashboard.addPath(dashboardPath); - } - }); - - super.addCommands( - AutoBuilder.pathfindToPose(endingPose, this.constraints), - new DriveToWaypointCommand( - DriveWaypoints.Algae23, - this.constraints, - this.idealStartingState, - this.goalEndState)); - } -} \ No newline at end of file From 83540946e5e14735ccf2ec94177d17e0e196eb1a Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Thu, 13 Feb 2025 19:05:44 -0500 Subject: [PATCH 14/18] Revert "Delete src/main/java/frc/robot/autons/FollowPath.java" This reverts commit 96f620cd149206afff7189bec047c52e82282d66. --- .../java/frc/robot/autons/FollowPath.java | 86 +++++++++++++++++++ 1 file changed, 86 insertions(+) create mode 100644 src/main/java/frc/robot/autons/FollowPath.java diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java new file mode 100644 index 0000000..33c324a --- /dev/null +++ b/src/main/java/frc/robot/autons/FollowPath.java @@ -0,0 +1,86 @@ +package frc.robot.autons; + +import java.util.Optional; +import java.util.function.BiConsumer; +import java.util.function.BooleanSupplier; +import java.util.function.Supplier; + +import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.controllers.PathFollowingController; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.IdealStartingState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.util.DriveFeedforwards; +import com.pathplanner.lib.util.PathPlannerLogging; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.commands.DriveToWaypointCommand; +import frc.robot.commands.DriveToWaypointCommand.DriveWaypoints; +import frc.robot.constants.AutoConstants; +import frc.robot.constants.DriveConstants; +import frc.robot.subsystems.Dashboard; +import frc.robot.subsystems.DriveTrain; + +public class FollowPath extends Auton { + private final DriveTrain driveTrain = DriveTrain.getInstance(); + private final Dashboard dashboard = Dashboard.getInstance(); + + public enum Position { + TEST(3.165, 3.947, new Rotation2d(0)); + + private final double x; + private final double y; + private final Rotation2d rotation; + + private Position(double x, double y, Rotation2d rotation) { + this.x = x; + this.y = y; + this.rotation = rotation; + } + + public Pose2d getPose() { + return new Pose2d(x, y, rotation); + } + } + + private Pose2d startingPose; + private final PathConstraints constraints = new PathConstraints( + DriveConstants.MaxSpeed, + AutoConstants.kMaxAccelerationMetersPerSecondSquared, + AutoConstants.kMaxAngularSpeedRadiansPerSecond, + AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared); + private final IdealStartingState idealStartingState = new IdealStartingState( + driveTrain.getState().Speeds.vxMetersPerSecond, + driveTrain.getState().Pose.getRotation()); + private final GoalEndState goalEndState = new GoalEndState(0, Rotation2d.kZero); + + public FollowPath(Position position) { + super(Optional.empty()); + + Pose2d endingPose = position.getPose(); + + PathPlannerLogging.setLogActivePathCallback((activePath) -> { + if (activePath.size() >= 2) { + PathPlannerPath dashboardPath = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(activePath), + constraints, + idealStartingState, goalEndState); + this.dashboard.addPath(dashboardPath); + } + }); + + super.addCommands( + AutoBuilder.pathfindToPose(endingPose, this.constraints), + new DriveToWaypointCommand( + DriveWaypoints.Algae23, + this.constraints, + this.idealStartingState, + this.goalEndState)); + } +} \ No newline at end of file From 16eb6db093303ec88a3081ac913b59a1d96dec10 Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Thu, 13 Feb 2025 19:37:51 -0500 Subject: [PATCH 15/18] Wes is so cool. --- src/main/java/frc/robot/DriverJoystick.java | 26 ++ .../java/frc/robot/autons/FollowPath.java | 86 ------- .../commands/DriveToWaypointCommand.java | 237 ------------------ .../frc/robot/constants/AutoConstants.java | 46 ++++ .../java/frc/robot/subsystems/DriveTrain.java | 18 -- 5 files changed, 72 insertions(+), 341 deletions(-) delete mode 100644 src/main/java/frc/robot/autons/FollowPath.java delete mode 100644 src/main/java/frc/robot/commands/DriveToWaypointCommand.java diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index 773cf6d..054e2af 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -2,10 +2,20 @@ import frc.robot.constants.DriveConstants; import frc.robot.constants.IOConstants; +import frc.robot.constants.AutoConstants; +import frc.robot.constants.AutoConstants.DriveWaypoints; import frc.robot.libraries.XboxController1038; import frc.robot.subsystems.DriveTrain; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.IdealStartingState; +import com.pathplanner.lib.path.PathConstraints; +import com.pathplanner.lib.path.PathPlannerPath; + import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; @@ -13,6 +23,8 @@ public class DriverJoystick extends XboxController1038 { // Subsystem Dependencies private final DriveTrain driveTrain = DriveTrain.getInstance(); + private Pose2d currentPose; + // Previous Status private double prevX = 0; private double prevY = 0; @@ -82,6 +94,20 @@ private DriverJoystick() { // Lock the wheels into an X formation super.xButton.whileTrue(this.driveTrain.setX()); + super.aButton.whileTrue( + new InstantCommand(() -> { + this.currentPose = this.driveTrain.getState().Pose; + }).andThen(AutoBuilder.followPath(new PathPlannerPath( + PathPlannerPath.waypointsFromPoses(this.currentPose, DriveWaypoints.Algae23.getEndpoint()), + new PathConstraints( + DriveConstants.MaxSpeed, + AutoConstants.kMaxAccelerationMetersPerSecondSquared, + AutoConstants.kMaxAngularSpeedRadiansPerSecond, + AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared), + new IdealStartingState( + driveTrain.getState().Speeds.vxMetersPerSecond, + driveTrain.getState().Pose.getRotation()), + new GoalEndState(0, Rotation2d.kZero))))); } /** diff --git a/src/main/java/frc/robot/autons/FollowPath.java b/src/main/java/frc/robot/autons/FollowPath.java deleted file mode 100644 index 33c324a..0000000 --- a/src/main/java/frc/robot/autons/FollowPath.java +++ /dev/null @@ -1,86 +0,0 @@ -package frc.robot.autons; - -import java.util.Optional; -import java.util.function.BiConsumer; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - -import com.ctre.phoenix6.swerve.SwerveRequest; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.config.PIDConstants; -import com.pathplanner.lib.controllers.PPHolonomicDriveController; -import com.pathplanner.lib.controllers.PathFollowingController; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.IdealStartingState; -import com.pathplanner.lib.path.PathConstraints; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.PathPlannerLogging; - -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.DriverStation; -import frc.robot.commands.DriveToWaypointCommand; -import frc.robot.commands.DriveToWaypointCommand.DriveWaypoints; -import frc.robot.constants.AutoConstants; -import frc.robot.constants.DriveConstants; -import frc.robot.subsystems.Dashboard; -import frc.robot.subsystems.DriveTrain; - -public class FollowPath extends Auton { - private final DriveTrain driveTrain = DriveTrain.getInstance(); - private final Dashboard dashboard = Dashboard.getInstance(); - - public enum Position { - TEST(3.165, 3.947, new Rotation2d(0)); - - private final double x; - private final double y; - private final Rotation2d rotation; - - private Position(double x, double y, Rotation2d rotation) { - this.x = x; - this.y = y; - this.rotation = rotation; - } - - public Pose2d getPose() { - return new Pose2d(x, y, rotation); - } - } - - private Pose2d startingPose; - private final PathConstraints constraints = new PathConstraints( - DriveConstants.MaxSpeed, - AutoConstants.kMaxAccelerationMetersPerSecondSquared, - AutoConstants.kMaxAngularSpeedRadiansPerSecond, - AutoConstants.kMaxAngularSpeedRadiansPerSecondSquared); - private final IdealStartingState idealStartingState = new IdealStartingState( - driveTrain.getState().Speeds.vxMetersPerSecond, - driveTrain.getState().Pose.getRotation()); - private final GoalEndState goalEndState = new GoalEndState(0, Rotation2d.kZero); - - public FollowPath(Position position) { - super(Optional.empty()); - - Pose2d endingPose = position.getPose(); - - PathPlannerLogging.setLogActivePathCallback((activePath) -> { - if (activePath.size() >= 2) { - PathPlannerPath dashboardPath = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(activePath), - constraints, - idealStartingState, goalEndState); - this.dashboard.addPath(dashboardPath); - } - }); - - super.addCommands( - AutoBuilder.pathfindToPose(endingPose, this.constraints), - new DriveToWaypointCommand( - DriveWaypoints.Algae23, - this.constraints, - this.idealStartingState, - this.goalEndState)); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/DriveToWaypointCommand.java b/src/main/java/frc/robot/commands/DriveToWaypointCommand.java deleted file mode 100644 index 0c3a971..0000000 --- a/src/main/java/frc/robot/commands/DriveToWaypointCommand.java +++ /dev/null @@ -1,237 +0,0 @@ -package frc.robot.commands; - -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.controllers.PathFollowingController; -import com.pathplanner.lib.events.EventScheduler; -import com.pathplanner.lib.path.*; -import com.pathplanner.lib.trajectory.PathPlannerTrajectory; -import com.pathplanner.lib.trajectory.PathPlannerTrajectoryState; -import com.pathplanner.lib.util.DriveFeedforwards; -import com.pathplanner.lib.util.PPLibTelemetry; -import com.pathplanner.lib.util.PathPlannerLogging; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Subsystem; - -import java.util.*; -import java.util.function.BiConsumer; -import java.util.function.BooleanSupplier; -import java.util.function.Supplier; - -/** - * Base command for following a path - */ -public class DriveToWaypointCommand extends Command { - private final Timer timer = new Timer(); - private static Supplier poseSupplier; - private static Supplier speedsSupplier; - private static BiConsumer output; - private static PathFollowingController controller; - private static RobotConfig robotConfig; - private static BooleanSupplier shouldFlipPath; - private static Subsystem[] requirements; - private final EventScheduler eventScheduler; - - private PathPlannerPath path; - private final DriveWaypoints driveWaypoint; - private final PathConstraints constraints; - private final IdealStartingState idealStartingState; - private final GoalEndState goalEndState; - private PathPlannerTrajectory trajectory; - - public enum DriveWaypoints { - LeftCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftoCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - Algae23(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - Algae34(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - Processor(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))); - - private Pose2d endpoint; - - private DriveWaypoints(Pose2d endpoint) { - this.endpoint = endpoint; - } - - public Pose2d getEndpoint() { - return endpoint; - } - - } - - /** - * Construct a base path following command - * - * @param driveWaypoint The waypoint to reach - * @param constraints The constraints of the path - * @param idealStartingState The ideal starting state of the path - * @param goalEndState The ending state of the path - */ - public DriveToWaypointCommand( - DriveWaypoints driveWaypoint, - PathConstraints constraints, - IdealStartingState idealStartingState, - GoalEndState goalEndState) { - this.driveWaypoint = driveWaypoint; - this.constraints = constraints; - this.idealStartingState = idealStartingState; - this.goalEndState = goalEndState; - this.eventScheduler = new EventScheduler(); - - addRequirements(requirements); - - } - - @Override - public void initialize() { - Pose2d currentPose = poseSupplier.get(); - List poses = Arrays.asList(currentPose, this.driveWaypoint.getEndpoint()); - this.path = new PathPlannerPath(PathPlannerPath.waypointsFromPoses(poses), this.constraints, - this.idealStartingState, - this.goalEndState); - - Optional idealTrajectory = this.path - .getIdealTrajectory(DriveToWaypointCommand.robotConfig); - idealTrajectory.ifPresent(traj -> this.trajectory = traj); - - if (shouldFlipPath.getAsBoolean() && !path.preventFlipping) { - path = path.flipPath(); - } - ChassisSpeeds currentSpeeds = speedsSupplier.get(); - - controller.reset(currentPose, currentSpeeds); - - double linearVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); - - if (path.getIdealStartingState() != null) { - // Check if we match the ideal starting state - boolean idealVelocity = Math.abs(linearVel - path.getIdealStartingState().velocityMPS()) <= 0.25; - boolean idealRotation = !robotConfig.isHolonomic - || Math.abs( - currentPose - .getRotation() - .minus(path.getIdealStartingState().rotation()) - .getDegrees()) <= 30.0; - if (idealVelocity && idealRotation) { - // We can use the ideal trajectory - trajectory = path.getIdealTrajectory(robotConfig).orElseThrow(); - } else { - // We need to regenerate - trajectory = path.generateTrajectory(currentSpeeds, currentPose.getRotation(), robotConfig); - } - } else { - // No ideal starting state, generate the trajectory - trajectory = path.generateTrajectory(currentSpeeds, currentPose.getRotation(), robotConfig); - } - - PathPlannerAuto.setCurrentTrajectory(trajectory); - PathPlannerAuto.currentPathName = path.name; - - PathPlannerLogging.logActivePath(path); - PPLibTelemetry.setCurrentPath(path); - - eventScheduler.initialize(trajectory); - - timer.reset(); - timer.start(); - } - - @Override - public void execute() { - double currentTime = timer.get(); - PathPlannerTrajectoryState targetState = trajectory.sample(currentTime); - if (!controller.isHolonomic() && path.isReversed()) { - targetState = targetState.reverse(); - } - - Pose2d currentPose = poseSupplier.get(); - ChassisSpeeds currentSpeeds = speedsSupplier.get(); - - ChassisSpeeds targetSpeeds = controller.calculateRobotRelativeSpeeds(currentPose, targetState); - - double currentVel = Math.hypot(currentSpeeds.vxMetersPerSecond, currentSpeeds.vyMetersPerSecond); - - PPLibTelemetry.setCurrentPose(currentPose); - PathPlannerLogging.logCurrentPose(currentPose); - - PPLibTelemetry.setTargetPose(targetState.pose); - PathPlannerLogging.logTargetPose(targetState.pose); - - PPLibTelemetry.setVelocities( - currentVel, - targetState.linearVelocity, - currentSpeeds.omegaRadiansPerSecond, - targetSpeeds.omegaRadiansPerSecond); - - output.accept(targetSpeeds, targetState.feedforwards); - - eventScheduler.execute(currentTime); - } - - @Override - public boolean isFinished() { - return timer.hasElapsed(trajectory.getTotalTimeSeconds()); - } - - @Override - public void end(boolean interrupted) { - timer.stop(); - PathPlannerAuto.currentPathName = ""; - PathPlannerAuto.setCurrentTrajectory(null); - - // Only output 0 speeds when ending a path that is supposed to stop, this allows - // interrupting - // the command to smoothly transition into some auto-alignment routine - if (!interrupted && path.getGoalEndState().velocityMPS() < 0.1) { - output.accept(new ChassisSpeeds(), DriveFeedforwards.zeros(robotConfig.numModules)); - } - - PathPlannerLogging.logActivePath(null); - - eventScheduler.end(); - } - - public static void configure(Supplier poseSupplier, - Supplier speedsSupplier, - BiConsumer output, - PathFollowingController controller, - RobotConfig robotConfig, - BooleanSupplier shouldFlipPath, - Subsystem... requirements) { - DriveToWaypointCommand.poseSupplier = poseSupplier; - DriveToWaypointCommand.speedsSupplier = speedsSupplier; - DriveToWaypointCommand.output = output; - DriveToWaypointCommand.controller = controller; - DriveToWaypointCommand.robotConfig = robotConfig; - DriveToWaypointCommand.shouldFlipPath = shouldFlipPath; - DriveToWaypointCommand.requirements = requirements; - } -} diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 8da73b5..3e2b224 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -4,9 +4,55 @@ import com.pathplanner.lib.config.RobotConfig; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; public final class AutoConstants { + public enum DriveWaypoints { + LeftCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Algae23(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Algae34(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Processor(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))); + + private Pose2d endpoint; + + private DriveWaypoints(Pose2d endpoint) { + this.endpoint = endpoint; + } + + public Pose2d getEndpoint() { + return endpoint; + } + + } + public static final double kMaxAccelerationMetersPerSecondSquared = 7; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index 4883470..fddf467 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -29,7 +29,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.commands.DriveToWaypointCommand; import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.SwerveConstants; @@ -167,23 +166,6 @@ private DriveTrain() { AutoConstants.kRobotConfig.get(), () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, this); - DriveToWaypointCommand.configure( - () -> this.getState().Pose, - () -> this.getState().Speeds, - (ChassisSpeeds speeds, DriveFeedforwards feedForwards) -> { - this.setControl( - new SwerveRequest.ApplyRobotSpeeds().withSpeeds(speeds) - .withWheelForceFeedforwardsX(feedForwards.robotRelativeForcesXNewtons()) - .withWheelForceFeedforwardsY(feedForwards.robotRelativeForcesYNewtons())); - }, - new PPHolonomicDriveController( - new PIDConstants(AutoConstants.kPXController, AutoConstants.kIXController, - AutoConstants.kDController), - new PIDConstants(AutoConstants.kPThetaController, AutoConstants.kIThetaController, - AutoConstants.kDThetaController)), - AutoConstants.kRobotConfig.get(), - () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, - this); if (Utils.isSimulation()) { startSimThread(); } From 806a237d754492f9aaf4eebea2e7f74bfeba4f2a Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Fri, 14 Feb 2025 11:25:14 -0500 Subject: [PATCH 16/18] Fix suggestions. --- src/main/java/frc/robot/DriverJoystick.java | 2 +- .../frc/robot/constants/AutoConstants.java | 43 -------------- .../frc/robot/constants/DriveWaypoints.java | 48 +++++++++++++++ .../frc/robot/constants/VisionConstants.java | 13 ++++- .../java/frc/robot/subsystems/Vision.java | 6 +- ....2.2.json => PathplannerLib-2025.2.3.json} | 8 +-- ...enix6-25.2.1.json => Phoenix6-25.2.2.json} | 58 +++++++++---------- 7 files changed, 95 insertions(+), 83 deletions(-) create mode 100644 src/main/java/frc/robot/constants/DriveWaypoints.java rename vendordeps/{PathplannerLib-2025.2.2.json => PathplannerLib-2025.2.3.json} (87%) rename vendordeps/{Phoenix6-25.2.1.json => Phoenix6-25.2.2.json} (92%) diff --git a/src/main/java/frc/robot/DriverJoystick.java b/src/main/java/frc/robot/DriverJoystick.java index 054e2af..e4a2a4b 100644 --- a/src/main/java/frc/robot/DriverJoystick.java +++ b/src/main/java/frc/robot/DriverJoystick.java @@ -3,7 +3,7 @@ import frc.robot.constants.DriveConstants; import frc.robot.constants.IOConstants; import frc.robot.constants.AutoConstants; -import frc.robot.constants.AutoConstants.DriveWaypoints; +import frc.robot.constants.DriveWaypoints; import frc.robot.libraries.XboxController1038; import frc.robot.subsystems.DriveTrain; diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 3e2b224..54a2a28 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -10,49 +10,6 @@ import edu.wpi.first.math.trajectory.TrapezoidProfile; public final class AutoConstants { - public enum DriveWaypoints { - LeftCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - Algae23(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - Algae34(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - Processor(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - LeftFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), - RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))); - - private Pose2d endpoint; - - private DriveWaypoints(Pose2d endpoint) { - this.endpoint = endpoint; - } - - public Pose2d getEndpoint() { - return endpoint; - } - - } - public static final double kMaxAccelerationMetersPerSecondSquared = 7; public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI; public static final double kMaxAngularSpeedRadiansPerSecondSquared = Math.PI; diff --git a/src/main/java/frc/robot/constants/DriveWaypoints.java b/src/main/java/frc/robot/constants/DriveWaypoints.java new file mode 100644 index 0000000..b388a34 --- /dev/null +++ b/src/main/java/frc/robot/constants/DriveWaypoints.java @@ -0,0 +1,48 @@ +package frc.robot.constants; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +public enum DriveWaypoints { + LeftCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightCoral4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Algae23(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Algae34(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + Processor(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + LeftFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation1(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation2(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation3(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation4(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation5(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation6(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation7(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation8(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))), + RightFeederStation9(new Pose2d(new Translation2d(0, 0), new Rotation2d(0))); + + private Pose2d endpoint; + + private DriveWaypoints(Pose2d endpoint) { + this.endpoint = endpoint; + } + + public Pose2d getEndpoint() { + return endpoint; + } + +} diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 8cf0ebd..3e4b10e 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -32,7 +32,18 @@ public final class VisionConstants { public static final double aprilTagArea = 28908; public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField); - public static final Transform3d kRobotToCam = new Transform3d(new Translation3d(0.1, 0.0, 0.1), + + private static final double kFrontCameraXOffset = 0.1; + private static final double kFrontCameraYOffset = 0.0; + private static final double kFrontCameraZOffset = 0.1; + public static final Transform3d kRobotToFrontCam = new Transform3d( + new Translation3d(kFrontCameraXOffset, kFrontCameraYOffset, kFrontCameraZOffset), + new Rotation3d(0, 0, 0)); + private static final double kBackCameraXOffset = 0.1; + private static final double kBackCameraYOffset = 0.0; + private static final double kBackCameraZOffset = 0.1; + public static final Transform3d kRobotToBackCam = new Transform3d( + new Translation3d(kBackCameraXOffset, kBackCameraYOffset, kBackCameraZOffset), new Rotation3d(0, 0, 0)); public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 937bcc9..7b947e4 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -59,7 +59,7 @@ private Vision() { backCam = new PhotonCamera("backCam"); photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - VisionConstants.kRobotToCam); + VisionConstants.kRobotToFrontCam); photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS); } @@ -146,8 +146,4 @@ private void updateEstimationStdDevs( public Matrix getEstimationStdDevs() { return curStdDevs; } - - // public int getBestTarget() { - // return frontCam.getAllUnreadResults().get(1).getBestTarget().getFiducialId(); - // } } \ No newline at end of file diff --git a/vendordeps/PathplannerLib-2025.2.2.json b/vendordeps/PathplannerLib-2025.2.3.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.2.json rename to vendordeps/PathplannerLib-2025.2.3.json index a5bf9ee..9151ce4 100644 --- a/vendordeps/PathplannerLib-2025.2.2.json +++ b/vendordeps/PathplannerLib-2025.2.3.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.2.json", + "fileName": "PathplannerLib-2025.2.3.json", "name": "PathplannerLib", - "version": "2025.2.2", + "version": "2025.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.2" + "version": "2025.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.2", + "version": "2025.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-25.2.1.json b/vendordeps/Phoenix6-25.2.2.json similarity index 92% rename from vendordeps/Phoenix6-25.2.1.json rename to vendordeps/Phoenix6-25.2.2.json index 1397da1..d617643 100644 --- a/vendordeps/Phoenix6-25.2.1.json +++ b/vendordeps/Phoenix6-25.2.2.json @@ -1,7 +1,7 @@ { - "fileName": "Phoenix6-25.2.1.json", + "fileName": "Phoenix6-25.2.2.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, From 820d0e84f030f9985e5bb34cf3f842b622bbc30c Mon Sep 17 00:00:00 2001 From: Wesley Reed <24629474+reediculous456@users.noreply.github.com> Date: Fri, 14 Feb 2025 12:18:42 -0500 Subject: [PATCH 17/18] Discard changes to src/main/java/frc/robot/subsystems/Dashboard.java --- src/main/java/frc/robot/subsystems/Dashboard.java | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Dashboard.java b/src/main/java/frc/robot/subsystems/Dashboard.java index df033d7..190e433 100644 --- a/src/main/java/frc/robot/subsystems/Dashboard.java +++ b/src/main/java/frc/robot/subsystems/Dashboard.java @@ -13,14 +13,13 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.autons.AutonSelector.AutonChoices; -import com.pathplanner.lib.path.PathPlannerPath; public class Dashboard extends SubsystemBase { // Inputs - private final DriveTrain driveTrain = DriveTrain.getInstance(); + private DriveTrain driveTrain = DriveTrain.getInstance(); // Choosers - private final SendableChooser autoChooser = new SendableChooser<>(); + private SendableChooser autoChooser = new SendableChooser<>(); private SendableChooser delayChooser = new SendableChooser<>(); // Tabs @@ -101,10 +100,6 @@ public void periodic() { field.setRobotPose(driveTrain.getState().Pose); } - public void addPath(PathPlannerPath path) { - this.field.getObject("poses").setPoses(path.getPathPoses()); - } - /** * Puts the given {@link Trajectory} on the dashboard * From c816ac9084e1ee6b0f42629f6d4a6fa54930f404 Mon Sep 17 00:00:00 2001 From: Murat Bayraktar Date: Sat, 15 Feb 2025 12:34:56 -0500 Subject: [PATCH 18/18] Fix vision. --- .../frc/robot/constants/DriveWaypoints.java | 1 - .../frc/robot/constants/VisionConstants.java | 2 + .../java/frc/robot/subsystems/DriveTrain.java | 13 ++++++ .../java/frc/robot/subsystems/Vision.java | 43 ++++++++++++------- 4 files changed, 43 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/constants/DriveWaypoints.java b/src/main/java/frc/robot/constants/DriveWaypoints.java index b388a34..c238687 100644 --- a/src/main/java/frc/robot/constants/DriveWaypoints.java +++ b/src/main/java/frc/robot/constants/DriveWaypoints.java @@ -44,5 +44,4 @@ private DriveWaypoints(Pose2d endpoint) { public Pose2d getEndpoint() { return endpoint; } - } diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 3e4b10e..e9c48e8 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -39,12 +39,14 @@ public final class VisionConstants { public static final Transform3d kRobotToFrontCam = new Transform3d( new Translation3d(kFrontCameraXOffset, kFrontCameraYOffset, kFrontCameraZOffset), new Rotation3d(0, 0, 0)); + public static final String kRobotToFrontCamName = "frontCam"; private static final double kBackCameraXOffset = 0.1; private static final double kBackCameraYOffset = 0.0; private static final double kBackCameraZOffset = 0.1; public static final Transform3d kRobotToBackCam = new Transform3d( new Translation3d(kBackCameraXOffset, kBackCameraYOffset, kBackCameraZOffset), new Rotation3d(0, 0, 0)); + public static final String kRobotToBackCamName = "backCam"; public static final Matrix kSingleTagStdDevs = VecBuilder.fill(4, 4, 8); public static final Matrix kMultiTagStdDevs = VecBuilder.fill(0.5, 0.5, 1); } diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index fddf467..e02ec3a 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -32,6 +32,7 @@ import frc.robot.constants.AutoConstants; import frc.robot.constants.DriveConstants; import frc.robot.constants.SwerveConstants; +import frc.robot.constants.VisionConstants; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements @@ -54,6 +55,8 @@ public static DriveTrain getInstance() { return instance; } + private static final Vision vision = Vision.getInstance(); + /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */ private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero; /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */ @@ -256,6 +259,16 @@ public void periodic() { hasAppliedOperatorPerspective = true; }); } + + vision.frontCamGetEstimatedGlobalPose().ifPresent(estimatedPose -> { + this.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds, + vision.getEstimationStdDevs()); + }); + + vision.backCamGetEstimatedGlobalPose().ifPresent(estimatedPose -> { + this.addVisionMeasurement(estimatedPose.estimatedPose.toPose2d(), estimatedPose.timestampSeconds, + vision.getEstimationStdDevs()); + }); } /** diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 7b947e4..f486dd6 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -26,6 +26,7 @@ import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -37,12 +38,18 @@ import org.photonvision.PhotonCamera; import org.photonvision.PhotonPoseEstimator; import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; public class Vision extends SubsystemBase { - private final PhotonCamera frontCam; - private final PhotonCamera backCam; - private final PhotonPoseEstimator photonEstimator; + private final PhotonCamera frontCam = new PhotonCamera(VisionConstants.kRobotToFrontCamName); + private final PhotonCamera backCam = new PhotonCamera(VisionConstants.kRobotToBackCamName); + private final PhotonPoseEstimator frontCamPhotonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + VisionConstants.kRobotToFrontCam); + private final PhotonPoseEstimator backCamPhotonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, + PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, + VisionConstants.kRobotToBackCam); private Matrix curStdDevs; private static Vision instance; @@ -55,12 +62,8 @@ public static Vision getInstance() { } private Vision() { - frontCam = new PhotonCamera("frontCam"); - backCam = new PhotonCamera("backCam"); - - photonEstimator = new PhotonPoseEstimator(VisionConstants.kTagLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, - VisionConstants.kRobotToFrontCam); - photonEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS); + frontCamPhotonEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS); + backCamPhotonEstimator.setMultiTagFallbackStrategy(PoseStrategy.AVERAGE_BEST_TARGETS); } /** @@ -72,11 +75,21 @@ private Vision() { * timestamp, and targets * used for estimation. */ - public Optional getEstimatedGlobalPose() { + public Optional frontCamGetEstimatedGlobalPose() { + Optional visionEst = Optional.empty(); + + for (PhotonPipelineResult change : frontCam.getAllUnreadResults()) { + visionEst = frontCamPhotonEstimator.update(change); + updateEstimationStdDevs(visionEst, change.getTargets()); + } + return visionEst; + } + + public Optional backCamGetEstimatedGlobalPose() { Optional visionEst = Optional.empty(); - for (var change : frontCam.getAllUnreadResults()) { - visionEst = photonEstimator.update(change); + for (PhotonPipelineResult change : backCam.getAllUnreadResults()) { + visionEst = backCamPhotonEstimator.update(change); updateEstimationStdDevs(visionEst, change.getTargets()); } return visionEst; @@ -99,14 +112,14 @@ private void updateEstimationStdDevs( } else { // Pose present. Start running Heuristic - var estStdDevs = VisionConstants.kSingleTagStdDevs; + Matrix estStdDevs = VisionConstants.kSingleTagStdDevs; int numTags = 0; double avgDist = 0; // Precalculation - see how many tags we found, and calculate an // average-distance metric - for (var tgt : targets) { - var tagPose = photonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); + for (PhotonTrackedTarget tgt : targets) { + Optional tagPose = frontCamPhotonEstimator.getFieldTags().getTagPose(tgt.getFiducialId()); if (tagPose.isEmpty()) continue; numTags++;