From 6a7732c2bd9b9b1df454563519434052747e0dcf Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Sat, 20 Jan 2024 14:24:29 -0600 Subject: [PATCH 1/8] Pathplanner Amp/Climb Code --- src/main/java/frc/robot/RobotContainer.java | 40 ++++++++++++++++++- .../java/frc/robot/settings/Constants.java | 23 +++++++++++ 2 files changed, 62 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f50d616e..3e4b1b20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,6 +7,7 @@ import static frc.robot.settings.Constants.PS4Driver.*; import static frc.robot.settings.Constants.PS4Operator.*; +import java.nio.file.Path; import java.util.function.BooleanSupplier; import static frc.robot.settings.Constants.DriveConstants.*; @@ -14,8 +15,10 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.util.HolonomicPathFollowerConfig; import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.util.PathPlannerLogging; import com.pathplanner.lib.util.ReplanningConfig; import frc.robot.commands.IntakeCommand; import frc.robot.commands.Autos; @@ -26,6 +29,7 @@ import frc.robot.settings.Constants.ClimberConstants; import frc.robot.settings.Constants.DriveConstants; import frc.robot.settings.Constants.IntakeConstants; +import frc.robot.settings.Constants.PathConstants; import frc.robot.settings.Constants.ShooterConstants; import frc.robot.subsystems.Climber; import frc.robot.commands.ManualShoot; @@ -80,6 +84,16 @@ public class RobotContainer { private IntakeDirection iDirection; private Pigeon2 pigeon; +//BA means B for blue alliance and A amp-side. S is source-side, and M is middle. + private Command climbBA; + private Command climbBM; + private Command climbBS; + private Command climbRA; + private Command climbRM; + private Command climbRS; + private Command ampRed_Go; + private Command ampBlue_Go; + // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -92,9 +106,9 @@ public RobotContainer() { driverController = new PS4Controller(DRIVE_CONTROLLER_ID); operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); + // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists); driveTrainInst(); - autoInit(); limelightInit(); if(intakeExists) {intakeInst();} if(shooterExists) {shooterInst();} @@ -124,6 +138,16 @@ private void climberInst() { } private void autoInit() { configureDriveTrain(); + PathPlannerPath amPath = PathPlannerPath.fromPathFile(PathConstants.PATH_PLAN_AMP); + climbBA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); + climbBM = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BM_POSE, DEFAUL_PATH_CONSTRAINTS); + climbBS = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); + climbRA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RA_POSE, DEFAUL_PATH_CONSTRAINTS); + climbRM = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RM_POSE, DEFAUL_PATH_CONSTRAINTS); + climbRS = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RS_POSE, DEFAUL_PATH_CONSTRAINTS); + ampRed_Go = AutoBuilder.pathfindToPose(PathConstants.AMP_RED_POSE, DEFAUL_PATH_CONSTRAINTS); + ampBlue_Go = AutoBuilder.pathfindToPose(PathConstants.AMP_BLUE_POSE, DEFAUL_PATH_CONSTRAINTS); + SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser()); registerNamedCommands(); } @@ -159,6 +183,20 @@ private void configureBindings() { new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); new Trigger(operatorController::getR1Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE)); new Trigger(operatorController::getR2Button).onTrue(new IntakeCommand(intake, iDirection.COAST)); + //Path finding is pretty epic. It was a dark and stormy night. I was waiting, at my computer in the programming room. + // More specifically, I was waiting for Rowan, who was working with sequential command groups. I called over to Rowan if he could come help soon, and he answered. + //but his voice sounded kinda weird. I didn't think much of it at the time + if (DriverStation.getAlliance().get() == Alliance.Red) { + new Trigger(()->driverController.getPOV() == 0).onTrue(climbRA); + new Trigger(()->driverController.getPOV() == 90).onTrue(climbRS); + new Trigger(()->driverController.getPOV() == 180).onTrue(climbRS); + new Trigger(()->driverController.getL2Button()).onTrue(ampRed_Go); + } else { + new Trigger(()->driverController.getPOV() == 0).onTrue(climbBA); + new Trigger(()->driverController.getPOV() == 90).onTrue(climbBS); + new Trigger(()->driverController.getPOV() == 180).onTrue(climbBS); + new Trigger(()->driverController.getL2Button()).onTrue(ampBlue_Go); + } //for testing Rotate Robot command }; diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index ac808df5..a65f9efe 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -387,4 +387,27 @@ public final class Vision{ public static final double K_DETECTOR_TA_I = 0; public static final double K_DETECTOR_TA_D = 0; } +public final class PathConstants{ + public static final String PATH_PLAN_AMP = "Replace Name Later"; + //I messed up the heading, it needs to be fixed. Yup + // PLEASE DO NOT MISS THIS MESSAGE + //Amp side of the blue stage + public static final Pose2d CLIMB_BA_POSE = new Pose2d(4.33, 4.88, new Rotation2d(116.98)); + //Middle side of the blue stage + public static final Pose2d CLIMB_BM_POSE = new Pose2d(5.91, 4.12, new Rotation2d(-0.90)); + //Source side of the blue stage + public static final Pose2d CLIMB_BS_POSE = new Pose2d(4.52, 3.24, new Rotation2d(-120.96)); + + //Amp side of the red stage + public static final Pose2d CLIMB_RA_POSE = new Pose2d(12.30, 4.88, new Rotation2d(59.86)); + //Middle side of the red stage + public static final Pose2d CLIMB_RM_POSE = new Pose2d( 8.97, 4.00, new Rotation2d(-179.64)); + //Source side of the red stage + public static final Pose2d CLIMB_RS_POSE = new Pose2d(12.19, 3.24, new Rotation2d(-61.23)); + + + public static final Pose2d AMP_RED_POSE = new Pose2d(14.77, 7.33, new Rotation2d(-89.33)); + public static final Pose2d AMP_BLUE_POSE = new Pose2d(1.93, 7.33, new Rotation2d(-89.33)); + +} } From 380cc6b6b831ad91cfb8a73e2598a215fe53b39e Mon Sep 17 00:00:00 2001 From: NoMythic <2491nomythic@gmail.com> Date: Tue, 23 Jan 2024 20:13:29 -0600 Subject: [PATCH 2/8] added commands to go to the positions of amp and climb place --- .../deploy/pathplanner/paths/New Path.path | 24 ++++- src/main/java/frc/robot/RobotContainer.java | 61 +++++++----- .../frc/robot/commands/goToPose/GoToAmp.java | 56 +++++++++++ .../goToPose/GoToNearestClimbSpot.java | 92 +++++++++++++++++++ .../java/frc/robot/settings/Constants.java | 49 ++++++---- 5 files changed, 233 insertions(+), 49 deletions(-) create mode 100644 src/main/java/frc/robot/commands/goToPose/GoToAmp.java create mode 100644 src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path index 97cefca6..cc92e9e8 100644 --- a/src/main/deploy/pathplanner/paths/New Path.path +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.8441688792209487, + "y": 5.484558433504332 }, "prevControl": null, "nextControl": { - "x": 3.0, - "y": 6.5 + "x": 3.8441688792209536, + "y": 4.984558433504332 }, "isLocked": false, "linkedName": null @@ -39,6 +39,22 @@ "x": 8.197125890736343, "y": 3.448242280285035 }, + "nextControl": { + "x": 5.802874109263657, + "y": -1.4482422802850352 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 14.783900564952129, + "y": 7.87016593976208 + }, + "prevControl": { + "x": 10.293387337107893, + "y": 3.2109618297385225 + }, "nextControl": null, "isLocked": false, "linkedName": null diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3e4b1b20..61621849 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,11 +35,14 @@ import frc.robot.commands.ManualShoot; import frc.robot.commands.RotateRobot; import frc.robot.commands.autoAimParallel; +import frc.robot.commands.goToPose.GoToAmp; +import frc.robot.commands.goToPose.GoToNearestClimbSpot; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Limelight; import frc.robot.subsystems.ShooterSubsystem; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -83,8 +86,8 @@ public class RobotContainer { private Limelight limelight; private IntakeDirection iDirection; private Pigeon2 pigeon; - -//BA means B for blue alliance and A amp-side. S is source-side, and M is middle. + public SendableChooser climbSpotChooser; +//BA means B for blue alliance and A is for amp-chain. S is source-chain, and M is middle. private Command climbBA; private Command climbBM; private Command climbBS; @@ -107,16 +110,33 @@ public RobotContainer() { operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID); pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID); // = new PathPlannerPath(null, DEFAUL_PATH_CONSTRAINTS, null, climberExists); - driveTrainInst(); limelightInit(); + autoInit(); + if(intakeExists) {intakeInst();} if(shooterExists) {shooterInst();} if(climberExists) {climberInst();} + climbSpotChooserInit(); // Configure the trigger bindings configureBindings(); } + private void climbSpotChooserInit() { + climbSpotChooser = new SendableChooser(); + climbSpotChooser.addOption("Climb L-Chain Right", "L-Chain Right"); + climbSpotChooser.addOption("Climb L-Chain Middle", "L-Chain Middle"); + climbSpotChooser.addOption("Climb L-Chain Left", "L-Chain Left"); + + climbSpotChooser.addOption("Climb Mid-Chain Left", "Mid-Chain Left"); + climbSpotChooser.addOption("Climb Mid-Chain Right", "Mid-Chain Right"); + climbSpotChooser.addOption("Climb Mid-Chain Middle", "Mid-Chain Middle"); + + climbSpotChooser.addOption("Climb R-Chain Left", "R-Chain Left"); + climbSpotChooser.addOption("Climb R-Chain Right", "R-Chain Right"); + climbSpotChooser.addOption("Climb R-Chain Middle", "R-Chain Middle"); + SmartDashboard.putData(climbSpotChooser); + } private void driveTrainInst() { driveTrain = new DrivetrainSubsystem(); defaultDriveCommand = new Drive( @@ -138,8 +158,7 @@ private void climberInst() { } private void autoInit() { configureDriveTrain(); - PathPlannerPath amPath = PathPlannerPath.fromPathFile(PathConstants.PATH_PLAN_AMP); - climbBA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); + /* climbBA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); climbBM = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BM_POSE, DEFAUL_PATH_CONSTRAINTS); climbBS = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); climbRA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RA_POSE, DEFAUL_PATH_CONSTRAINTS); @@ -147,7 +166,7 @@ private void autoInit() { climbRS = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RS_POSE, DEFAUL_PATH_CONSTRAINTS); ampRed_Go = AutoBuilder.pathfindToPose(PathConstants.AMP_RED_POSE, DEFAUL_PATH_CONSTRAINTS); ampBlue_Go = AutoBuilder.pathfindToPose(PathConstants.AMP_BLUE_POSE, DEFAUL_PATH_CONSTRAINTS); - +*/ SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser()); registerNamedCommands(); } @@ -173,30 +192,22 @@ private void configureBindings() { // new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain/*, shooter*/)); new Trigger(driverController::getPSButton).onTrue(new InstantCommand(driveTrain::zeroGyroscope)); SmartDashboard.putData(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle)); - new Trigger(driverController::getCrossButton).onTrue(new InstantCommand(()->SmartDashboard.putNumber("calculated robot angle", driveTrain.calculateSpeakerAngle()))); - // new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain)); - new Trigger(driverController::getCrossButton).onTrue(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle)); + // new Trigger(driverController::getCrossButton).onTrue(new InstantCommand(()->SmartDashboard.putNumber("calculated robot angle", driveTrain.calculateSpeakerAngle()))); + // // new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain)); + // new Trigger(driverController::getCrossButton).onTrue(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle)); - new Trigger(operatorController::getCircleButton).onTrue(new ManualShoot(shooter)); - new Trigger(operatorController::getCrossButtonPressed).onTrue(new ClimbCommandGroup(climber, ClimberConstants.CLIMBER_SPEED)); + if(shooterExists){new Trigger(operatorController::getCircleButton).onTrue(new ManualShoot(shooter));} + if(climberExists) {new Trigger(operatorController::getCrossButtonPressed).onTrue(new ClimbCommandGroup(climber, ClimberConstants.CLIMBER_SPEED));} //Intake bindings - new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); - new Trigger(operatorController::getR1Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE)); - new Trigger(operatorController::getR2Button).onTrue(new IntakeCommand(intake, iDirection.COAST)); + // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); + // new Trigger(operatorController::getR1Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE)); + // new Trigger(operatorController::getR2Button).onTrue(new IntakeCommand(intake, iDirection.COAST)); //Path finding is pretty epic. It was a dark and stormy night. I was waiting, at my computer in the programming room. // More specifically, I was waiting for Rowan, who was working with sequential command groups. I called over to Rowan if he could come help soon, and he answered. //but his voice sounded kinda weird. I didn't think much of it at the time - if (DriverStation.getAlliance().get() == Alliance.Red) { - new Trigger(()->driverController.getPOV() == 0).onTrue(climbRA); - new Trigger(()->driverController.getPOV() == 90).onTrue(climbRS); - new Trigger(()->driverController.getPOV() == 180).onTrue(climbRS); - new Trigger(()->driverController.getL2Button()).onTrue(ampRed_Go); - } else { - new Trigger(()->driverController.getPOV() == 0).onTrue(climbBA); - new Trigger(()->driverController.getPOV() == 90).onTrue(climbBS); - new Trigger(()->driverController.getPOV() == 180).onTrue(climbBS); - new Trigger(()->driverController.getL2Button()).onTrue(ampBlue_Go); - } + new Trigger(driverController::getTriangleButton).onTrue(new GoToNearestClimbSpot(driveTrain, climbSpotChooser)); + new Trigger(driverController::getCrossButton).onTrue(new GoToAmp(driveTrain, ()->DriverStation.getAlliance().get().equals(Alliance.Red))); + //for testing Rotate Robot command }; diff --git a/src/main/java/frc/robot/commands/goToPose/GoToAmp.java b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java new file mode 100644 index 00000000..24f222b6 --- /dev/null +++ b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.goToPose; + +import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS; + +import java.util.function.BooleanSupplier; + +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.settings.Constants.PathConstants; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class GoToAmp extends Command { + Command actualCommand; + /** Creates a new GoToAmp. */ + BooleanSupplier isAllianceRed; + DrivetrainSubsystem driveTrain; + public GoToAmp(DrivetrainSubsystem drivetrain, BooleanSupplier isAllianceRedSupl) { + isAllianceRed = isAllianceRedSupl; + this.driveTrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + if (isAllianceRed.getAsBoolean()) { + actualCommand = AutoBuilder.pathfindToPose(PathConstants.AMP_RED_POSE, DEFAUL_PATH_CONSTRAINTS); + } + else{ + actualCommand = AutoBuilder.pathfindToPose(PathConstants.AMP_BLUE_POSE, DEFAUL_PATH_CONSTRAINTS); + } + actualCommand.initialize(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + actualCommand.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return actualCommand.isFinished(); + } +} diff --git a/src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java b/src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java new file mode 100644 index 00000000..f00673d8 --- /dev/null +++ b/src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java @@ -0,0 +1,92 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.goToPose; + +import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS; + +import java.util.function.BooleanSupplier; + +import com.pathplanner.lib.auto.AutoBuilder; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.settings.Constants.PathConstants; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class GoToNearestClimbSpot extends Command { + Command actualCommand; + DrivetrainSubsystem drivetrain; + SendableChooser climbSpotChooser; + /** Creates a new GoToAmp. */ + public GoToNearestClimbSpot(DrivetrainSubsystem drivetrain, SendableChooser climbSpotChooser) { + this.climbSpotChooser = climbSpotChooser; + this.drivetrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + switch (climbSpotChooser.getSelected()) { + case "L-Chain Left": + System.out.println("L-Chain Left!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_LL_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "L-Chain Middle": + System.out.println("L-Chain Middle!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_LM_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "L-Chain Right": + System.out.println("L-Chain Right!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_LR_POSE, DEFAUL_PATH_CONSTRAINTS); + case "Mid-Chain Left": + System.out.println("Mid-Chain Left!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_ML_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "Mid-Chain Middle": + System.out.println("Mid-Chain Middle!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_MM_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "Mid-Chain Right": + System.out.println("Mid-Chain Right!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_MR_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "R-Chain Left": + System.out.println("R-Chain Left!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RL_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "R-Chain Middle": + System.out.println("R-Chain Middle!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RM_POSE, DEFAUL_PATH_CONSTRAINTS); + break; + case "R-Chain Right": + System.out.println("R-Chain Right!"); + actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RR_POSE, DEFAUL_PATH_CONSTRAINTS); + default: + System.out.println("We broke!(Or went to the default command)"); + break; + } + + actualCommand.initialize(); +} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + actualCommand.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return actualCommand.isFinished(); + } +} diff --git a/src/main/java/frc/robot/settings/Constants.java b/src/main/java/frc/robot/settings/Constants.java index a65f9efe..fd727758 100644 --- a/src/main/java/frc/robot/settings/Constants.java +++ b/src/main/java/frc/robot/settings/Constants.java @@ -388,26 +388,35 @@ public final class Vision{ public static final double K_DETECTOR_TA_D = 0; } public final class PathConstants{ - public static final String PATH_PLAN_AMP = "Replace Name Later"; - //I messed up the heading, it needs to be fixed. Yup - // PLEASE DO NOT MISS THIS MESSAGE - //Amp side of the blue stage - public static final Pose2d CLIMB_BA_POSE = new Pose2d(4.33, 4.88, new Rotation2d(116.98)); - //Middle side of the blue stage - public static final Pose2d CLIMB_BM_POSE = new Pose2d(5.91, 4.12, new Rotation2d(-0.90)); - //Source side of the blue stage - public static final Pose2d CLIMB_BS_POSE = new Pose2d(4.52, 3.24, new Rotation2d(-120.96)); - - //Amp side of the red stage - public static final Pose2d CLIMB_RA_POSE = new Pose2d(12.30, 4.88, new Rotation2d(59.86)); - //Middle side of the red stage - public static final Pose2d CLIMB_RM_POSE = new Pose2d( 8.97, 4.00, new Rotation2d(-179.64)); - //Source side of the red stage - public static final Pose2d CLIMB_RS_POSE = new Pose2d(12.19, 3.24, new Rotation2d(-61.23)); - - - public static final Pose2d AMP_RED_POSE = new Pose2d(14.77, 7.33, new Rotation2d(-89.33)); - public static final Pose2d AMP_BLUE_POSE = new Pose2d(1.93, 7.33, new Rotation2d(-89.33)); + //Welcome, to Pathconstantic Park + //Here the fine beasts of the Pathplanner Period reside, after being brought back through DNA + //Middle of the Left-chain + public static final Pose2d CLIMB_LM_POSE = new Pose2d(4.33, 4.88, new Rotation2d(116.98)); + //Middle of the Mid-chain + public static final Pose2d CLIMB_MM_POSE = new Pose2d(5.91, 4.12, new Rotation2d(-0.90)); + //Middle of the Right-chain + public static final Pose2d CLIMB_RM_POSE = new Pose2d(4.52, 3.24, new Rotation2d(-120.96)); + + //Left side of the Left-chain + public static final Pose2d CLIMB_LL_POSE = new Pose2d(2.491, 2.491, new Rotation2d(59.86)); + //Left side of the Mid-chain + public static final Pose2d CLIMB_ML_POSE = new Pose2d( 2.491, 2.491, new Rotation2d(-179.64)); + //Left side of the Right-chain + public static final Pose2d CLIMB_RL_POSE = new Pose2d( 2.491, 2.491, new Rotation2d(-179.64)); + + //Right side of the Left-chain + public static final Pose2d CLIMB_LR_POSE = new Pose2d(2.491, 2.491, new Rotation2d(59.86)); + //Right side of the Mid-chain + public static final Pose2d CLIMB_MR_POSE = new Pose2d( 2.491, 2.491, new Rotation2d(-179.64)); + //Right side of the Right-chain + public static final Pose2d CLIMB_RR_POSE = new Pose2d( 2.491, 2.491, new Rotation2d(-179.64)); + + + + public static final Pose2d CLIMB__POSE = new Pose2d(12.19, 3.24, new Rotation2d(-61.23)); + + public static final Pose2d AMP_RED_POSE = new Pose2d(1.93, 0.5, new Rotation2d(-89.33)); + public static final Pose2d AMP_BLUE_POSE = new Pose2d(1.93, 8, new Rotation2d(-89.33)); } } From 37de70d824cbb82b2bff71a94cc81354982e56e0 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 25 Jan 2024 19:39:39 -0600 Subject: [PATCH 3/8] added many options for autoChooser --- src/main/java/frc/robot/RobotContainer.java | 61 ++++++++++++++------- 1 file changed, 42 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 61621849..5261ffa3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,17 +86,19 @@ public class RobotContainer { private Limelight limelight; private IntakeDirection iDirection; private Pigeon2 pigeon; - public SendableChooser climbSpotChooser; -//BA means B for blue alliance and A is for amp-chain. S is source-chain, and M is middle. - private Command climbBA; - private Command climbBM; - private Command climbBS; - private Command climbRA; - private Command climbRM; - private Command climbRS; - private Command ampRed_Go; - private Command ampBlue_Go; +//BA means B for blue alliance and A amp-side. S is source-side, and M is middle. + private Command climbMidR; + private Command climbMidM; + private Command climbMidL; + private Command climbAmpSideR; + private Command climbAmpSideM; + private Command climbAmpSideL; + private Command climbSourceSideR; + private Command climbSourceSideM; + private Command climbSourceSideL; + private Command goToAmp; + private SendableChooser climbSpotChooser; // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ @@ -158,15 +160,36 @@ private void climberInst() { } private void autoInit() { configureDriveTrain(); - /* climbBA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); - climbBM = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BM_POSE, DEFAUL_PATH_CONSTRAINTS); - climbBS = AutoBuilder.pathfindToPose(PathConstants.CLIMB_BS_POSE, DEFAUL_PATH_CONSTRAINTS); - climbRA = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RA_POSE, DEFAUL_PATH_CONSTRAINTS); - climbRM = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RM_POSE, DEFAUL_PATH_CONSTRAINTS); - climbRS = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RS_POSE, DEFAUL_PATH_CONSTRAINTS); - ampRed_Go = AutoBuilder.pathfindToPose(PathConstants.AMP_RED_POSE, DEFAUL_PATH_CONSTRAINTS); - ampBlue_Go = AutoBuilder.pathfindToPose(PathConstants.AMP_BLUE_POSE, DEFAUL_PATH_CONSTRAINTS); -*/ + PathPlannerPath ampPath = PathPlannerPath.fromPathFile(PathConstants.PATH_PLAN_AMP); + PathPlannerPath climbMidRPath = PathPlannerPath.fromPathFile("climbMidR"); + PathPlannerPath climbMidMPath = PathPlannerPath.fromPathFile("climbMidM"); + PathPlannerPath climbMidLPath = PathPlannerPath.fromPathFile("climbMidL"); + PathPlannerPath climbSourceSideRPath = PathPlannerPath.fromPathFile("climbSourceSideR"); + PathPlannerPath climbSourceSideMPath = PathPlannerPath.fromPathFile("climbSourceSideM"); + PathPlannerPath climbSourceSideLPath = PathPlannerPath.fromPathFile("climbSourceSideL"); + PathPlannerPath climbAmpSideRPath = PathPlannerPath.fromPathFile("climbAmpSideR"); + PathPlannerPath climbAmpSideMPath = PathPlannerPath.fromPathFile("climbAmpSideM"); + PathPlannerPath climbAmpSideLPath = PathPlannerPath.fromPathFile("climbAmpSideL"); + climbMidR = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAUL_PATH_CONSTRAINTS); + climbMidM = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAUL_PATH_CONSTRAINTS); + climbMidL = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAUL_PATH_CONSTRAINTS); + climbAmpSideR = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAUL_PATH_CONSTRAINTS); + climbAmpSideM = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAUL_PATH_CONSTRAINTS); + climbAmpSideL = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAUL_PATH_CONSTRAINTS); + climbSourceSideR = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAUL_PATH_CONSTRAINTS); + climbSourceSideM = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAUL_PATH_CONSTRAINTS); + climbSourceSideL = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAUL_PATH_CONSTRAINTS); + goToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAUL_PATH_CONSTRAINTS); + climbSpotChooser = new SendableChooser(); + climbSpotChooser.addOption("AmpSideR", "AmpSideR"); + climbSpotChooser.addOption("AmpSideM", "AmpSideM"); + climbSpotChooser.addOption("AmpSideL", "AmpSideL"); + climbSpotChooser.addOption("SourceSideR", "SourceSideR"); + climbSpotChooser.addOption("SourceSideM", "SourceSideM"); + climbSpotChooser.addOption("SourceSideL", "SourceSideL"); + climbSpotChooser.addOption("MiddleR", "MiddleR"); + climbSpotChooser.addOption("MiddleM", "MiddleM"); + climbSpotChooser.addOption("MiddleL", "MiddleL"); SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser()); registerNamedCommands(); } From 6a8e6fb61db4803fa70d780840758436ae6ce9e8 Mon Sep 17 00:00:00 2001 From: Rowan Flood <121908273+rflood07@users.noreply.github.com> Date: Thu, 25 Jan 2024 19:51:47 -0600 Subject: [PATCH 4/8] combined changes from multiple commits together --- src/main/java/frc/robot/RobotContainer.java | 49 ++------- .../frc/robot/commands/goToPose/GoToAmp.java | 12 +- .../commands/goToPose/GoToClimbSpot.java | 103 ++++++++++++++++++ .../goToPose/GoToNearestClimbSpot.java | 92 ---------------- 4 files changed, 116 insertions(+), 140 deletions(-) create mode 100644 src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java delete mode 100644 src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5261ffa3..0a29fe13 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -36,7 +36,7 @@ import frc.robot.commands.RotateRobot; import frc.robot.commands.autoAimParallel; import frc.robot.commands.goToPose.GoToAmp; -import frc.robot.commands.goToPose.GoToNearestClimbSpot; +import frc.robot.commands.goToPose.GoToClimbSpot; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.ExampleSubsystem; import frc.robot.subsystems.IntakeSubsystem; @@ -125,18 +125,17 @@ public RobotContainer() { } private void climbSpotChooserInit() { climbSpotChooser = new SendableChooser(); - climbSpotChooser.addOption("Climb L-Chain Right", "L-Chain Right"); + climbSpotChooser.addOption("Climb L-Chain Amp", "L-Chain Amp"); climbSpotChooser.addOption("Climb L-Chain Middle", "L-Chain Middle"); - climbSpotChooser.addOption("Climb L-Chain Left", "L-Chain Left"); + climbSpotChooser.addOption("Climb L-Chain Source", "L-Chain Source"); - - climbSpotChooser.addOption("Climb Mid-Chain Left", "Mid-Chain Left"); - climbSpotChooser.addOption("Climb Mid-Chain Right", "Mid-Chain Right"); + climbSpotChooser.addOption("Climb Mid-Chain Source", "Mid-Chain Source"); + climbSpotChooser.addOption("Climb Mid-Chain Amp", "Mid-Chain Amp"); climbSpotChooser.addOption("Climb Mid-Chain Middle", "Mid-Chain Middle"); - climbSpotChooser.addOption("Climb R-Chain Left", "R-Chain Left"); + climbSpotChooser.addOption("Climb R-Chain Source", "R-Chain Source"); climbSpotChooser.addOption("Climb R-Chain Right", "R-Chain Right"); - climbSpotChooser.addOption("Climb R-Chain Middle", "R-Chain Middle"); + climbSpotChooser.addOption("Climb R-Chain Amp", "R-Chain Amp"); SmartDashboard.putData(climbSpotChooser); } private void driveTrainInst() { @@ -160,36 +159,6 @@ private void climberInst() { } private void autoInit() { configureDriveTrain(); - PathPlannerPath ampPath = PathPlannerPath.fromPathFile(PathConstants.PATH_PLAN_AMP); - PathPlannerPath climbMidRPath = PathPlannerPath.fromPathFile("climbMidR"); - PathPlannerPath climbMidMPath = PathPlannerPath.fromPathFile("climbMidM"); - PathPlannerPath climbMidLPath = PathPlannerPath.fromPathFile("climbMidL"); - PathPlannerPath climbSourceSideRPath = PathPlannerPath.fromPathFile("climbSourceSideR"); - PathPlannerPath climbSourceSideMPath = PathPlannerPath.fromPathFile("climbSourceSideM"); - PathPlannerPath climbSourceSideLPath = PathPlannerPath.fromPathFile("climbSourceSideL"); - PathPlannerPath climbAmpSideRPath = PathPlannerPath.fromPathFile("climbAmpSideR"); - PathPlannerPath climbAmpSideMPath = PathPlannerPath.fromPathFile("climbAmpSideM"); - PathPlannerPath climbAmpSideLPath = PathPlannerPath.fromPathFile("climbAmpSideL"); - climbMidR = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAUL_PATH_CONSTRAINTS); - climbMidM = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAUL_PATH_CONSTRAINTS); - climbMidL = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAUL_PATH_CONSTRAINTS); - climbAmpSideR = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAUL_PATH_CONSTRAINTS); - climbAmpSideM = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAUL_PATH_CONSTRAINTS); - climbAmpSideL = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAUL_PATH_CONSTRAINTS); - climbSourceSideR = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAUL_PATH_CONSTRAINTS); - climbSourceSideM = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAUL_PATH_CONSTRAINTS); - climbSourceSideL = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAUL_PATH_CONSTRAINTS); - goToAmp = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAUL_PATH_CONSTRAINTS); - climbSpotChooser = new SendableChooser(); - climbSpotChooser.addOption("AmpSideR", "AmpSideR"); - climbSpotChooser.addOption("AmpSideM", "AmpSideM"); - climbSpotChooser.addOption("AmpSideL", "AmpSideL"); - climbSpotChooser.addOption("SourceSideR", "SourceSideR"); - climbSpotChooser.addOption("SourceSideM", "SourceSideM"); - climbSpotChooser.addOption("SourceSideL", "SourceSideL"); - climbSpotChooser.addOption("MiddleR", "MiddleR"); - climbSpotChooser.addOption("MiddleM", "MiddleM"); - climbSpotChooser.addOption("MiddleL", "MiddleL"); SmartDashboard.putData("Auto Chooser", AutoBuilder.buildAutoChooser()); registerNamedCommands(); } @@ -228,8 +197,8 @@ private void configureBindings() { //Path finding is pretty epic. It was a dark and stormy night. I was waiting, at my computer in the programming room. // More specifically, I was waiting for Rowan, who was working with sequential command groups. I called over to Rowan if he could come help soon, and he answered. //but his voice sounded kinda weird. I didn't think much of it at the time - new Trigger(driverController::getTriangleButton).onTrue(new GoToNearestClimbSpot(driveTrain, climbSpotChooser)); - new Trigger(driverController::getCrossButton).onTrue(new GoToAmp(driveTrain, ()->DriverStation.getAlliance().get().equals(Alliance.Red))); + new Trigger(driverController::getTriangleButton).onTrue(new GoToClimbSpot(driveTrain, climbSpotChooser)); + new Trigger(driverController::getCrossButton).onTrue(new GoToAmp(driveTrain)); //for testing Rotate Robot command diff --git a/src/main/java/frc/robot/commands/goToPose/GoToAmp.java b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java index 24f222b6..88e04f41 100644 --- a/src/main/java/frc/robot/commands/goToPose/GoToAmp.java +++ b/src/main/java/frc/robot/commands/goToPose/GoToAmp.java @@ -9,6 +9,7 @@ import java.util.function.BooleanSupplier; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.settings.Constants.PathConstants; @@ -19,8 +20,7 @@ public class GoToAmp extends Command { /** Creates a new GoToAmp. */ BooleanSupplier isAllianceRed; DrivetrainSubsystem driveTrain; - public GoToAmp(DrivetrainSubsystem drivetrain, BooleanSupplier isAllianceRedSupl) { - isAllianceRed = isAllianceRedSupl; + public GoToAmp(DrivetrainSubsystem drivetrain) { this.driveTrain = drivetrain; addRequirements(drivetrain); // Use addRequirements() here to declare subsystem dependencies. @@ -29,12 +29,8 @@ public GoToAmp(DrivetrainSubsystem drivetrain, BooleanSupplier isAllianceRedSupl // Called when the command is initially scheduled. @Override public void initialize() { - if (isAllianceRed.getAsBoolean()) { - actualCommand = AutoBuilder.pathfindToPose(PathConstants.AMP_RED_POSE, DEFAUL_PATH_CONSTRAINTS); - } - else{ - actualCommand = AutoBuilder.pathfindToPose(PathConstants.AMP_BLUE_POSE, DEFAUL_PATH_CONSTRAINTS); - } + PathPlannerPath ampPath = PathPlannerPath.fromPathFile("goToAmp"); + actualCommand = AutoBuilder.pathfindThenFollowPath(ampPath, DEFAUL_PATH_CONSTRAINTS); actualCommand.initialize(); } diff --git a/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java b/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java new file mode 100644 index 00000000..82bebb41 --- /dev/null +++ b/src/main/java/frc/robot/commands/goToPose/GoToClimbSpot.java @@ -0,0 +1,103 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.goToPose; + +import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS; + +import java.util.function.BooleanSupplier; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.settings.Constants.PathConstants; +import frc.robot.subsystems.DrivetrainSubsystem; + +public class GoToClimbSpot extends Command { + Command actualCommand; + DrivetrainSubsystem drivetrain; + SendableChooser climbSpotChooser; + /** Creates a new GoToAmp. */ + public GoToClimbSpot(DrivetrainSubsystem drivetrain, SendableChooser climbSpotChooser) { + this.climbSpotChooser = climbSpotChooser; + this.drivetrain = drivetrain; + addRequirements(drivetrain); + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + PathPlannerPath climbMidRPath = PathPlannerPath.fromPathFile("climbMidR"); + PathPlannerPath climbMidMPath = PathPlannerPath.fromPathFile("climbMidM"); + PathPlannerPath climbMidLPath = PathPlannerPath.fromPathFile("climbMidL"); + PathPlannerPath climbSourceSideRPath = PathPlannerPath.fromPathFile("climbSourceSideR"); + PathPlannerPath climbSourceSideMPath = PathPlannerPath.fromPathFile("climbSourceSideM"); + PathPlannerPath climbSourceSideLPath = PathPlannerPath.fromPathFile("climbSourceSideL"); + PathPlannerPath climbAmpSideRPath = PathPlannerPath.fromPathFile("climbAmpSideR"); + PathPlannerPath climbAmpSideMPath = PathPlannerPath.fromPathFile("climbAmpSideM"); + PathPlannerPath climbAmpSideLPath = PathPlannerPath.fromPathFile("climbAmpSideL"); + switch (climbSpotChooser.getSelected()) { + case "L-Chain Left": + System.out.println("L-Chain Source!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideLPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "L-Chain Middle": + System.out.println("L-Chain Middle!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidLPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "L-Chain Right": + System.out.println("L-Chain Amp!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideLPath, DEFAUL_PATH_CONSTRAINTS); + case "Mid-Chain Left": + System.out.println("Mid-Chain Source!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideMPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "Mid-Chain Middle": + System.out.println("Mid-Chain Middle!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidMPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "Mid-Chain Right": + System.out.println("Mid-Chain Amp!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideMPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "R-Chain Left": + System.out.println("R-Chain Source!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbSourceSideRPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "R-Chain Middle": + System.out.println("R-Chain Middle!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbMidRPath, DEFAUL_PATH_CONSTRAINTS); + break; + case "R-Chain Right": + System.out.println("R-Chain Amp!"); + actualCommand = AutoBuilder.pathfindThenFollowPath(climbAmpSideRPath, DEFAUL_PATH_CONSTRAINTS); + break; + default: + System.out.println("We broke!(Or went to the default command)"); + break; + } + + actualCommand.initialize(); +} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + actualCommand.execute(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return actualCommand.isFinished(); + } +} diff --git a/src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java b/src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java deleted file mode 100644 index f00673d8..00000000 --- a/src/main/java/frc/robot/commands/goToPose/GoToNearestClimbSpot.java +++ /dev/null @@ -1,92 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands.goToPose; - -import static frc.robot.settings.Constants.DriveConstants.DEFAUL_PATH_CONSTRAINTS; - -import java.util.function.BooleanSupplier; - -import com.pathplanner.lib.auto.AutoBuilder; - -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.settings.Constants.PathConstants; -import frc.robot.subsystems.DrivetrainSubsystem; - -public class GoToNearestClimbSpot extends Command { - Command actualCommand; - DrivetrainSubsystem drivetrain; - SendableChooser climbSpotChooser; - /** Creates a new GoToAmp. */ - public GoToNearestClimbSpot(DrivetrainSubsystem drivetrain, SendableChooser climbSpotChooser) { - this.climbSpotChooser = climbSpotChooser; - this.drivetrain = drivetrain; - addRequirements(drivetrain); - // Use addRequirements() here to declare subsystem dependencies. - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - switch (climbSpotChooser.getSelected()) { - case "L-Chain Left": - System.out.println("L-Chain Left!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_LL_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "L-Chain Middle": - System.out.println("L-Chain Middle!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_LM_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "L-Chain Right": - System.out.println("L-Chain Right!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_LR_POSE, DEFAUL_PATH_CONSTRAINTS); - case "Mid-Chain Left": - System.out.println("Mid-Chain Left!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_ML_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "Mid-Chain Middle": - System.out.println("Mid-Chain Middle!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_MM_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "Mid-Chain Right": - System.out.println("Mid-Chain Right!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_MR_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "R-Chain Left": - System.out.println("R-Chain Left!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RL_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "R-Chain Middle": - System.out.println("R-Chain Middle!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RM_POSE, DEFAUL_PATH_CONSTRAINTS); - break; - case "R-Chain Right": - System.out.println("R-Chain Right!"); - actualCommand = AutoBuilder.pathfindToPose(PathConstants.CLIMB_RR_POSE, DEFAUL_PATH_CONSTRAINTS); - default: - System.out.println("We broke!(Or went to the default command)"); - break; - } - - actualCommand.initialize(); -} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - actualCommand.execute(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return actualCommand.isFinished(); - } -} From 2388fceec36fbdd533df13e425dbc133d5bb5aba Mon Sep 17 00:00:00 2001 From: Rowan Flood Date: Thu, 25 Jan 2024 22:09:27 -0600 Subject: [PATCH 5/8] added lineup paths and made close notes auto and paths --- .pathplanner/settings.json | 7 +- .../autos/StartAmpScore3Close.auto | 43 +++++++++ .../pathplanner/paths/CloseLeftPickup.path | 70 ++++++++++++++ .../paths/CloseMiddlePickupFromLeft.path | 70 ++++++++++++++ .../paths/CloseRightPickupFromMiddle.path | 92 +++++++++++++++++++ .../pathplanner/paths/climbAmpSideL.path | 52 +++++++++++ .../pathplanner/paths/climbAmpSideM.path | 52 +++++++++++ .../pathplanner/paths/climbAmpSideR.path | 52 +++++++++++ .../deploy/pathplanner/paths/climbMidL.path | 52 +++++++++++ .../deploy/pathplanner/paths/climbMidM.path | 52 +++++++++++ .../deploy/pathplanner/paths/climbMidR.path | 52 +++++++++++ .../pathplanner/paths/climbSourceSideL.path | 52 +++++++++++ .../pathplanner/paths/climbSourceSideM.path | 52 +++++++++++ .../pathplanner/paths/climbSourceSideR.path | 52 +++++++++++ .../deploy/pathplanner/paths/goToAmp.path | 52 +++++++++++ vendordeps/PathplannerLib.json | 6 +- 16 files changed, 804 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto create mode 100644 src/main/deploy/pathplanner/paths/CloseLeftPickup.path create mode 100644 src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path create mode 100644 src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path create mode 100644 src/main/deploy/pathplanner/paths/climbAmpSideL.path create mode 100644 src/main/deploy/pathplanner/paths/climbAmpSideM.path create mode 100644 src/main/deploy/pathplanner/paths/climbAmpSideR.path create mode 100644 src/main/deploy/pathplanner/paths/climbMidL.path create mode 100644 src/main/deploy/pathplanner/paths/climbMidM.path create mode 100644 src/main/deploy/pathplanner/paths/climbMidR.path create mode 100644 src/main/deploy/pathplanner/paths/climbSourceSideL.path create mode 100644 src/main/deploy/pathplanner/paths/climbSourceSideM.path create mode 100644 src/main/deploy/pathplanner/paths/climbSourceSideR.path create mode 100644 src/main/deploy/pathplanner/paths/goToAmp.path diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab015..8aef89ae 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -2,7 +2,12 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], + "pathFolders": [ + "Collect Notes", + "Initial Shots", + "lineup", + "to shooting pos" + ], "autoFolders": [], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, diff --git a/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto b/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto new file mode 100644 index 00000000..df9f0412 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/StartAmpScore3Close.auto @@ -0,0 +1,43 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.087122174244349, + "y": 7.036539787365289 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "intakeOn" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseLeftPickup" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseMiddlePickupFromLeft" + } + }, + { + "type": "path", + "data": { + "pathName": "CloseRightPickupFromMiddle" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CloseLeftPickup.path b/src/main/deploy/pathplanner/paths/CloseLeftPickup.path new file mode 100644 index 00000000..98eef62c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseLeftPickup.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.1132842265655905, + "y": 6.611415508541712 + }, + "prevControl": null, + "nextControl": { + "x": 2.186581516017312, + "y": 6.6993722558837785 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.6117057948372984, + "y": 6.7433506295548105 + }, + "prevControl": { + "x": 2.391813926482133, + "y": 6.684712797993434 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.65, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feedShooter" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -146.67371031288477, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": { + "rotation": -169.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path new file mode 100644 index 00000000..672dabf9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseMiddlePickupFromLeft.path @@ -0,0 +1,70 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.904894952647049, + "y": 6.714031713777714 + }, + "prevControl": null, + "nextControl": { + "x": 3.2127435683442798, + "y": 5.4826372509887875 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.377154468594652, + "y": 5.541275082550166 + }, + "prevControl": { + "x": 2.157262600239487, + "y": 5.48263725098879 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 0.6, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feedShooter" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": { + "rotation": -169.0, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path b/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path new file mode 100644 index 00000000..133c8b95 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CloseRightPickupFromMiddle.path @@ -0,0 +1,92 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 2.421132842265685, + "y": 5.599912914111543 + }, + "prevControl": null, + "nextControl": { + "x": 1.556224826735368, + "y": 5.3800210457563775 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.53840850538844, + "y": 4.133967125077108 + }, + "prevControl": { + "x": 2.318516637033275, + "y": 4.075329293515732 + }, + "nextControl": { + "x": 2.758300373743605, + "y": 4.192604956638484 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.333176094923619, + "y": 4.119307667186764 + }, + "prevControl": { + "x": 2.2782031278348276, + "y": 4.104648209296419 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [ + { + "name": "New Event Marker", + "waypointRelativePos": 1.75, + "command": { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "feedShooter" + } + } + ] + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 152.65012421993018, + "rotateFast": false + }, + "reversed": false, + "folder": "Collect Notes", + "previewStartingState": { + "rotation": -178.1523897340052, + "velocity": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbAmpSideL.path b/src/main/deploy/pathplanner/paths/climbAmpSideL.path new file mode 100644 index 00000000..1b516da4 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbAmpSideL.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.634710983707682, + "y": 5.2774048405239675 + }, + "prevControl": null, + "nextControl": { + "x": 4.693348815269059, + "y": 5.204107551072246 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.6786893573787145, + "y": 5.160129177401212 + }, + "prevControl": { + "x": 4.649370441598026, + "y": 5.204107551072245 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -57.380756928807095, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": -55.30484646876604, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbAmpSideM.path b/src/main/deploy/pathplanner/paths/climbAmpSideM.path new file mode 100644 index 00000000..ca3a06fa --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbAmpSideM.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.180267789107007, + "y": 5.160129177401212 + }, + "prevControl": null, + "nextControl": { + "x": 4.238905620668384, + "y": 5.086831887949491 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.326862368010451, + "y": 4.998875140607424 + }, + "prevControl": { + "x": 4.297543452229762, + "y": 5.042853514278457 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -57.380756928807095, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": -55.30484646876604, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbAmpSideR.path b/src/main/deploy/pathplanner/paths/climbAmpSideR.path new file mode 100644 index 00000000..0f2f7369 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbAmpSideR.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.6085489313835772, + "y": 4.705685982800538 + }, + "prevControl": null, + "nextControl": { + "x": 3.6671867629449544, + "y": 4.632388693348816 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.667186762944955, + "y": 4.64704815123916 + }, + "prevControl": { + "x": 3.593889473493233, + "y": 4.749664356471571 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": -57.380756928807095, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": -55.30484646876604, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbMidL.path b/src/main/deploy/pathplanner/paths/climbMidL.path new file mode 100644 index 00000000..8e23f6a6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbMidL.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.9540621938386735, + "y": 3.7 + }, + "prevControl": null, + "nextControl": { + "x": 5.8601958637133835, + "y": 3.7 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.836786530715918, + "y": 3.7 + }, + "prevControl": { + "x": 5.88964195781006, + "y": 3.7 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.4088597288054, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 178.3634229583833, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbMidM.path b/src/main/deploy/pathplanner/paths/climbMidM.path new file mode 100644 index 00000000..40b7e85e --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbMidM.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.9540621938386735, + "y": 4.1 + }, + "prevControl": null, + "nextControl": { + "x": 5.8601958637133835, + "y": 4.1 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.836786530715918, + "y": 4.1 + }, + "prevControl": { + "x": 5.88964195781006, + "y": 4.1 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.4088597288054, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 178.3634229583833, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbMidR.path b/src/main/deploy/pathplanner/paths/climbMidR.path new file mode 100644 index 00000000..9953e40c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbMidR.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 5.9540621938386735, + "y": 4.471134656555028 + }, + "prevControl": null, + "nextControl": { + "x": 5.8601958637133835, + "y": 4.471134656555028 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.836786530715918, + "y": 4.471134656555028 + }, + "prevControl": { + "x": 5.88964195781006, + "y": 4.471134656555028 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.4088597288054, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 178.3634229583833, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbSourceSideL.path b/src/main/deploy/pathplanner/paths/climbSourceSideL.path new file mode 100644 index 00000000..0db98439 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbSourceSideL.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 3.9457164628614976, + "y": 3.3130374832178244 + }, + "prevControl": null, + "nextControl": { + "x": 4.048332668093908, + "y": 3.4449726042309234 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.033673210203563, + "y": 3.430313146340579 + }, + "prevControl": { + "x": 3.960375920751842, + "y": 3.3423563989985134 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 57.57308675850948, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 55.92280471986929, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbSourceSideM.path b/src/main/deploy/pathplanner/paths/climbSourceSideM.path new file mode 100644 index 00000000..478e9ac3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbSourceSideM.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.297543452229762, + "y": 3.0051888675205927 + }, + "prevControl": null, + "nextControl": { + "x": 4.400159657462172, + "y": 3.1371239885336917 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.414819115352517, + "y": 3.1371239885336917 + }, + "prevControl": { + "x": 4.341521825900796, + "y": 3.049167241191626 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 57.57308675850948, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 55.92280471986929, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/climbSourceSideR.path b/src/main/deploy/pathplanner/paths/climbSourceSideR.path new file mode 100644 index 00000000..77b0f5fb --- /dev/null +++ b/src/main/deploy/pathplanner/paths/climbSourceSideR.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 4.69334881526906, + "y": 3.1078050727530036 + }, + "prevControl": null, + "nextControl": { + "x": 4.79596502050147, + "y": 3.2397401937661026 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.781305562611125, + "y": 3.1957618200950693 + }, + "prevControl": { + "x": 4.708008273159404, + "y": 3.1078050727530036 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 57.57308675850948, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 55.92280471986929, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/goToAmp.path b/src/main/deploy/pathplanner/paths/goToAmp.path new file mode 100644 index 00000000..119618c1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/goToAmp.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8494139845492417, + "y": 7.315069487275001 + }, + "prevControl": null, + "nextControl": { + "x": 1.8494139845492417, + "y": 7.415069487275001 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8347545266588972, + "y": 7.534961355630165 + }, + "prevControl": { + "x": 1.8578011650468185, + "y": 7.437653326881164 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.81845546146276, + "rotateFast": false + }, + "reversed": false, + "folder": "lineup", + "previewStartingState": { + "rotation": 90.0, + "velocity": 0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 3c741462..6ddd312f 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.1.2", + "version": "2024.1.5", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.1.2" + "version": "2024.1.5" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.1.2", + "version": "2024.1.5", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From d231b7ddf09978041234f5499eb02d46e6fbf3a9 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Sat, 27 Jan 2024 10:26:23 -0600 Subject: [PATCH 6/8] resolved errors from changing name of constant --- src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 99711cae..6358ed33 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -193,7 +193,7 @@ private void limelightInit() { limelight = Limelight.getInstance(); } private void lightsInst() { - lights = new Lights(Constants.LIGHTS_COUNT-1); + lights = new Lights(Constants.LED_COUNT-1); } diff --git a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java index d3074ec6..385da6b3 100644 --- a/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java +++ b/src/main/java/frc/robot/subsystems/DrivetrainSubsystem.java @@ -267,7 +267,7 @@ public double calculateSpeakerAngle(){ speakerDist = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2)); SmartDashboard.putNumber("dist to speakre", speakerDist); if(speakerDist Date: Sat, 27 Jan 2024 10:32:41 -0600 Subject: [PATCH 7/8] changed name of command --- src/main/java/frc/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6358ed33..2bac18c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -218,7 +218,7 @@ private void configureBindings() { // // new Trigger(driverController::getCrossButton).onTrue(new autoAimParallel(driveTrain)); // new Trigger(driverController::getCrossButton).onTrue(new RotateRobot(driveTrain, driveTrain::calculateSpeakerAngle)); - if(climberExists) {new Trigger(operatorController::getCrossButtonPressed).onTrue(new ClimbCommandGroup(climber, ClimberConstants.CLIMBER_SPEED));} + if(climberExists) {new Trigger(operatorController::getCrossButtonPressed).onTrue(new AutoClimb(climber));} //Intake bindings // new Trigger(operatorController::getL1Button).onTrue(new IntakeCommand(intake, iDirection.INTAKE)); // new Trigger(operatorController::getR1Button).onTrue(new IntakeCommand(intake, iDirection.OUTAKE)); From 6aa0f068286a4bc8a2dc4f6798273eebe62a1a66 Mon Sep 17 00:00:00 2001 From: Liam Sykes <78829557+trixydevs@users.noreply.github.com> Date: Sat, 27 Jan 2024 10:37:11 -0600 Subject: [PATCH 8/8] deleted unnecessary commands --- src/main/java/frc/robot/RobotContainer.java | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2bac18c3..0f8395ee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -101,18 +101,6 @@ public class RobotContainer { private Pigeon2 pigeon; private IndexCommand defaulNoteHandlingCommand; private IndexerSubsystem indexer; - -//BA means B for blue alliance and A amp-side. S is source-side, and M is middle. - private Command climbMidR; - private Command climbMidM; - private Command climbMidL; - private Command climbAmpSideR; - private Command climbAmpSideM; - private Command climbAmpSideL; - private Command climbSourceSideR; - private Command climbSourceSideM; - private Command climbSourceSideL; - private Command goToAmp; private SendableChooser climbSpotChooser; // Replace with CommandPS4Controller or CommandJoystick if needed