From 7e390a7788ad43a61342584fb20e6fbcccae784c Mon Sep 17 00:00:00 2001 From: KaraParikh1 Date: Fri, 21 Nov 2025 02:30:56 -0500 Subject: [PATCH 01/42] added Superstructure and necessary methods, did not add drivetrain idle mode which we need to add --- src/main/java/frc/robot/Superstructure.java | 130 ++++++++++++++ .../frc/robot/commands/DrivetrainCommand.java | 4 +- .../frc/robot/commands/IntakeCommand.java | 7 +- .../java/frc/robot/commands/PivotCommand.java | 6 +- src/main/java/frc/robot/subsystems/Pivot.java | 166 ++++++++++++++++++ 5 files changed, 308 insertions(+), 5 deletions(-) create mode 100644 src/main/java/frc/robot/Superstructure.java diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java new file mode 100644 index 0000000..ac1934e --- /dev/null +++ b/src/main/java/frc/robot/Superstructure.java @@ -0,0 +1,130 @@ +package frc.robot; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.commands.DrivetrainCommand; +import frc.robot.commands.IntakeCommand; +import frc.robot.commands.PivotCommand; +import frc.robot.commands.ShooterCommand; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; + +public class Superstructure { + private final Intake intake; + private final Pivot pivot; + private final Shooter shooter; + private final CommandSwerveDrivetrain drivetrain; + + private SuperState state = SuperState.IDLE; + + public Superstructure( + Intake intake, Pivot pivot, Shooter shooter, CommandSwerveDrivetrain drivetrain) { + this.intake = intake; + this.pivot = pivot; + this.shooter = shooter; + this.drivetrain = drivetrain; + } + + public enum SuperState { + IDLE, + READY_CLOSE_HIGH, + READY_FAR_HIGH, + READY_LOW_SCORE, + INTAKE + } + + public Command toggleCloseHigh() { + return Commands.runOnce( + () -> { + if (state == SuperState.READY_CLOSE_HIGH) { + setState(SuperState.IDLE); + } else { + setState(SuperState.READY_CLOSE_HIGH); + } + }); + } + + public Command toggleFarHigh() { + return Commands.runOnce( + () -> { + if (state == SuperState.READY_FAR_HIGH) { + setState(SuperState.IDLE); + } else { + setState(SuperState.READY_FAR_HIGH); + } + }); + } + + public Command toggleLowScore() { + return Commands.runOnce( + () -> { + if (state == SuperState.READY_LOW_SCORE) { + setState(SuperState.IDLE); + } else { + setState(SuperState.READY_LOW_SCORE); + } + }); + } + + public Command toggleIntake() { + return Commands.runOnce( + () -> { + if (state == SuperState.INTAKE) { + setState(SuperState.IDLE); + } else { + setState(SuperState.INTAKE); + } + }); + } + + public Command action() { + return Commands.runOnce( + () -> { + switch (state) { + case READY_CLOSE_HIGH: + case READY_FAR_HIGH: + new ShooterCommand(shooter, ShooterCommand.Positions.SHOOT); + case READY_LOW_SCORE: + new IntakeCommand(intake, IntakeCommand.Speeds.OUTTAKE_SCORE); + case INTAKE: + new IntakeCommand(intake, IntakeCommand.Speeds.OUTTAKE_SCORE); + case IDLE: + break; + } + }); + } + + private void setState(SuperState newState) { + state = newState; + + switch (newState) { + case IDLE: + new DrivetrainCommand(drivetrain, DrivetrainCommand.Position.IDLE); + new PivotCommand(pivot, PivotCommand.Position.IDLE); + new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); + new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); + case READY_CLOSE_HIGH: + new DrivetrainCommand(drivetrain, pivot.getClosestCosmicConverterDrivetrain()); + new PivotCommand(pivot, pivot.getClosestCosmicConverterPivot()); + new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); + new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); + case READY_FAR_HIGH: + new DrivetrainCommand(drivetrain, pivot.getFarthestCosmicConverterDrivetrain()); + new PivotCommand(pivot, pivot.getFarthestCosmicConverterPivot()); + new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); + new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); + case READY_LOW_SCORE: + new DrivetrainCommand(drivetrain, DrivetrainCommand.Position.IDLE); + new PivotCommand(pivot, PivotCommand.Position.OUTTAKE_SCORE); + new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); + new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); + case INTAKE: + new DrivetrainCommand(drivetrain, DrivetrainCommand.Position.IDLE); + new PivotCommand(pivot, PivotCommand.Position.INTAKE_GROUND); + new IntakeCommand(intake, IntakeCommand.Speeds.INTAKE); + new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); + } + } +} diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index 0f45f97..9dacc3d 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -12,7 +12,8 @@ public class DrivetrainCommand extends Command { public static enum Position { OUTER_COSMIC_CONVERTER, - INNER_COSMIC_CONVERTER + INNER_COSMIC_CONVERTER, + IDLE } private DrivetrainCommand.Position state; @@ -65,6 +66,7 @@ public void initialize() { cosmicConverter.getX() - drivetrain.getState().Pose.getX(), cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); break; + case IDLE: } } diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index a8fef87..15cfdc7 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -8,7 +8,8 @@ public class IntakeCommand extends Command { public static enum Speeds { INTAKE, OUTTAKE_SCORE, - SHOOT + SHOOT, + IDLE } private Speeds speed; @@ -38,6 +39,10 @@ public void initialize() { intake.runInitial(IntakeConfig.K_KICKER_INTAKE_VELOCITY); break; + case IDLE: + intake.stopIntake(); + break; + default: intake.stopIntake(); break; diff --git a/src/main/java/frc/robot/commands/PivotCommand.java b/src/main/java/frc/robot/commands/PivotCommand.java index acffdc7..2fc0518 100644 --- a/src/main/java/frc/robot/commands/PivotCommand.java +++ b/src/main/java/frc/robot/commands/PivotCommand.java @@ -7,7 +7,7 @@ import frc.robot.subsystems.Pivot; public class PivotCommand extends Command { - public static enum Positions { + public static enum Position { INTAKE_GROUND, INTAKE_STAR_SPIRE, OUTTAKE_SCORE, @@ -16,10 +16,10 @@ public static enum Positions { IDLE } - private Positions pose; + private Position pose; Pivot pivot; - public PivotCommand(Pivot pivot, Positions pose) { + public PivotCommand(Pivot pivot, Position pose) { this.pivot = pivot; this.pose = pose; addRequirements(pivot); diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 4a4eb0c..28f375d 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -12,6 +12,8 @@ import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.commands.DrivetrainCommand; +import frc.robot.commands.PivotCommand; import frc.robot.config.CANMappings; import frc.robot.config.PivotConfig; import frc.robot.config.TunerConstants; @@ -180,4 +182,168 @@ public static Translation2d getLocation(int innerouter) { System.out.println("error in getLocation in pivot subsystem"); return new Translation2d(0.0, 0.0); } + + public DrivetrainCommand.Position getClosestCosmicConverterDrivetrain() { + Translation2d currentLocation = + new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); + // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer + // innerouter: 0 - outer, 1 - inner + // getAlliance(): blue - 0, red - 1 + + List locations = + new ArrayList<>( + Arrays.asList( + new Translation2d(4.0, 196.125), + new Translation2d(4.0, 20.5), + new Translation2d(644.0, 196.125), + new Translation2d(644.0, 20.5))); // same order as explained above + + if (Pivot.getAlliance() == 1) { // red + if (Math.sqrt( + Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { + return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + } else { + return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + } + } else if (Pivot.getAlliance() == 0) { // blue + if (Math.sqrt( + Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { + return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + } else { + return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + } + } else { + System.out.println("error in getClosestCosmicConverter() in Pivot"); + return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; + } + } + + public DrivetrainCommand.Position getFarthestCosmicConverterDrivetrain() { + Translation2d currentLocation = + new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); + // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer + // innerouter: 0 - outer, 1 - inner + // getAlliance(): blue - 0, red - 1 + + List locations = + new ArrayList<>( + Arrays.asList( + new Translation2d(4.0, 196.125), + new Translation2d(4.0, 20.5), + new Translation2d(644.0, 196.125), + new Translation2d(644.0, 20.5))); // same order as explained above + + if (Pivot.getAlliance() == 1) { // red + if (Math.sqrt( + Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { + return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + } else { + return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + } + } else if (Pivot.getAlliance() == 0) { // blue + if (Math.sqrt( + Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { + return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + } else { + return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + } + } else { + System.out.println("error in getClosestCosmicConverter() in Pivot"); + return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; + } + } + + public PivotCommand.Position getClosestCosmicConverterPivot() { + Translation2d currentLocation = + new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); + // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer + // innerouter: 0 - outer, 1 - inner + // getAlliance(): blue - 0, red - 1 + + List locations = + new ArrayList<>( + Arrays.asList( + new Translation2d(4.0, 196.125), + new Translation2d(4.0, 20.5), + new Translation2d(644.0, 196.125), + new Translation2d(644.0, 20.5))); // same order as explained above + + if (Pivot.getAlliance() == 1) { // red + if (Math.sqrt( + Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { + return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer + } else { + return PivotCommand.Position.INNER_HIGH_SHOOT; // inner + } + } else if (Pivot.getAlliance() == 0) { // blue + if (Math.sqrt( + Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { + return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer + } else { + return PivotCommand.Position.INNER_HIGH_SHOOT; // inner + } + } else { + System.out.println("error in getClosestCosmicConverter() in Pivot"); + return PivotCommand.Position.OUTER_HIGH_SHOOT; + } + } + + public PivotCommand.Position getFarthestCosmicConverterPivot() { + Translation2d currentLocation = + new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); + // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer + // innerouter: 0 - outer, 1 - inner + // getAlliance(): blue - 0, red - 1 + + List locations = + new ArrayList<>( + Arrays.asList( + new Translation2d(4.0, 196.125), + new Translation2d(4.0, 20.5), + new Translation2d(644.0, 196.125), + new Translation2d(644.0, 20.5))); // same order as explained above + + if (Pivot.getAlliance() == 1) { // red + if (Math.sqrt( + Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { + return PivotCommand.Position.INNER_HIGH_SHOOT; // inner + } else { + return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer + } + } else if (Pivot.getAlliance() == 0) { // blue + if (Math.sqrt( + Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) + > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) + + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { + return PivotCommand.Position.INNER_HIGH_SHOOT; // inner + } else { + return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer + } + } else { + System.out.println("error in getClosestCosmicConverter() in Pivot"); + return PivotCommand.Position.OUTER_HIGH_SHOOT; + } + } } From 3c24fb9b7f3ec9137b10e722f5717ccc6cb1b7c7 Mon Sep 17 00:00:00 2001 From: KaraParikh1 Date: Fri, 21 Nov 2025 15:44:59 -0500 Subject: [PATCH 02/42] added Superstructure and necessary methods, did not add drivetrain idle mode which we need to add --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 24 ++++++++++++++- src/main/java/frc/robot/Superstructure.java | 23 ++++++++++---- .../frc/robot/commands/DrivetrainCommand.java | 30 ++++++++++++++++++- .../java/frc/robot/config/CANMappings.java | 1 + 5 files changed, 72 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1a811b6..94689db 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -41,6 +41,7 @@ public void disabledExit() { @Override public void autonomousInit() { + Superstructure.zeroPigeon(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index defe7f6..c9b1c9a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,16 +4,38 @@ package frc.robot; +import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.config.CANMappings; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Intake; +import frc.robot.subsystems.Pivot; +import frc.robot.subsystems.Shooter; public class RobotContainer { + private CommandXboxController controller = new CommandXboxController(0); + private CommandSwerveDrivetrain drivetrain; + private Intake intake; + private Pivot pivot; + private Shooter shooter; + private final Superstructure superstructure = + new Superstructure(intake, pivot, shooter, drivetrain); + private final Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); + // links xbox controller to controls public RobotContainer() { configureBindings(); } - private void configureBindings() {} + private void configureBindings() { + controller.leftTrigger().onTrue(superstructure.toggleCloseHigh()); + controller.rightTrigger().onTrue(superstructure.action()); + controller.leftBumper().onTrue(superstructure.toggleFarHigh()); + controller.rightBumper().onTrue(superstructure.toggleLowScore()); + controller.rightStick().onTrue(superstructure.toggleIntake()); + } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index ac1934e..013c70f 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,11 +1,14 @@ package frc.robot; +import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.commands.DrivetrainCommand; import frc.robot.commands.IntakeCommand; import frc.robot.commands.PivotCommand; import frc.robot.commands.ShooterCommand; +import frc.robot.config.CANMappings; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Pivot; @@ -101,30 +104,40 @@ private void setState(SuperState newState) { switch (newState) { case IDLE: - new DrivetrainCommand(drivetrain, DrivetrainCommand.Position.IDLE); + new DrivetrainCommand( + drivetrain, DrivetrainCommand.Position.IDLE, new CommandXboxController(0)); new PivotCommand(pivot, PivotCommand.Position.IDLE); new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); case READY_CLOSE_HIGH: - new DrivetrainCommand(drivetrain, pivot.getClosestCosmicConverterDrivetrain()); + new DrivetrainCommand( + drivetrain, pivot.getClosestCosmicConverterDrivetrain(), new CommandXboxController(0)); new PivotCommand(pivot, pivot.getClosestCosmicConverterPivot()); new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); case READY_FAR_HIGH: - new DrivetrainCommand(drivetrain, pivot.getFarthestCosmicConverterDrivetrain()); + new DrivetrainCommand( + drivetrain, pivot.getFarthestCosmicConverterDrivetrain(), new CommandXboxController(0)); new PivotCommand(pivot, pivot.getFarthestCosmicConverterPivot()); new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); case READY_LOW_SCORE: - new DrivetrainCommand(drivetrain, DrivetrainCommand.Position.IDLE); + new DrivetrainCommand( + drivetrain, DrivetrainCommand.Position.IDLE, new CommandXboxController(0)); new PivotCommand(pivot, PivotCommand.Position.OUTTAKE_SCORE); new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); case INTAKE: - new DrivetrainCommand(drivetrain, DrivetrainCommand.Position.IDLE); + new DrivetrainCommand( + drivetrain, DrivetrainCommand.Position.IDLE, new CommandXboxController(0)); new PivotCommand(pivot, PivotCommand.Position.INTAKE_GROUND); new IntakeCommand(intake, IntakeCommand.Speeds.INTAKE); new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); } } + + public static void zeroPigeon() { + Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); + pigeon.reset(); + } } diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index 9dacc3d..f604e56 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -1,11 +1,16 @@ package frc.robot.commands; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; 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.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.config.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Pivot; @@ -16,8 +21,15 @@ public static enum Position { IDLE } + private double MaxSpeed = + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = + RotationsPerSecond.of(1.5) + .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + private DrivetrainCommand.Position state; private CommandSwerveDrivetrain drivetrain; + private CommandXboxController xboxController; private Pose2d robotPose; private Translation2d diff; private Rotation2d targetRotation; @@ -29,9 +41,13 @@ public static enum Position { .withDriveRequestType( SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - public DrivetrainCommand(CommandSwerveDrivetrain drivetrain, DrivetrainCommand.Position state) { + public DrivetrainCommand( + CommandSwerveDrivetrain drivetrain, + DrivetrainCommand.Position state, + CommandXboxController xboxController) { this.drivetrain = drivetrain; this.state = state; + this.xboxController = xboxController; addRequirements(drivetrain); } @@ -67,6 +83,18 @@ public void initialize() { cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); break; case IDLE: + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + new RunCommand( + () -> + drivetrain.setControl( + (new SwerveRequest.FieldCentric() + .withVelocityX(xboxController.getLeftY()) + .withVelocityY(xboxController.getLeftX()) + .withRotationalRate( + xboxController + .getRightX()))))); // Drive counterclockwise with negative X + // (left) } } diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index f6e5e47..eec4ff9 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -7,4 +7,5 @@ public class CANMappings { public static final int K_KICKER_INTAKE_ID = 4; public static final int K_TOP_SHOOTER_ID = 5; public static final int K_BOTTOM_SHOOTER_ID = 6; + public static final int PIGEON_CAN_ID = 0; } From b9a568896ee97bc46a9c23ee7418817430688234 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Sun, 30 Nov 2025 21:03:06 -0500 Subject: [PATCH 03/42] Improved Logging Updates & Added Simulation Fixes --- build.gradle | 4 + simgui-ds.json | 102 ++++++++++++++++++ src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 11 +- src/main/java/frc/robot/Superstructure.java | 2 + .../frc/robot/commands/DrivetrainCommand.java | 2 + .../frc/robot/commands/IntakeCommand.java | 2 + .../java/frc/robot/commands/PivotCommand.java | 2 + .../frc/robot/commands/ShooterCommand.java | 2 + .../java/frc/robot/subsystems/Intake.java | 2 + .../java/frc/robot/subsystems/Shooter.java | 2 + 11 files changed, 133 insertions(+), 6 deletions(-) create mode 100644 simgui-ds.json diff --git a/build.gradle b/build.gradle index ac1ca9e..12ace0a 100644 --- a/build.gradle +++ b/build.gradle @@ -57,6 +57,10 @@ dependencies { implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() + implementation "edu.wpi.first.epilogue:epilogue-runtime-java:2025.3.2" + annotationProcessor "edu.wpi.first.epilogue:epilogue-processor-java:2025.3.2" + + roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..8d4d057 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,102 @@ +{ + "System Joysticks": { + "window": { + "enabled": false + } + }, + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000" + } + ] +} diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 94689db..e1ce52d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -4,18 +4,22 @@ package frc.robot; +import edu.wpi.first.epilogue.Epilogue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -// we'll do nothing in this file too? +@Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; - private final RobotContainer m_robotContainer; + @Logged private final RobotContainer m_robotContainer; public Robot() { m_robotContainer = new RobotContainer(); + + Epilogue.bind(this); } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c9b1c9a..59a2c87 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,21 +5,24 @@ package frc.robot; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.config.CANMappings; +import frc.robot.config.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Shooter; +@Logged public class RobotContainer { private CommandXboxController controller = new CommandXboxController(0); - private CommandSwerveDrivetrain drivetrain; - private Intake intake; - private Pivot pivot; - private Shooter shooter; + private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private Intake intake = new Intake(); + private Pivot pivot = new Pivot(); + private Shooter shooter = new Shooter(); private final Superstructure superstructure = new Superstructure(intake, pivot, shooter, drivetrain); private final Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 013c70f..00e6bb0 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -1,6 +1,7 @@ package frc.robot; import com.ctre.phoenix6.hardware.Pigeon2; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -14,6 +15,7 @@ import frc.robot.subsystems.Pivot; import frc.robot.subsystems.Shooter; +@Logged public class Superstructure { private final Intake intake; private final Pivot pivot; diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java index f604e56..3ad8db9 100644 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ b/src/main/java/frc/robot/commands/DrivetrainCommand.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -14,6 +15,7 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Pivot; +@Logged public class DrivetrainCommand extends Command { public static enum Position { OUTER_COSMIC_CONVERTER, diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java index 15cfdc7..fe787f5 100644 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -1,9 +1,11 @@ package frc.robot.commands; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.config.IntakeConfig; import frc.robot.subsystems.Intake; +@Logged public class IntakeCommand extends Command { public static enum Speeds { INTAKE, diff --git a/src/main/java/frc/robot/commands/PivotCommand.java b/src/main/java/frc/robot/commands/PivotCommand.java index 2fc0518..cdd0f55 100644 --- a/src/main/java/frc/robot/commands/PivotCommand.java +++ b/src/main/java/frc/robot/commands/PivotCommand.java @@ -1,11 +1,13 @@ package frc.robot.commands; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.config.PivotConfig; import frc.robot.subsystems.Pivot; +@Logged public class PivotCommand extends Command { public static enum Position { INTAKE_GROUND, diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java index 9dd06db..41fd071 100644 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ b/src/main/java/frc/robot/commands/ShooterCommand.java @@ -1,9 +1,11 @@ package frc.robot.commands; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.config.ShooterConfig; import frc.robot.subsystems.Shooter; +@Logged public class ShooterCommand extends Command { public static enum Positions { SHOOT, diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index d50fe22..555374e 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -4,10 +4,12 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.CANMappings; import frc.robot.config.IntakeConfig; +@Logged public class Intake extends SubsystemBase { protected TalonFX mInitialIntake; protected TalonFX mKickerIntake; diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 6372e38..bcf7b16 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -4,10 +4,12 @@ import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.CANMappings; import frc.robot.config.ShooterConfig; +@Logged public class Shooter extends SubsystemBase { protected TalonFX mTopShooter; protected TalonFX mBottomShooter; From 96c72d52af1863b7af4cb5c2044fb8d6c83f0bdb Mon Sep 17 00:00:00 2001 From: KaraParikh1 Date: Mon, 1 Dec 2025 16:19:43 -0500 Subject: [PATCH 04/42] added Superstructure and necessary methods, did not add drivetrain idle mode which we need to add --- src/main/java/frc/robot/Robot.java | 68 ++++++++++++++++++- src/main/java/frc/robot/RobotContainer.java | 33 ++++++++- src/main/java/frc/robot/Superstructure.java | 13 +++- .../java/frc/robot/config/VisionConfig.java | 7 ++ .../java/frc/robot/subsystems/Vision.java | 1 + 5 files changed, 118 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index e1ce52d..0e1daee 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,14 +6,22 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.config.TunerConstants; +import frc.robot.config.VisionConfig; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Vision; +import java.util.List; +import org.photonvision.targeting.PhotonPipelineResult; @Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; - + CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @Logged private final RobotContainer m_robotContainer; public Robot() { @@ -26,6 +34,64 @@ public Robot() { public void robotPeriodic() { // loop continuously runs as long as the robot is active CommandScheduler.getInstance().run(); + m_robotContainer.m_odometry.update( + new Rotation2d(RobotContainer.pigeon2.getYaw().getValueAsDouble()), + new SwerveModulePosition[] { + drivetrain.getModule(0).getPosition(true), + drivetrain.getModule(1).getPosition(true), + drivetrain.getModule(2).getPosition(true), + drivetrain.getModule(3).getPosition(true) + }); + + // Updates the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE_STRATEGY + // (not in use) + VisionConfig.photonPoseEstimatorLeft.setReferencePose( + m_robotContainer.m_odometry.getEstimatedPosition()); + VisionConfig.photonPoseEstimatorRight.setReferencePose( + m_robotContainer.m_odometry.getEstimatedPosition()); + + // Puts the pose data from one camera into a list + List results = Vision.leftCameraApril.getAllUnreadResults(); + + // If there is pose data from the cameras, get the latest estimated pose and update the 'vision' + // photon pose estimator + // If there is no multi tag result and the distance from the camera to the target is greater + // than + // 4 meters, return + // Otherwise, add the latest vision pose estimate to a filter with the odometry pose estimate + // and set + // the guessed pose from that to the current pose + if (!results.isEmpty()) { + PhotonPipelineResult result = results.get(results.size() - 1); + VisionConfig.photonPoseEstimatorLeft + .update(result) + .ifPresent( + (pose) -> { + if (result.multitagResult.isEmpty() + && result.targets.get(0).bestCameraToTarget.getTranslation().getNorm() > 4) { + return; + } + m_robotContainer.m_odometry.addVisionMeasurement( + pose.estimatedPose.toPose2d(), pose.timestampSeconds); + }); + } + + results = Vision.rightCameraApril.getAllUnreadResults(); + + if (!results.isEmpty()) { + PhotonPipelineResult result = results.get(results.size() - 1); + VisionConfig.photonPoseEstimatorRight + .update(result) + .ifPresent( + (pose) -> { + if (result.multitagResult.isEmpty() + && result.targets.get(0).bestCameraToTarget.getTranslation().getNorm() > 4) { + return; + } + m_robotContainer.m_odometry.addVisionMeasurement( + pose.estimatedPose.toPose2d(), pose.timestampSeconds); + }); + } } @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59a2c87..95f9765 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,6 +6,15 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.util.Units.*; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -25,7 +34,28 @@ public class RobotContainer { private Shooter shooter = new Shooter(); private final Superstructure superstructure = new Superstructure(intake, pivot, shooter, drivetrain); - private final Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); + public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); + Translation2d m_frontLeftLocation = + new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(10.875)); + Translation2d m_frontRightLocation = + new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(-10.875)); + Translation2d m_backLeftLocation = + new Translation2d(Units.inchesToMeters(-10.875), Units.inchesToMeters(10.875)); + Translation2d m_backRightLocation = + new Translation2d(Units.inchesToMeters(-10.855), Units.inchesToMeters(-10.875)); + SwerveDrivePoseEstimator m_odometry = + new SwerveDrivePoseEstimator( + new SwerveDriveKinematics( + m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation), + pigeon2.getRotation2d(), + new SwerveModulePosition[] { + drivetrain.getModule(0).getPosition(true), + drivetrain.getModule(1).getPosition(true), + drivetrain.getModule(2).getPosition(true), + drivetrain.getModule(3).getPosition(true) + }, + new Pose2d(0.0, 0.0, new Rotation2d())); + static Field2d m_field = new Field2d(); // links xbox controller to controls public RobotContainer() { @@ -38,6 +68,7 @@ private void configureBindings() { controller.leftBumper().onTrue(superstructure.toggleFarHigh()); controller.rightBumper().onTrue(superstructure.toggleLowScore()); controller.rightStick().onTrue(superstructure.toggleIntake()); + controller.a().onTrue(superstructure.incrementPivDeg()); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 00e6bb0..63cc832 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -39,7 +39,6 @@ public enum SuperState { READY_LOW_SCORE, INTAKE } - public Command toggleCloseHigh() { return Commands.runOnce( () -> { @@ -83,7 +82,17 @@ public Command toggleIntake() { } }); } - +// public Command incPivDeg(){ +// return Commands.runOnce( +// ()-> { +// switch (state){ +// case READY_CLOSE_HIGH: +// case READY_FAR_HIGH: +// +// } +// } +// ) +// } public Command action() { return Commands.runOnce( () -> { diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index bf34e01..52d3046 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -8,6 +8,8 @@ import edu.wpi.first.math.util.Units; import java.util.ArrayList; import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; import org.photonvision.PhotonPoseEstimator; public class VisionConfig { @@ -84,4 +86,9 @@ public class VisionConfig { new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); public static final Transform3d REAR_CAMERA_POSITION = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); + Optional visionEst = Optional.empty(); + public static PhotonPoseEstimator photonPoseEstimatorLeft = + new PhotonPoseEstimator(FIELD_LAYOUT, STRATEGY, LEFT_CAMERA_POSITION); + public static PhotonPoseEstimator photonPoseEstimatorRight = + new PhotonPoseEstimator(FIELD_LAYOUT, STRATEGY, RIGHT_CAMERA_POSITION); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 73ae05f..7167fab 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,6 +3,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.VisionConfig; import org.photonvision.PhotonCamera; +import org.photonvision.estimation.*; public class Vision extends SubsystemBase { public static final PhotonCamera leftCameraApril = new PhotonCamera(VisionConfig.CAMERA_NAME); From 0be088256c8cd36c244699369f1f7aacd4eb4efc Mon Sep 17 00:00:00 2001 From: KaraParikh1 Date: Mon, 1 Dec 2025 16:26:42 -0500 Subject: [PATCH 05/42] added Superstructure and necessary methods, did not add drivetrain idle mode which we need to add --- gradlew | 0 src/main/java/frc/robot/RobotContainer.java | 2 +- src/main/java/frc/robot/Superstructure.java | 24 +++++++++++---------- 3 files changed, 14 insertions(+), 12 deletions(-) mode change 100644 => 100755 gradlew diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 95f9765..6994996 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,7 +68,7 @@ private void configureBindings() { controller.leftBumper().onTrue(superstructure.toggleFarHigh()); controller.rightBumper().onTrue(superstructure.toggleLowScore()); controller.rightStick().onTrue(superstructure.toggleIntake()); - controller.a().onTrue(superstructure.incrementPivDeg()); + // controller.a().onTrue(superstructure.incrementPivDeg()); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 63cc832..854eb51 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -39,6 +39,7 @@ public enum SuperState { READY_LOW_SCORE, INTAKE } + public Command toggleCloseHigh() { return Commands.runOnce( () -> { @@ -82,17 +83,18 @@ public Command toggleIntake() { } }); } -// public Command incPivDeg(){ -// return Commands.runOnce( -// ()-> { -// switch (state){ -// case READY_CLOSE_HIGH: -// case READY_FAR_HIGH: -// -// } -// } -// ) -// } + + // public Command incPivDeg(){ + // return Commands.runOnce( + // ()-> { + // switch (state){ + // case READY_CLOSE_HIGH: + // case READY_FAR_HIGH: + // + // } + // } + // ) + // } public Command action() { return Commands.runOnce( () -> { From cdf2bef2f86e5f9f0bced61f238937c0ff5b11da Mon Sep 17 00:00:00 2001 From: KaraParikh1 Date: Mon, 1 Dec 2025 16:58:20 -0500 Subject: [PATCH 06/42] added Superstructure and necessary methods, did not add drivetrain idle mode which we need to add --- src/main/java/frc/robot/RobotContainer.java | 3 +- src/main/java/frc/robot/Superstructure.java | 48 ++++++++++++++----- .../java/frc/robot/commands/PivotCommand.java | 6 ++- .../java/frc/robot/config/PivotConfig.java | 2 + 4 files changed, 45 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6994996..cd3edee 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -68,7 +68,8 @@ private void configureBindings() { controller.leftBumper().onTrue(superstructure.toggleFarHigh()); controller.rightBumper().onTrue(superstructure.toggleLowScore()); controller.rightStick().onTrue(superstructure.toggleIntake()); - // controller.a().onTrue(superstructure.incrementPivDeg()); + controller.povUp().onTrue(superstructure.incPivDegUp()); + controller.povDown().onTrue(superstructure.incPivDegDown()); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 854eb51..1115cb2 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -10,6 +10,7 @@ import frc.robot.commands.PivotCommand; import frc.robot.commands.ShooterCommand; import frc.robot.config.CANMappings; +import frc.robot.config.PivotConfig; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Intake; import frc.robot.subsystems.Pivot; @@ -84,17 +85,42 @@ public Command toggleIntake() { }); } - // public Command incPivDeg(){ - // return Commands.runOnce( - // ()-> { - // switch (state){ - // case READY_CLOSE_HIGH: - // case READY_FAR_HIGH: - // - // } - // } - // ) - // } + public Command incPivDegUp() { + return Commands.runOnce( + () -> { + switch (state) { + case READY_CLOSE_HIGH: + case READY_FAR_HIGH: + case READY_LOW_SCORE: + case INTAKE: + case IDLE: + incrementPivDegreeUp(); + } + }); + } + + public Command incPivDegDown() { + return Commands.runOnce( + () -> { + switch (state) { + case READY_CLOSE_HIGH: + case READY_FAR_HIGH: + case READY_LOW_SCORE: + case INTAKE: + case IDLE: + incrementPivDegreeDown(); + } + }); + } + + public static void incrementPivDegreeUp() { + PivotConfig.ANGLE_ADD++; + } + + public static void incrementPivDegreeDown() { + PivotConfig.ANGLE_ADD--; + } + public Command action() { return Commands.runOnce( () -> { diff --git a/src/main/java/frc/robot/commands/PivotCommand.java b/src/main/java/frc/robot/commands/PivotCommand.java index cdd0f55..23f6d5a 100644 --- a/src/main/java/frc/robot/commands/PivotCommand.java +++ b/src/main/java/frc/robot/commands/PivotCommand.java @@ -50,11 +50,13 @@ public void initialize() { break; case INNER_HIGH_SHOOT: - pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(1))); + // pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(1))); + pivot.setPivotAngle(new Rotation2d(PivotConfig.ANGLE_ADD)); break; case OUTER_HIGH_SHOOT: - pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(0))); + // pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(0))); + pivot.setPivotAngle(new Rotation2d(PivotConfig.ANGLE_ADD)); break; default: diff --git a/src/main/java/frc/robot/config/PivotConfig.java b/src/main/java/frc/robot/config/PivotConfig.java index e943121..92fefa8 100644 --- a/src/main/java/frc/robot/config/PivotConfig.java +++ b/src/main/java/frc/robot/config/PivotConfig.java @@ -25,4 +25,6 @@ public class PivotConfig { public static final double PIVOT_GROUND_INTAKE_ANGLE = 0.0; public static final double PIVOT_OUTTAKE_ANGLE = 0.0; public static final double PIVOT_IDLE_ANGLE = 0.0; + + public static double ANGLE_ADD = 0.0; } From ee2d90d71aa753567aa057da6a4f58b6cb089d7a Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 2 Dec 2025 08:22:36 -0500 Subject: [PATCH 07/42] Testing changes --- simgui-ds.json | 5 ----- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++++++++----- src/main/java/frc/robot/Superstructure.java | 11 ++++++++++ .../java/frc/robot/config/CANMappings.java | 12 +++++------ .../java/frc/robot/config/IntakeConfig.java | 4 ++-- .../java/frc/robot/config/PivotConfig.java | 4 ++-- .../java/frc/robot/config/ShooterConfig.java | 2 +- .../CommandSwerveDrivetrainLogger.java | 18 ++++++++++++++++ src/main/java/frc/robot/subsystems/Pivot.java | 16 +++++++++----- .../java/frc/robot/subsystems/Shooter.java | 4 ++-- 10 files changed, 69 insertions(+), 28 deletions(-) create mode 100644 src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java diff --git a/simgui-ds.json b/simgui-ds.json index 8d4d057..49cf3aa 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,9 +1,4 @@ { - "System Joysticks": { - "window": { - "enabled": false - } - }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd3edee..e4566b2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,12 +64,23 @@ public RobotContainer() { private void configureBindings() { controller.leftTrigger().onTrue(superstructure.toggleCloseHigh()); - controller.rightTrigger().onTrue(superstructure.action()); - controller.leftBumper().onTrue(superstructure.toggleFarHigh()); - controller.rightBumper().onTrue(superstructure.toggleLowScore()); - controller.rightStick().onTrue(superstructure.toggleIntake()); - controller.povUp().onTrue(superstructure.incPivDegUp()); + // controller.rightTrigger().onTrue(superstructure.action()); + // controller.leftBumper().onTrue(superstructure.toggleFarHigh()); + // controller.rightBumper().onTrue(superstructure.toggleLowScore()); + // controller.rightStick().onTrue(superstructure.toggleIntake()); + controller.povUp().whileTrue((Commands.run(() -> pivot.setPivotAngleRot(0.17)))); controller.povDown().onTrue(superstructure.incPivDegDown()); + controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); + controller + .rightTrigger() + .whileTrue( + Commands.run(() -> shooter.shoot(1)) + .alongWith(Commands.run(() -> intake.runKicker(-0.3))) + .alongWith(Commands.run(() -> intake.runInitial(-0.5)))); + // controller.b().whileTrue((Commands.run(() -> intake.runInitial(-0.5)))); + controller + .a() + .onTrue(Commands.run(() -> intake.stopKicker()).andThen(() -> shooter.stopShooter())); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java index 1115cb2..ea8277e 100644 --- a/src/main/java/frc/robot/Superstructure.java +++ b/src/main/java/frc/robot/Superstructure.java @@ -48,6 +48,7 @@ public Command toggleCloseHigh() { setState(SuperState.IDLE); } else { setState(SuperState.READY_CLOSE_HIGH); + System.out.println("Toggle close high"); } }); } @@ -90,11 +91,20 @@ public Command incPivDegUp() { () -> { switch (state) { case READY_CLOSE_HIGH: + incrementPivDegreeUp(); + System.out.println("pivot up"); case READY_FAR_HIGH: + incrementPivDegreeUp(); + System.out.println("pivot up"); case READY_LOW_SCORE: + incrementPivDegreeUp(); + System.out.println("pivot up"); case INTAKE: + incrementPivDegreeUp(); + System.out.println("pivot up"); case IDLE: incrementPivDegreeUp(); + System.out.println("pivot up"); } }); } @@ -109,6 +119,7 @@ public Command incPivDegDown() { case INTAKE: case IDLE: incrementPivDegreeDown(); + System.out.println("Pivot down"); } }); } diff --git a/src/main/java/frc/robot/config/CANMappings.java b/src/main/java/frc/robot/config/CANMappings.java index eec4ff9..569cf28 100644 --- a/src/main/java/frc/robot/config/CANMappings.java +++ b/src/main/java/frc/robot/config/CANMappings.java @@ -1,11 +1,11 @@ package frc.robot.config; public class CANMappings { - public static final int K_PIVOT_LEFT_ID = 1; - public static final int K_PIVOT_RIGHT_ID = 2; - public static final int K_INITIAL_INTAKE_ID = 3; - public static final int K_KICKER_INTAKE_ID = 4; - public static final int K_TOP_SHOOTER_ID = 5; - public static final int K_BOTTOM_SHOOTER_ID = 6; + public static final int K_PIVOT_LEFT_ID = 15; + public static final int K_PIVOT_RIGHT_ID = 16; + public static final int K_INITIAL_INTAKE_ID = 4; + public static final int K_KICKER_INTAKE_ID = 1; + public static final int K_TOP_SHOOTER_ID = 2; + public static final int K_BOTTOM_SHOOTER_ID = 3; public static final int PIGEON_CAN_ID = 0; } diff --git a/src/main/java/frc/robot/config/IntakeConfig.java b/src/main/java/frc/robot/config/IntakeConfig.java index 40c623f..296c478 100644 --- a/src/main/java/frc/robot/config/IntakeConfig.java +++ b/src/main/java/frc/robot/config/IntakeConfig.java @@ -1,8 +1,8 @@ package frc.robot.config; public class IntakeConfig { - public static final double K_INITIAL_INTAKE_STATOR_CURRENT_LIMIT = 120.0; - public static final double K_KICKER_INTAKE_STATOR_CURRENT_LIMIT = 120.0; + public static final double K_INITIAL_INTAKE_STATOR_CURRENT_LIMIT = 80.0; + public static final double K_KICKER_INTAKE_STATOR_CURRENT_LIMIT = 80.0; public static final double K_INITIAL_INTAKE_SUPPLY_CURRENT_LIMIT = 70.0; public static final double K_KICKER_INTAKE_SUPPLY_CURRENT_LIMIT = 70.0; diff --git a/src/main/java/frc/robot/config/PivotConfig.java b/src/main/java/frc/robot/config/PivotConfig.java index 92fefa8..fdc316c 100644 --- a/src/main/java/frc/robot/config/PivotConfig.java +++ b/src/main/java/frc/robot/config/PivotConfig.java @@ -3,14 +3,14 @@ public class PivotConfig { public static final double K_PIVOT_ANGLE_TOLERANCE = 0.0006; // in rotations - public static final double K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT = 120.0; + public static final double K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT = 80.0; public static final double K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT = 70.0; public static final double K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY = 3000.0; public static final double K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION = 500.0; public static final double K_LEFT_AND_RIGHT_PIVOT_JERK = 0.0; - public static final double K_LEFT_AND_RIGHT_PIVOT_P = 0.0; + public static final double K_LEFT_AND_RIGHT_PIVOT_P = 6.5; public static final double K_LEFT_AND_RIGHT_PIVOT_I = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_D = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_S = 0.0; diff --git a/src/main/java/frc/robot/config/ShooterConfig.java b/src/main/java/frc/robot/config/ShooterConfig.java index bf29013..1854abc 100644 --- a/src/main/java/frc/robot/config/ShooterConfig.java +++ b/src/main/java/frc/robot/config/ShooterConfig.java @@ -1,7 +1,7 @@ package frc.robot.config; public class ShooterConfig { - public static final double K_TOP_AND_BOTTOM_SHOOTER_STATOR_CURRENT_LIMIT = 120.0; + public static final double K_TOP_AND_BOTTOM_SHOOTER_STATOR_CURRENT_LIMIT = 80.0; public static final double K_TOP_AND_BOTTOM_SHOOTER_SUPPLY_CURRENT_LIMIT = 70.0; public static final double K_TOP_AND_BOTTOM_SHOOTER_MAX_CRUISE_VELOCITY = 3000.0; diff --git a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java new file mode 100644 index 0000000..afaefac --- /dev/null +++ b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java @@ -0,0 +1,18 @@ +package frc.robot.loggers; + +import edu.wpi.first.epilogue.CustomLoggerFor; +import edu.wpi.first.epilogue.logging.ClassSpecificLogger; +import edu.wpi.first.epilogue.logging.EpilogueBackend; +import frc.robot.subsystems.CommandSwerveDrivetrain; + +@CustomLoggerFor(CommandSwerveDrivetrain.class) +public class CommandSwerveDrivetrainLogger extends ClassSpecificLogger { + public CommandSwerveDrivetrainLogger() { + super(CommandSwerveDrivetrain.class); + } + + @Override + protected void update(EpilogueBackend backend, CommandSwerveDrivetrain drivetrain) { + // backend.log(drivetrain.getState().ModulePositions.); + } +} diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 28f375d..5bdf4e9 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Rotation2d; @@ -89,13 +88,12 @@ public Pivot() { leftPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; rightPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - ; + // leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; mPivotLeft.getConfigurator().apply(leftPivotConfig); mPivotRight.getConfigurator().apply(rightPivotConfig); - follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, true); + // follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, false); } public void setPivotAngle(Rotation2d angleSetpoint) { @@ -103,6 +101,12 @@ public void setPivotAngle(Rotation2d angleSetpoint) { mPivotRight.setControl(follower); } + public void setPivotAngleRot(double rotation) { + mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); + mPivotRight.setControl(new MotionMagicVoltage(rotation)); + // mPivotRight.setControl(follower); + } + public void zeroPivot() { mPivotLeft.setPosition(0.0); mPivotRight.setPosition(0.0); @@ -122,8 +126,10 @@ public Rotation2d getHighAngle(Translation2d location) { // location: the cosmic converter we're shooting on - 1 is blue inner, 2 is blue outer, 3 is red // inner, 4 is red outer // want 5-8 calibrations (distance, angle) + // in, InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - map.put(0.0, 0.0); + map.put(59.0, 0.18); + map.put(77.0, 0.0); double distance = Math.sqrt( diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index bcf7b16..5a64a31 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -71,8 +71,8 @@ public Shooter() { // Velocity is rotations per second of motor accounting for SensorToMechanismRatio public void shoot(double velocity) { - mTopShooter.setControl(new DutyCycleOut(velocity)); - mBottomShooter.setControl(new DutyCycleOut(-velocity)); + mTopShooter.setControl(new DutyCycleOut(-velocity)); + mBottomShooter.setControl(new DutyCycleOut(velocity)); } public void stopShooter() { From 8d68fdc54a9ca3f261870587ac3546fd457c0145 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 2 Dec 2025 11:08:55 -0500 Subject: [PATCH 08/42] fixes --- src/main/java/frc/robot/RobotContainer.java | 25 +++++++++--- .../java/frc/robot/subsystems/Intake.java | 5 +++ src/main/java/frc/robot/subsystems/Pivot.java | 39 +++++++++++++++++++ 3 files changed, 64 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e4566b2..1bd0eef 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,6 +5,7 @@ package frc.robot; import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -14,6 +15,7 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units.*; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -68,19 +70,32 @@ private void configureBindings() { // controller.leftBumper().onTrue(superstructure.toggleFarHigh()); // controller.rightBumper().onTrue(superstructure.toggleLowScore()); // controller.rightStick().onTrue(superstructure.toggleIntake()); + + // Pivot to angle controller.povUp().whileTrue((Commands.run(() -> pivot.setPivotAngleRot(0.17)))); - controller.povDown().onTrue(superstructure.incPivDegDown()); + // Zero pivot controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); + // Intake and shoot controller .rightTrigger() .whileTrue( Commands.run(() -> shooter.shoot(1)) - .alongWith(Commands.run(() -> intake.runKicker(-0.3))) - .alongWith(Commands.run(() -> intake.runInitial(-0.5)))); - // controller.b().whileTrue((Commands.run(() -> intake.runInitial(-0.5)))); + .alongWith(Commands.run(() -> intake.intake()))); + // Stop everything controller .a() - .onTrue(Commands.run(() -> intake.stopKicker()).andThen(() -> shooter.stopShooter())); + .onTrue(Commands.run(() -> intake.stopIntake()).andThen(() -> shooter.stopShooter())); + // Intake + controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + // Drive + drivetrain.setControl( + (new SwerveRequest.FieldCentric() + .withVelocityX(controller.getLeftY()) + .withVelocityY(controller.getLeftX()) + .withRotationalRate(controller.getRightX()))); // Drive counterclockwise with negative X + + + } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 555374e..86e590d 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -75,6 +75,11 @@ public void intake(double velocity) { mKickerIntake.setControl(new DutyCycleOut(velocity)); } + public void intake() { + mInitialIntake.setControl(new DutyCycleOut(-0.5)); + mKickerIntake.setControl(new DutyCycleOut(-0.3)); + } + public void runKicker(double velocity) { mKickerIntake.setControl(new DutyCycleOut(velocity)); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 5bdf4e9..ab3d256 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -5,11 +5,15 @@ import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.ctre.phoenix6.swerve.SwerveModule; +import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.DrivetrainCommand; import frc.robot.commands.PivotCommand; @@ -145,6 +149,11 @@ public double getPivotAngleDegrees() { return currentAngle; } + private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType( + SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle + public static int getAlliance() { Optional alliance = DriverStation.getAlliance(); @@ -352,4 +361,34 @@ public PivotCommand.Position getFarthestCosmicConverterPivot() { return PivotCommand.Position.OUTER_HIGH_SHOOT; } } + public Command getCosmicConverter(boolean isInner){ + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } + else{cosmicConverter = new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5));} + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) {cosmicConverter = new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125));} + else{cosmicConverter = new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5));} + } + + drivetrain.setControl( + m_faceAngle + .withVelocityX(0.1) + .withVelocityY(0.1) + // Set the desired direction in Radians + .withTargetDirection( + new Rotation2d( + Math.atan2( + cosmicConverter.getX() - drivetrain.getState().Pose.getX(), + cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); + } + else{cosmicConverter = null; + System.out.println("no alliance detected: likely causing many errors");} + + } } From bd4fd19f6bee4e00e70753f48b90d9887e63e32f Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 2 Dec 2025 11:55:36 -0500 Subject: [PATCH 09/42] added getCosmicConverter math --- src/main/java/frc/robot/RobotContainer.java | 8 +- src/main/java/frc/robot/subsystems/Pivot.java | 89 ++++++++++++------- 2 files changed, 60 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1bd0eef..5e62fc8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; import edu.wpi.first.math.util.Units.*; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -79,8 +78,7 @@ private void configureBindings() { controller .rightTrigger() .whileTrue( - Commands.run(() -> shooter.shoot(1)) - .alongWith(Commands.run(() -> intake.intake()))); + Commands.run(() -> shooter.shoot(1)).alongWith(Commands.run(() -> intake.intake()))); // Stop everything controller .a() @@ -93,11 +91,9 @@ private void configureBindings() { .withVelocityX(controller.getLeftY()) .withVelocityY(controller.getLeftX()) .withRotationalRate(controller.getRightX()))); // Drive counterclockwise with negative X - - - } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index ab3d256..c171b03 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.commands.DrivetrainCommand; import frc.robot.commands.PivotCommand; @@ -149,10 +150,10 @@ public double getPivotAngleDegrees() { return currentAngle; } - private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = - new SwerveRequest.FieldCentricFacingAngle() - .withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle + private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType( + SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle public static int getAlliance() { Optional alliance = DriverStation.getAlliance(); @@ -361,34 +362,60 @@ public PivotCommand.Position getFarthestCosmicConverterPivot() { return PivotCommand.Position.OUTER_HIGH_SHOOT; } } - public Command getCosmicConverter(boolean isInner){ - Optional alliance1 = DriverStation.getAlliance(); - Translation2d cosmicConverter = new Translation2d(); - if (alliance1.isPresent()) { - if (alliance1.get() == DriverStation.Alliance.Blue) { - if (isInner) { - cosmicConverter = new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); - } - else{cosmicConverter = new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5));} - } - if (alliance1.get() == DriverStation.Alliance.Red) { - if (isInner) {cosmicConverter = new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125));} - else{cosmicConverter = new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5));} - } - - drivetrain.setControl( - m_faceAngle - .withVelocityX(0.1) - .withVelocityY(0.1) - // Set the desired direction in Radians - .withTargetDirection( - new Rotation2d( - Math.atan2( - cosmicConverter.getX() - drivetrain.getState().Pose.getX(), - cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); + + public Command getCosmicConverter(boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } } - else{cosmicConverter = null; - System.out.println("no alliance detected: likely causing many errors");} + Rotation2d heading = drivetrain.getState().Pose.getRotation(); + + // shooter offset in robot frame (meters) + double shooterOffsetX = 0.20; // forward + double shooterOffsetY = -0.10; // right + + // convert to field frame + double shooterX = + drivetrain.getState().Pose.getX() + + shooterOffsetX * heading.getCos() + - shooterOffsetY * heading.getSin(); + + double shooterY = + drivetrain.getState().Pose.getY() + + shooterOffsetX * heading.getSin() + + shooterOffsetY * heading.getCos(); + + // compute target angle + Rotation2d aimAngle = + new Rotation2d( + Math.atan2(cosmicConverter.getY() - shooterY, cosmicConverter.getX() - shooterX)); + + return Commands.runOnce( + () -> + drivetrain.setControl( + m_faceAngle.withVelocityX(0.1).withVelocityY(0.1).withTargetDirection(aimAngle))); + } else { + cosmicConverter = null; + System.out.println("no alliance detected: likely causing many errors"); + return null; } + } } From 7a891a12c96849bdf58e4d4f1624924b8c73c560 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 2 Dec 2025 15:00:15 -0500 Subject: [PATCH 10/42] Fixed drivetrain controls --- src/main/java/frc/robot/RobotContainer.java | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e62fc8..59952c0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,14 +86,21 @@ private void configureBindings() { // Intake controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); // Drive - drivetrain.setControl( - (new SwerveRequest.FieldCentric() - .withVelocityX(controller.getLeftY()) - .withVelocityY(controller.getLeftX()) - .withRotationalRate(controller.getRightX()))); // Drive counterclockwise with negative X + drivetrain.setDefaultCommand( + Commands.run( + () -> + drivetrain.setControl( + (new SwerveRequest.FieldCentric() + .withVelocityX(controller.getLeftY()) + .withVelocityY(controller.getLeftX()) + .withRotationalRate( + controller.getRightX()))))); // Drive counterclockwise with negative X + // auto align with inner cosmic converter + controller.rightBumper().toggleOnTrue(pivot.getCosmicConverter(true)); + // auto align with outer cosmic converter + controller.rightBumper().toggleOnTrue(pivot.getCosmicConverter(false)); } - public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } From dea60c2f9d08d4980384a5f77e6bb3c80cc66760 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 2 Dec 2025 22:56:33 -0500 Subject: [PATCH 11/42] drivetrain generated, pivot calibrated --- src/main/java/frc/robot/RobotContainer.java | 24 +- src/main/java/frc/robot/Telemetry.java | 127 ++++ .../java/frc/robot/config/PivotConfig.java | 2 +- .../java/frc/robot/config/TunerConstants.java | 544 +++++++++--------- .../subsystems/CommandSwerveDrivetrain.java | 456 ++++++++------- src/main/java/frc/robot/subsystems/Pivot.java | 10 +- tuner-project.json | 1 + 7 files changed, 650 insertions(+), 514 deletions(-) create mode 100644 src/main/java/frc/robot/Telemetry.java create mode 100644 tuner-project.json diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 59952c0..c7f21dc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ 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.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; @@ -70,8 +71,17 @@ private void configureBindings() { // controller.rightBumper().onTrue(superstructure.toggleLowScore()); // controller.rightStick().onTrue(superstructure.toggleIntake()); + InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); + map.put(59.0, 0.18); + map.put(76.5, 0.155); + map.put(96.5, 0.142); + map.put(125.5, 0.13); + map.put(169.5,0.12); + map.put(210.5,0.118); + // Pivot to angle - controller.povUp().whileTrue((Commands.run(() -> pivot.setPivotAngleRot(0.17)))); + controller.povUp().whileTrue((Commands.run(() -> pivot.setPivotAngleRot(map.get(drivetrain.getState().Pose.getTranslation().getDistance(new Translation2d())))))); + controller.povUp().whileFalse(Commands.run(()->pivot.setPivotAngleRot(0.0))); // Zero pivot controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); // Intake and shoot @@ -81,12 +91,16 @@ private void configureBindings() { Commands.run(() -> shooter.shoot(1)).alongWith(Commands.run(() -> intake.intake()))); // Stop everything controller - .a() - .onTrue(Commands.run(() -> intake.stopIntake()).andThen(() -> shooter.stopShooter())); + .rightTrigger() + .whileFalse(Commands.run(() -> intake.stopIntake())); + controller.rightTrigger().whileFalse((Commands.run(() -> shooter.stopShooter()))); // Intake - controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + + controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + shooter.setDefaultCommand(Commands.run(()->shooter.stopShooter())); + intake.setDefaultCommand(Commands.run(()->intake.stopIntake())); // Drive - drivetrain.setDefaultCommand( + drivetrain.setDefaultCommand( Commands.run( () -> drivetrain.setControl( diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java new file mode 100644 index 0000000..7b3feb7 --- /dev/null +++ b/src/main/java/frc/robot/Telemetry.java @@ -0,0 +1,127 @@ +package frc.robot; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.networktables.DoubleArrayPublisher; +import edu.wpi.first.networktables.DoublePublisher; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StructArrayPublisher; +import edu.wpi.first.networktables.StructPublisher; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; + +public class Telemetry { + private final double MaxSpeed; + + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); + + /* Set up the module state Mechanism2d telemetry */ + for (int i = 0; i < 4; ++i) { + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); + } + } + + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { + m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), + }; + + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; + + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } + + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); + + /* Telemeterize each module state to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); + } + } +} diff --git a/src/main/java/frc/robot/config/PivotConfig.java b/src/main/java/frc/robot/config/PivotConfig.java index fdc316c..8123040 100644 --- a/src/main/java/frc/robot/config/PivotConfig.java +++ b/src/main/java/frc/robot/config/PivotConfig.java @@ -10,7 +10,7 @@ public class PivotConfig { public static final double K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION = 500.0; public static final double K_LEFT_AND_RIGHT_PIVOT_JERK = 0.0; - public static final double K_LEFT_AND_RIGHT_PIVOT_P = 6.5; + public static final double K_LEFT_AND_RIGHT_PIVOT_P = 10; public static final double K_LEFT_AND_RIGHT_PIVOT_I = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_D = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_S = 0.0; diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 9329c5d..5cb49e6 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -8,307 +8,279 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; + import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = - new Slot0Configs() - .withKP(100) - .withKI(0) - .withKD(0.5) - .withKS(0.1) - .withKV(2.66) - .withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = - new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = - DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = - SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = - new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively - // low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true)); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.96); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 3.125; - - private static final double kDriveGearRatio = 5.357142857142857; - private static final double kSteerGearRatio = 21.428571428571427; - private static final Distance kWheelRadius = Inches.of(2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 15; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = - new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - ConstantCreator = - new SwerveModuleConstantsFactory< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.3349609375); - private static final boolean kFrontLeftSteerMotorInverted = false; - private static final boolean kFrontLeftEncoderInverted = true; - - private static final Distance kFrontLeftXPos = Inches.of(10.5); - private static final Distance kFrontLeftYPos = Inches.of(10.5); - - // Front Right - private static final int kFrontRightDriveMotorId = 3; - private static final int kFrontRightSteerMotorId = 4; - private static final int kFrontRightEncoderId = 10; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.265625); - private static final boolean kFrontRightSteerMotorInverted = false; - private static final boolean kFrontRightEncoderInverted = true; - - private static final Distance kFrontRightXPos = Inches.of(10.5); - private static final Distance kFrontRightYPos = Inches.of(-10.5); - - // Back Left - private static final int kBackLeftDriveMotorId = 5; - private static final int kBackLeftSteerMotorId = 6; - private static final int kBackLeftEncoderId = 11; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.41845703125); - private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = true; - - private static final Distance kBackLeftXPos = Inches.of(-10.5); - private static final Distance kBackLeftYPos = Inches.of(10.5); - - // Back Right - private static final int kBackRightDriveMotorId = 7; - private static final int kBackRightSteerMotorId = 8; - private static final int kBackRightEncoderId = 12; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.43408203125); - private static final boolean kBackRightSteerMotorInverted = false; - private static final boolean kBackRightEncoderInverted = true; - - private static final Distance kBackRightXPos = Inches.of(-10.5); - private static final Distance kBackRightYPos = Inches.of(-10.5); - - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, - kFrontLeftDriveMotorId, - kFrontLeftEncoderId, - kFrontLeftEncoderOffset, - kFrontLeftXPos, - kFrontLeftYPos, - kInvertLeftSide, - kFrontLeftSteerMotorInverted, - kFrontLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, - kFrontRightDriveMotorId, - kFrontRightEncoderId, - kFrontRightEncoderOffset, - kFrontRightXPos, - kFrontRightYPos, - kInvertRightSide, - kFrontRightSteerMotorInverted, - kFrontRightEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, - kBackLeftDriveMotorId, - kBackLeftEncoderId, - kBackLeftEncoderOffset, - kBackLeftXPos, - kBackLeftYPos, - kInvertLeftSide, - kBackLeftSteerMotorInverted, - kBackLeftEncoderInverted); - public static final SwerveModuleConstants< - TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> - BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, - kBackRightDriveMotorId, - kBackRightEncoderId, - kBackRightEncoderOffset, - kBackRightXPos, - kBackRightYPos, - kInvertRightSide, - kBackRightSteerMotorInverted, - kBackRightEncoderInverted); - - /** - * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot - * program,. - */ - public static CommandSwerveDrivetrain createDrivetrain() { - return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); - } - - /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); - } + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = new Slot0Configs() + .withKP(100).withKI(0).withKD(0.5) + .withKS(0.1).withKV(2.49).withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = new Slot0Configs() + .withKP(0.1).withKI(0).withKD(0) + .withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true) + ); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.54); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 0; + + private static final double kDriveGearRatio = 7.03; + private static final double kSteerGearRatio = 26.09; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 0; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory ConstantCreator = + new SwerveModuleConstantsFactory() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + + // Front Left + private static final int kFrontLeftDriveMotorId = 51; + private static final int kFrontLeftSteerMotorId = 50; + private static final int kFrontLeftEncoderId = 52; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.0185546875); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.875); + private static final Distance kFrontLeftYPos = Inches.of(10.875); + + // Front Right + private static final int kFrontRightDriveMotorId = 21; + private static final int kFrontRightSteerMotorId = 20; + private static final int kFrontRightEncoderId = 22; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.43505859375); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.875); + private static final Distance kFrontRightYPos = Inches.of(-10.875); + + // Back Left + private static final int kBackLeftDriveMotorId = 41; + private static final int kBackLeftSteerMotorId = 40; + private static final int kBackLeftEncoderId = 42; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.31787109375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.875); + private static final Distance kBackLeftYPos = Inches.of(10.875); + + // Back Right + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 30; + private static final int kBackRightEncoderId = 32; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.2109375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.875); + private static final Distance kBackRightYPos = Inches.of(-10.875); + + + public static final SwerveModuleConstants FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, + kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted + ); + public static final SwerveModuleConstants FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, + kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted + ); + public static final SwerveModuleConstants BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, + kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted + ); + public static final SwerveModuleConstants BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, + kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted + ); /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - modules); + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight + ); } + /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set - * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]áµ€, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, - * y, theta]áµ€, with units in meters and radians - * @param modules Constants for each specific module + * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - TalonFX::new, - TalonFX::new, - CANcoder::new, - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, modules + ); + } + + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]ᵀ, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super( + TalonFX::new, TalonFX::new, CANcoder::new, + drivetrainConstants, odometryUpdateFrequency, + odometryStandardDeviation, visionStandardDeviation, modules + ); + } } - } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index d044773..d127487 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -2,9 +2,14 @@ import static edu.wpi.first.units.Units.*; +import java.util.function.Supplier; + import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.swerve.*; +import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; +import com.ctre.phoenix6.swerve.SwerveModuleConstants; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,258 +22,267 @@ 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.config.TunerConstants.TunerSwerveDrivetrain; -import java.util.function.Supplier; + +import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; /** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily - * be used in command-based projects. + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements + * Subsystem so it can easily be used in command-based projects. */ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; - /* 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) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; + /* 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) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = - new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = - new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = - new SwerveRequest.SysIdSwerveRotation(); + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), + null, + this + ) + ); - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = - new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), + null, + this + ) + ); - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = - new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), - new SysIdRoutine.Mechanism( - output -> { + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) + ), + new SysIdRoutine.Mechanism( + output -> { /* output is actually radians per second, but SysId only supports "volts" */ setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); /* also log the requested output for SysId */ SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this)); + }, + null, + this + ) + ); - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); + } } - } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to - * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); + } } - } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - * - *

This constructs the underlying hardware devices, so users should not construct the devices - * themselves. If they need the devices, they can access them through getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to - * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation in the form - * [x, y, theta]áµ€, with units in meters and radians - * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y, - * theta]áµ€, with units in meters and radians - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules) { - super( - drivetrainConstants, - odometryUpdateFrequency, - odometryStandardDeviation, - visionStandardDeviation, - modules); - if (Utils.isSimulation()) { - startSimThread(); + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + *

+ * This constructs the underlying hardware devices, so users should not construct + * the devices themselves. If they need the devices, they can access them through + * getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If + * unspecified or set to 0 Hz, this is 250 Hz on + * CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param visionStandardDeviation The standard deviation for vision calculation + * in the form [x, y, theta]áµ€, with units in meters + * and radians + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules + ) { + super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); + if (Utils.isSimulation()) { + startSimThread(); + } } - } - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - *

// @param request Function returning the request to apply - * - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } - - /** - * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link - * #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + * @param request Function returning the request to apply + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } - /** - * Runs the SysId Dynamic test in the given direction for the routine specified by {@link - * #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } + /** + * Runs the SysId Quasistatic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + /** + * Runs the SysId Dynamic test in the given direction for the routine + * specified by {@link #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance() - .ifPresent( - allianceColor -> { + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } + + @Override + public void periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance().ifPresent(allianceColor -> { setOperatorPerspectiveForward( allianceColor == Alliance.Red ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation); + : kBlueAlliancePerspectiveRotation + ); m_hasAppliedOperatorPerspective = true; - }); + }); + } } - } - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = - new Notifier( - () -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = new Notifier(() -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); - } + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - * - *

Note that the vision measurement standard deviations passed into this method will continue - * to apply to future measurements until a subsequent call to {@link - * #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form - * [x, y, theta]áµ€, with units in meters and radians. - */ - @Override - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs) { - super.addVisionMeasurement( - visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); - } + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + *

+ * Note that the vision measurement standard deviations passed into this method + * will continue to apply to future measurements until a subsequent call to + * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement + * in the form [x, y, theta]áµ€, with units in meters and radians. + */ + @Override + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs + ) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index c171b03..04b22b6 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -32,6 +32,7 @@ public class Pivot extends SubsystemBase { protected TalonFX mPivotRight; protected Follower follower; protected CommandSwerveDrivetrain drivetrain; + InterpolatingDoubleTreeMap map1 = new InterpolatingDoubleTreeMap(); private double currentAngle; @@ -106,6 +107,7 @@ public void setPivotAngle(Rotation2d angleSetpoint) { mPivotRight.setControl(follower); } + public void setPivotAngleRot(double rotation) { mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); mPivotRight.setControl(new MotionMagicVoltage(rotation)); @@ -127,6 +129,8 @@ public boolean pivotAtSetpoint() { <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; } + + public Rotation2d getHighAngle(Translation2d location) { // location: the cosmic converter we're shooting on - 1 is blue inner, 2 is blue outer, 3 is red // inner, 4 is red outer @@ -134,7 +138,11 @@ public Rotation2d getHighAngle(Translation2d location) { // in, InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); map.put(59.0, 0.18); - map.put(77.0, 0.0); + map.put(76.5, 0.155); + map.put(96.5, 0.142); + map.put(125.5, 0.13); + map.put(169.5,0.12); + map.put(210.5,0.118); double distance = Math.sqrt( diff --git a/tuner-project.json b/tuner-project.json new file mode 100644 index 0000000..959f076 --- /dev/null +++ b/tuner-project.json @@ -0,0 +1 @@ +{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":52,"Name":"Front Left CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":50,"Name":"Front Left Turn","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":51,"Name":"Front Left Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.0185546875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":50,"ValidatedDriveId":51,"ValidatedEncoderId":52},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":22,"Name":"Front Right CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":20,"Name":"Front Right Turning","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":21,"Name":"Front Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.43505859375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":20,"ValidatedDriveId":21,"ValidatedEncoderId":22},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":42,"Name":"Back Left CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":40,"Name":"Back Left Turn","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":41,"Name":"Back Left Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.31787109375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":40,"ValidatedDriveId":41,"ValidatedEncoderId":42},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":32,"Name":"Back Right CANCoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":30,"Name":"Back Right Turning","Model":"Talon FX vers. F","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x44","FreeSpeedRps":125.5,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":31,"Name":"Back Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":false,"SelectedEncoderType":"CANcoder","EncoderOffset":0.2109375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":30,"ValidatedDriveId":31,"ValidatedEncoderId":32}],"SwerveOptions":{"kSpeedAt12Volts":4.540338742599189,"Gyro":{"Id":0,"Name":"THE PIGEON","Model":"Pigeon 2 vers. S","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":21.75,"HorizontalTrackSizeInches":21.75,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":0,"SwerveModuleConfiguration":{"ModuleBrand":-1,"DriveRatio":7.03,"SteerRatio":26.09,"CouplingRatio":0.0,"CustomName":null},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"Custom","HasVerifiedDrive":true,"IsValidConfiguration":true}} \ No newline at end of file From 4ff858047ae7e882e5dd22810c30a7526ec08616 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 11:44:07 -0500 Subject: [PATCH 12/42] drivetrain generated, pivot calibrated --- src/main/java/frc/robot/RobotContainer.java | 41 +- src/main/java/frc/robot/Telemetry.java | 190 +++--- .../java/frc/robot/config/TunerConstants.java | 544 +++++++++--------- .../subsystems/CommandSwerveDrivetrain.java | 452 +++++++-------- src/main/java/frc/robot/subsystems/Pivot.java | 7 +- 5 files changed, 637 insertions(+), 597 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c7f21dc..1e5c1ae 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -71,17 +71,28 @@ private void configureBindings() { // controller.rightBumper().onTrue(superstructure.toggleLowScore()); // controller.rightStick().onTrue(superstructure.toggleIntake()); - InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - map.put(59.0, 0.18); - map.put(76.5, 0.155); - map.put(96.5, 0.142); - map.put(125.5, 0.13); - map.put(169.5,0.12); - map.put(210.5,0.118); + InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); + map.put(59.0, 0.18); + map.put(76.5, 0.155); + map.put(96.5, 0.142); + map.put(125.5, 0.13); + map.put(169.5, 0.12); + map.put(210.5, 0.118); // Pivot to angle - controller.povUp().whileTrue((Commands.run(() -> pivot.setPivotAngleRot(map.get(drivetrain.getState().Pose.getTranslation().getDistance(new Translation2d())))))); - controller.povUp().whileFalse(Commands.run(()->pivot.setPivotAngleRot(0.0))); + controller + .povUp() + .whileTrue( + (Commands.run( + () -> + pivot.setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(new Translation2d())))))); + controller.povUp().whileFalse(Commands.run(() -> pivot.setPivotAngleRot(0.0))); // Zero pivot controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); // Intake and shoot @@ -90,17 +101,15 @@ private void configureBindings() { .whileTrue( Commands.run(() -> shooter.shoot(1)).alongWith(Commands.run(() -> intake.intake()))); // Stop everything - controller - .rightTrigger() - .whileFalse(Commands.run(() -> intake.stopIntake())); + controller.rightTrigger().whileFalse(Commands.run(() -> intake.stopIntake())); controller.rightTrigger().whileFalse((Commands.run(() -> shooter.stopShooter()))); // Intake - controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); - shooter.setDefaultCommand(Commands.run(()->shooter.stopShooter())); - intake.setDefaultCommand(Commands.run(()->intake.stopIntake())); + controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); + intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); // Drive - drivetrain.setDefaultCommand( + drivetrain.setDefaultCommand( Commands.run( () -> drivetrain.setControl( diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java index 7b3feb7..8874e67 100644 --- a/src/main/java/frc/robot/Telemetry.java +++ b/src/main/java/frc/robot/Telemetry.java @@ -2,7 +2,6 @@ import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveModulePosition; @@ -21,107 +20,126 @@ import edu.wpi.first.wpilibj.util.Color8Bit; public class Telemetry { - private final double MaxSpeed; + private final double MaxSpeed; - /** - * Construct a telemetry object, with the specified max speed of the robot - * - * @param maxSpeed Maximum speed in meters per second - */ - public Telemetry(double maxSpeed) { - MaxSpeed = maxSpeed; - SignalLogger.start(); + /** + * Construct a telemetry object, with the specified max speed of the robot + * + * @param maxSpeed Maximum speed in meters per second + */ + public Telemetry(double maxSpeed) { + MaxSpeed = maxSpeed; + SignalLogger.start(); - /* Set up the module state Mechanism2d telemetry */ - for (int i = 0; i < 4; ++i) { - SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); - } + /* Set up the module state Mechanism2d telemetry */ + for (int i = 0; i < 4; ++i) { + SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]); } + } - /* What to publish over networktables for telemetry */ - private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); + /* What to publish over networktables for telemetry */ + private final NetworkTableInstance inst = NetworkTableInstance.getDefault(); - /* Robot swerve drive state */ - private final NetworkTable driveStateTable = inst.getTable("DriveState"); - private final StructPublisher drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); - private final StructPublisher driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); - private final StructArrayPublisher driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); - private final StructArrayPublisher driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); - private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish(); - private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish(); + /* Robot swerve drive state */ + private final NetworkTable driveStateTable = inst.getTable("DriveState"); + private final StructPublisher drivePose = + driveStateTable.getStructTopic("Pose", Pose2d.struct).publish(); + private final StructPublisher driveSpeeds = + driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish(); + private final StructArrayPublisher driveModuleStates = + driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModuleTargets = + driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish(); + private final StructArrayPublisher driveModulePositions = + driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish(); + private final DoublePublisher driveTimestamp = + driveStateTable.getDoubleTopic("Timestamp").publish(); + private final DoublePublisher driveOdometryFrequency = + driveStateTable.getDoubleTopic("OdometryFrequency").publish(); - /* Robot pose for field positioning */ - private final NetworkTable table = inst.getTable("Pose"); - private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); - private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); + /* Robot pose for field positioning */ + private final NetworkTable table = inst.getTable("Pose"); + private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish(); + private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish(); - /* Mechanisms to represent the swerve module states */ - private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] { - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - new Mechanism2d(1, 1), - }; - /* A direction and length changing ligament for speed representation */ - private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)), - }; - /* A direction changing and length constant ligament for module direction */ - private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] { - m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5) + /* Mechanisms to represent the swerve module states */ + private final Mechanism2d[] m_moduleMechanisms = + new Mechanism2d[] { + new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), new Mechanism2d(1, 1), + }; + /* A direction and length changing ligament for speed representation */ + private final MechanismLigament2d[] m_moduleSpeeds = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[1] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[2] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + m_moduleMechanisms[3] + .getRoot("RootSpeed", 0.5, 0.5) + .append(new MechanismLigament2d("Speed", 0.5, 0)), + }; + /* A direction changing and length constant ligament for module direction */ + private final MechanismLigament2d[] m_moduleDirections = + new MechanismLigament2d[] { + m_moduleMechanisms[0] + .getRoot("RootDirection", 0.5, 0.5) .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5) + m_moduleMechanisms[1] + .getRoot("RootDirection", 0.5, 0.5) .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5) + m_moduleMechanisms[2] + .getRoot("RootDirection", 0.5, 0.5) .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5) + m_moduleMechanisms[3] + .getRoot("RootDirection", 0.5, 0.5) .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))), - }; + }; - private final double[] m_poseArray = new double[3]; - private final double[] m_moduleStatesArray = new double[8]; - private final double[] m_moduleTargetsArray = new double[8]; + private final double[] m_poseArray = new double[3]; + private final double[] m_moduleStatesArray = new double[8]; + private final double[] m_moduleTargetsArray = new double[8]; - /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ - public void telemeterize(SwerveDriveState state) { - /* Telemeterize the swerve drive state */ - drivePose.set(state.Pose); - driveSpeeds.set(state.Speeds); - driveModuleStates.set(state.ModuleStates); - driveModuleTargets.set(state.ModuleTargets); - driveModulePositions.set(state.ModulePositions); - driveTimestamp.set(state.Timestamp); - driveOdometryFrequency.set(1.0 / state.OdometryPeriod); + /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */ + public void telemeterize(SwerveDriveState state) { + /* Telemeterize the swerve drive state */ + drivePose.set(state.Pose); + driveSpeeds.set(state.Speeds); + driveModuleStates.set(state.ModuleStates); + driveModuleTargets.set(state.ModuleTargets); + driveModulePositions.set(state.ModulePositions); + driveTimestamp.set(state.Timestamp); + driveOdometryFrequency.set(1.0 / state.OdometryPeriod); - /* Also write to log file */ - m_poseArray[0] = state.Pose.getX(); - m_poseArray[1] = state.Pose.getY(); - m_poseArray[2] = state.Pose.getRotation().getDegrees(); - for (int i = 0; i < 4; ++i) { - m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians(); - m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond; - m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians(); - m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; - } + /* Also write to log file */ + m_poseArray[0] = state.Pose.getX(); + m_poseArray[1] = state.Pose.getY(); + m_poseArray[2] = state.Pose.getRotation().getDegrees(); + for (int i = 0; i < 4; ++i) { + m_moduleStatesArray[i * 2 + 0] = state.ModuleStates[i].angle.getRadians(); + m_moduleStatesArray[i * 2 + 1] = state.ModuleStates[i].speedMetersPerSecond; + m_moduleTargetsArray[i * 2 + 0] = state.ModuleTargets[i].angle.getRadians(); + m_moduleTargetsArray[i * 2 + 1] = state.ModuleTargets[i].speedMetersPerSecond; + } - SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); - SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); - SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); - SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); + SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray); + SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray); + SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray); + SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds"); - /* Telemeterize the pose to a Field2d */ - fieldTypePub.set("Field2d"); - fieldPub.set(m_poseArray); + /* Telemeterize the pose to a Field2d */ + fieldTypePub.set("Field2d"); + fieldPub.set(m_poseArray); - /* Telemeterize each module state to a Mechanism2d */ - for (int i = 0; i < 4; ++i) { - m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); - m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); - m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); - } + /* Telemeterize each module state to a Mechanism2d */ + for (int i = 0; i < 4; ++i) { + m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle); + m_moduleDirections[i].setAngle(state.ModuleStates[i].angle); + m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed)); } + } } diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 5cb49e6..6dc4890 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -8,279 +8,307 @@ import com.ctre.phoenix6.signals.*; import com.ctre.phoenix6.swerve.*; import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; - import frc.robot.subsystems.CommandSwerveDrivetrain; // Generated by the Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { - // Both sets of gains need to be tuned to your individual robot. - - // The steer motor uses any SwerveModule.SteerRequestType control request with the - // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput - private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100).withKI(0).withKD(0.5) - .withKS(0.1).withKV(2.49).withKA(0) - .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); - // When using closed-loop control, the drive motor uses the control - // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput - private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); - - // The closed-loop output type to use for the steer motors; - // This affects the PID/FF gains for the steer motors - private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; - // The closed-loop output type to use for the drive motors; - // This affects the PID/FF gains for the drive motors - private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - - // The type of motor used for the drive motor - private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; - // The type of motor used for the drive motor - private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - - // The remote sensor feedback type to use for the steer motors; - // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* - private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - - // The stator current at which the wheels start to slip; - // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); - - // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. - // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); - private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() - .withCurrentLimits( - new CurrentLimitsConfigs() - // Swerve azimuth does not require much torque output, so we can set a relatively low - // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) - .withStatorCurrentLimitEnable(true) - ); - private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); - // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs - private static final Pigeon2Configuration pigeonConfigs = null; - - // CAN bus that the devices are located on; - // All swerve devices must share the same CAN bus - public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); - - // Theoretical free speed (m/s) at 12 V applied output; - // This needs to be tuned to your individual robot - public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.54); - - // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; - // This may need to be tuned to your individual robot - private static final double kCoupleRatio = 0; - - private static final double kDriveGearRatio = 7.03; - private static final double kSteerGearRatio = 26.09; - private static final Distance kWheelRadius = Inches.of(2); - - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; - - private static final int kPigeonId = 0; - - // These are only used for simulation - private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); - private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); - // Simulated voltage necessary to overcome friction - private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); - private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - - public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withCANBusName(kCANBus.getName()) - .withPigeon2Id(kPigeonId) - .withPigeon2Configs(pigeonConfigs); - - private static final SwerveModuleConstantsFactory ConstantCreator = - new SwerveModuleConstantsFactory() - .withDriveMotorGearRatio(kDriveGearRatio) - .withSteerMotorGearRatio(kSteerGearRatio) - .withCouplingGearRatio(kCoupleRatio) - .withWheelRadius(kWheelRadius) - .withSteerMotorGains(steerGains) - .withDriveMotorGains(driveGains) - .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) - .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) - .withSlipCurrent(kSlipCurrent) - .withSpeedAt12Volts(kSpeedAt12Volts) - .withDriveMotorType(kDriveMotorType) - .withSteerMotorType(kSteerMotorType) - .withFeedbackSource(kSteerFeedbackType) - .withDriveMotorInitialConfigs(driveInitialConfigs) - .withSteerMotorInitialConfigs(steerInitialConfigs) - .withEncoderInitialConfigs(encoderInitialConfigs) - .withSteerInertia(kSteerInertia) - .withDriveInertia(kDriveInertia) - .withSteerFrictionVoltage(kSteerFrictionVoltage) - .withDriveFrictionVoltage(kDriveFrictionVoltage); - - - // Front Left - private static final int kFrontLeftDriveMotorId = 51; - private static final int kFrontLeftSteerMotorId = 50; - private static final int kFrontLeftEncoderId = 52; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.0185546875); - private static final boolean kFrontLeftSteerMotorInverted = false; - private static final boolean kFrontLeftEncoderInverted = false; - - private static final Distance kFrontLeftXPos = Inches.of(10.875); - private static final Distance kFrontLeftYPos = Inches.of(10.875); - - // Front Right - private static final int kFrontRightDriveMotorId = 21; - private static final int kFrontRightSteerMotorId = 20; - private static final int kFrontRightEncoderId = 22; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.43505859375); - private static final boolean kFrontRightSteerMotorInverted = false; - private static final boolean kFrontRightEncoderInverted = false; - - private static final Distance kFrontRightXPos = Inches.of(10.875); - private static final Distance kFrontRightYPos = Inches.of(-10.875); - - // Back Left - private static final int kBackLeftDriveMotorId = 41; - private static final int kBackLeftSteerMotorId = 40; - private static final int kBackLeftEncoderId = 42; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.31787109375); - private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = false; - - private static final Distance kBackLeftXPos = Inches.of(-10.875); - private static final Distance kBackLeftYPos = Inches.of(10.875); - - // Back Right - private static final int kBackRightDriveMotorId = 31; - private static final int kBackRightSteerMotorId = 30; - private static final int kBackRightEncoderId = 32; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.2109375); - private static final boolean kBackRightSteerMotorInverted = false; - private static final boolean kBackRightEncoderInverted = false; - - private static final Distance kBackRightXPos = Inches.of(-10.875); - private static final Distance kBackRightYPos = Inches.of(-10.875); - - - public static final SwerveModuleConstants FrontLeft = - ConstantCreator.createModuleConstants( - kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, - kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted - ); - public static final SwerveModuleConstants FrontRight = - ConstantCreator.createModuleConstants( - kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, - kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted - ); - public static final SwerveModuleConstants BackLeft = - ConstantCreator.createModuleConstants( - kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, - kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted - ); - public static final SwerveModuleConstants BackRight = - ConstantCreator.createModuleConstants( - kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, - kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted - ); - + // Both sets of gains need to be tuned to your individual robot. + + // The steer motor uses any SwerveModule.SteerRequestType control request with the + // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput + private static final Slot0Configs steerGains = + new Slot0Configs() + .withKP(100) + .withKI(0) + .withKD(0.5) + .withKS(0.1) + .withKV(2.49) + .withKA(0) + .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); + // When using closed-loop control, the drive motor uses the control + // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput + private static final Slot0Configs driveGains = + new Slot0Configs().withKP(0.1).withKI(0).withKD(0).withKS(0).withKV(0.124); + + // The closed-loop output type to use for the steer motors; + // This affects the PID/FF gains for the steer motors + private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; + // The closed-loop output type to use for the drive motors; + // This affects the PID/FF gains for the drive motors + private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + + // The type of motor used for the drive motor + private static final DriveMotorArrangement kDriveMotorType = + DriveMotorArrangement.TalonFX_Integrated; + // The type of motor used for the drive motor + private static final SteerMotorArrangement kSteerMotorType = + SteerMotorArrangement.TalonFX_Integrated; + + // The remote sensor feedback type to use for the steer motors; + // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* + private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + + // The stator current at which the wheels start to slip; + // This needs to be tuned to your individual robot + private static final Current kSlipCurrent = Amps.of(120.0); + + // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. + // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration steerInitialConfigs = + new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + // Swerve azimuth does not require much torque output, so we can set a relatively + // low + // stator current limit to help avoid brownouts without impacting performance. + .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimitEnable(true)); + private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); + // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs + private static final Pigeon2Configuration pigeonConfigs = null; + + // CAN bus that the devices are located on; + // All swerve devices must share the same CAN bus + public static final CANBus kCANBus = new CANBus("", "./logs/example.hoot"); + + // Theoretical free speed (m/s) at 12 V applied output; + // This needs to be tuned to your individual robot + public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.54); + + // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; + // This may need to be tuned to your individual robot + private static final double kCoupleRatio = 0; + + private static final double kDriveGearRatio = 7.03; + private static final double kSteerGearRatio = 26.09; + private static final Distance kWheelRadius = Inches.of(2); + + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; + + private static final int kPigeonId = 0; + + // These are only used for simulation + private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); + private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); + // Simulated voltage necessary to overcome friction + private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); + private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + + public static final SwerveDrivetrainConstants DrivetrainConstants = + new SwerveDrivetrainConstants() + .withCANBusName(kCANBus.getName()) + .withPigeon2Id(kPigeonId) + .withPigeon2Configs(pigeonConfigs); + + private static final SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + ConstantCreator = + new SwerveModuleConstantsFactory< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>() + .withDriveMotorGearRatio(kDriveGearRatio) + .withSteerMotorGearRatio(kSteerGearRatio) + .withCouplingGearRatio(kCoupleRatio) + .withWheelRadius(kWheelRadius) + .withSteerMotorGains(steerGains) + .withDriveMotorGains(driveGains) + .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) + .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) + .withSlipCurrent(kSlipCurrent) + .withSpeedAt12Volts(kSpeedAt12Volts) + .withDriveMotorType(kDriveMotorType) + .withSteerMotorType(kSteerMotorType) + .withFeedbackSource(kSteerFeedbackType) + .withDriveMotorInitialConfigs(driveInitialConfigs) + .withSteerMotorInitialConfigs(steerInitialConfigs) + .withEncoderInitialConfigs(encoderInitialConfigs) + .withSteerInertia(kSteerInertia) + .withDriveInertia(kDriveInertia) + .withSteerFrictionVoltage(kSteerFrictionVoltage) + .withDriveFrictionVoltage(kDriveFrictionVoltage); + + // Front Left + private static final int kFrontLeftDriveMotorId = 51; + private static final int kFrontLeftSteerMotorId = 50; + private static final int kFrontLeftEncoderId = 52; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.0185546875); + private static final boolean kFrontLeftSteerMotorInverted = false; + private static final boolean kFrontLeftEncoderInverted = false; + + private static final Distance kFrontLeftXPos = Inches.of(10.875); + private static final Distance kFrontLeftYPos = Inches.of(10.875); + + // Front Right + private static final int kFrontRightDriveMotorId = 21; + private static final int kFrontRightSteerMotorId = 20; + private static final int kFrontRightEncoderId = 22; + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.43505859375); + private static final boolean kFrontRightSteerMotorInverted = false; + private static final boolean kFrontRightEncoderInverted = false; + + private static final Distance kFrontRightXPos = Inches.of(10.875); + private static final Distance kFrontRightYPos = Inches.of(-10.875); + + // Back Left + private static final int kBackLeftDriveMotorId = 41; + private static final int kBackLeftSteerMotorId = 40; + private static final int kBackLeftEncoderId = 42; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.31787109375); + private static final boolean kBackLeftSteerMotorInverted = false; + private static final boolean kBackLeftEncoderInverted = false; + + private static final Distance kBackLeftXPos = Inches.of(-10.875); + private static final Distance kBackLeftYPos = Inches.of(10.875); + + // Back Right + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 30; + private static final int kBackRightEncoderId = 32; + private static final Angle kBackRightEncoderOffset = Rotations.of(0.2109375); + private static final boolean kBackRightSteerMotorInverted = false; + private static final boolean kBackRightEncoderInverted = false; + + private static final Distance kBackRightXPos = Inches.of(-10.875); + private static final Distance kBackRightYPos = Inches.of(-10.875); + + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontLeft = + ConstantCreator.createModuleConstants( + kFrontLeftSteerMotorId, + kFrontLeftDriveMotorId, + kFrontLeftEncoderId, + kFrontLeftEncoderOffset, + kFrontLeftXPos, + kFrontLeftYPos, + kInvertLeftSide, + kFrontLeftSteerMotorInverted, + kFrontLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + FrontRight = + ConstantCreator.createModuleConstants( + kFrontRightSteerMotorId, + kFrontRightDriveMotorId, + kFrontRightEncoderId, + kFrontRightEncoderOffset, + kFrontRightXPos, + kFrontRightYPos, + kInvertRightSide, + kFrontRightSteerMotorInverted, + kFrontRightEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackLeft = + ConstantCreator.createModuleConstants( + kBackLeftSteerMotorId, + kBackLeftDriveMotorId, + kBackLeftEncoderId, + kBackLeftEncoderOffset, + kBackLeftXPos, + kBackLeftYPos, + kInvertLeftSide, + kBackLeftSteerMotorInverted, + kBackLeftEncoderInverted); + public static final SwerveModuleConstants< + TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> + BackRight = + ConstantCreator.createModuleConstants( + kBackRightSteerMotorId, + kBackRightDriveMotorId, + kBackRightEncoderId, + kBackRightEncoderOffset, + kBackRightXPos, + kBackRightYPos, + kInvertRightSide, + kBackRightSteerMotorInverted, + kBackRightEncoderInverted); + + /** + * Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot + * program,. + */ + public static CommandSwerveDrivetrain createDrivetrain() { + return new CommandSwerveDrivetrain( + DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight); + } + + /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */ + public static class TunerSwerveDrivetrain extends SwerveDrivetrain { /** - * Creates a CommandSwerveDrivetrain instance. - * This should only be called once in your robot program,. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module */ - public static CommandSwerveDrivetrain createDrivetrain() { - return new CommandSwerveDrivetrain( - DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight - ); + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(TalonFX::new, TalonFX::new, CANcoder::new, drivetrainConstants, modules); } - /** - * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module */ - public static class TunerSwerveDrivetrain extends SwerveDrivetrain { - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, modules - ); - } - - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, modules - ); - } + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + modules); + } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public TunerSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super( - TalonFX::new, TalonFX::new, CANcoder::new, - drivetrainConstants, odometryUpdateFrequency, - odometryStandardDeviation, visionStandardDeviation, modules - ); - } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set + * to 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]ᵀ, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, + * y, theta]ᵀ, with units in meters and radians + * @param modules Constants for each specific module + */ + public TunerSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + TalonFX::new, + TalonFX::new, + CANcoder::new, + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); } + } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index d127487..cd1b65f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -2,14 +2,11 @@ import static edu.wpi.first.units.Units.*; -import java.util.function.Supplier; - import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; - import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -22,267 +19,258 @@ 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.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.config.TunerConstants.TunerSwerveDrivetrain; +import java.util.function.Supplier; /** - * Class that extends the Phoenix 6 SwerveDrivetrain class and implements - * Subsystem so it can easily be used in command-based projects. + * Class that extends the Phoenix 6 SwerveDrivetrain class and implements Subsystem so it can easily + * be used in command-based projects. */ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem { - private static final double kSimLoopPeriod = 0.005; // 5 ms - private Notifier m_simNotifier = null; - private double m_lastSimTime; + private static final double kSimLoopPeriod = 0.005; // 5 ms + private Notifier m_simNotifier = null; + private double m_lastSimTime; - /* 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) */ - private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; - /* Keep track if we've ever applied the operator perspective before or not */ - private boolean m_hasAppliedOperatorPerspective = false; + /* 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) */ + private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; + /* Keep track if we've ever applied the operator perspective before or not */ + private boolean m_hasAppliedOperatorPerspective = false; - /* Swerve requests to apply during SysId characterization */ - private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation(); - private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); - private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + /* Swerve requests to apply during SysId characterization */ + private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = + new SwerveRequest.SysIdSwerveTranslation(); + private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = + new SwerveRequest.SysIdSwerveSteerGains(); + private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = + new SwerveRequest.SysIdSwerveRotation(); - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ - private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdTranslation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> setControl(m_translationCharacterization.withVolts(output)), - null, - this - ) - ); + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ + private final SysIdRoutine m_sysIdRoutineTranslation = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> setControl(m_translationCharacterization.withVolts(output)), null, this)); - /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ - private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine( - new SysIdRoutine.Config( - null, // Use default ramp rate (1 V/s) - Volts.of(7), // Use dynamic voltage of 7 V - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdSteer_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - volts -> setControl(m_steerCharacterization.withVolts(volts)), - null, - this - ) - ); + /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */ + private final SysIdRoutine m_sysIdRoutineSteer = + new SysIdRoutine( + new SysIdRoutine.Config( + null, // Use default ramp rate (1 V/s) + Volts.of(7), // Use dynamic voltage of 7 V + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdSteer_State", state.toString())), + new SysIdRoutine.Mechanism( + volts -> setControl(m_steerCharacterization.withVolts(volts)), null, this)); - /* - * SysId routine for characterizing rotation. - * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. - * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. - */ - private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine( - new SysIdRoutine.Config( - /* This is in radians per second², but SysId only supports "volts per second" */ - Volts.of(Math.PI / 6).per(Second), - /* This is in radians per second, but SysId only supports "volts" */ - Volts.of(Math.PI), - null, // Use default timeout (10 s) - // Log state with SignalLogger class - state -> SignalLogger.writeString("SysIdRotation_State", state.toString()) - ), - new SysIdRoutine.Mechanism( - output -> { + /* + * SysId routine for characterizing rotation. + * This is used to find PID gains for the FieldCentricFacingAngle HeadingController. + * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId. + */ + private final SysIdRoutine m_sysIdRoutineRotation = + new SysIdRoutine( + new SysIdRoutine.Config( + /* This is in radians per second², but SysId only supports "volts per second" */ + Volts.of(Math.PI / 6).per(Second), + /* This is in radians per second, but SysId only supports "volts" */ + Volts.of(Math.PI), + null, // Use default timeout (10 s) + // Log state with SignalLogger class + state -> SignalLogger.writeString("SysIdRotation_State", state.toString())), + new SysIdRoutine.Mechanism( + output -> { /* output is actually radians per second, but SysId only supports "volts" */ setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts))); /* also log the requested output for SysId */ SignalLogger.writeDouble("Rotational_Rate", output.in(Volts)); - }, - null, - this - ) - ); + }, + null, + this)); - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; + /* The SysId routine to test */ + private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, modules); - if (Utils.isSimulation()) { - startSimThread(); - } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, SwerveModuleConstants... modules) { + super(drivetrainConstants, modules); + if (Utils.isSimulation()) { + startSimThread(); } + } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, modules); - if (Utils.isSimulation()) { - startSimThread(); - } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + SwerveModuleConstants... modules) { + super(drivetrainConstants, odometryUpdateFrequency, modules); + if (Utils.isSimulation()) { + startSimThread(); } + } - /** - * Constructs a CTRE SwerveDrivetrain using the specified constants. - *

- * This constructs the underlying hardware devices, so users should not construct - * the devices themselves. If they need the devices, they can access them through - * getters in the classes. - * - * @param drivetrainConstants Drivetrain-wide constants for the swerve drive - * @param odometryUpdateFrequency The frequency to run the odometry loop. If - * unspecified or set to 0 Hz, this is 250 Hz on - * CAN FD, and 100 Hz on CAN 2.0. - * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]áµ€, with units in meters - * and radians - * @param modules Constants for each specific module - */ - public CommandSwerveDrivetrain( - SwerveDrivetrainConstants drivetrainConstants, - double odometryUpdateFrequency, - Matrix odometryStandardDeviation, - Matrix visionStandardDeviation, - SwerveModuleConstants... modules - ) { - super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules); - if (Utils.isSimulation()) { - startSimThread(); - } + /** + * Constructs a CTRE SwerveDrivetrain using the specified constants. + * + *

This constructs the underlying hardware devices, so users should not construct the devices + * themselves. If they need the devices, they can access them through getters in the classes. + * + * @param drivetrainConstants Drivetrain-wide constants for the swerve drive + * @param odometryUpdateFrequency The frequency to run the odometry loop. If unspecified or set to + * 0 Hz, this is 250 Hz on CAN FD, and 100 Hz on CAN 2.0. + * @param odometryStandardDeviation The standard deviation for odometry calculation in the form + * [x, y, theta]áµ€, with units in meters and radians + * @param visionStandardDeviation The standard deviation for vision calculation in the form [x, y, + * theta]áµ€, with units in meters and radians + * @param modules Constants for each specific module + */ + public CommandSwerveDrivetrain( + SwerveDrivetrainConstants drivetrainConstants, + double odometryUpdateFrequency, + Matrix odometryStandardDeviation, + Matrix visionStandardDeviation, + SwerveModuleConstants... modules) { + super( + drivetrainConstants, + odometryUpdateFrequency, + odometryStandardDeviation, + visionStandardDeviation, + modules); + if (Utils.isSimulation()) { + startSimThread(); } + } - /** - * Returns a command that applies the specified control request to this swerve drivetrain. - * - * @param request Function returning the request to apply - * @return Command to run - */ - public Command applyRequest(Supplier requestSupplier) { - return run(() -> this.setControl(requestSupplier.get())); - } + /** + * Returns a command that applies the specified control request to this swerve drivetrain. + * + *

//* @param request Function returning the request to apply + * + * @return Command to run + */ + public Command applyRequest(Supplier requestSupplier) { + return run(() -> this.setControl(requestSupplier.get())); + } - /** - * Runs the SysId Quasistatic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Quasistatic test - * @return Command to run - */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.quasistatic(direction); - } + /** + * Runs the SysId Quasistatic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Quasistatic test + * @return Command to run + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.quasistatic(direction); + } - /** - * Runs the SysId Dynamic test in the given direction for the routine - * specified by {@link #m_sysIdRoutineToApply}. - * - * @param direction Direction of the SysId Dynamic test - * @return Command to run - */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return m_sysIdRoutineToApply.dynamic(direction); - } + /** + * Runs the SysId Dynamic test in the given direction for the routine specified by {@link + * #m_sysIdRoutineToApply}. + * + * @param direction Direction of the SysId Dynamic test + * @return Command to run + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutineToApply.dynamic(direction); + } - @Override - public void periodic() { - /* - * Periodically try to apply the operator perspective. - * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. - * This allows us to correct the perspective in case the robot code restarts mid-match. - * Otherwise, only check and apply the operator perspective if the DS is disabled. - * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. - */ - if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { - DriverStation.getAlliance().ifPresent(allianceColor -> { + @Override + public void periodic() { + /* + * Periodically try to apply the operator perspective. + * If we haven't applied the operator perspective before, then we should apply it regardless of DS state. + * This allows us to correct the perspective in case the robot code restarts mid-match. + * Otherwise, only check and apply the operator perspective if the DS is disabled. + * This ensures driving behavior doesn't change until an explicit disable event occurs during testing. + */ + if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { + DriverStation.getAlliance() + .ifPresent( + allianceColor -> { setOperatorPerspectiveForward( allianceColor == Alliance.Red ? kRedAlliancePerspectiveRotation - : kBlueAlliancePerspectiveRotation - ); + : kBlueAlliancePerspectiveRotation); m_hasAppliedOperatorPerspective = true; - }); - } + }); } + } - private void startSimThread() { - m_lastSimTime = Utils.getCurrentTimeSeconds(); + private void startSimThread() { + m_lastSimTime = Utils.getCurrentTimeSeconds(); - /* Run simulation at a faster rate so PID gains behave more reasonably */ - m_simNotifier = new Notifier(() -> { - final double currentTime = Utils.getCurrentTimeSeconds(); - double deltaTime = currentTime - m_lastSimTime; - m_lastSimTime = currentTime; + /* Run simulation at a faster rate so PID gains behave more reasonably */ + m_simNotifier = + new Notifier( + () -> { + final double currentTime = Utils.getCurrentTimeSeconds(); + double deltaTime = currentTime - m_lastSimTime; + m_lastSimTime = currentTime; - /* use the measured time delta, get battery voltage from WPILib */ - updateSimState(deltaTime, RobotController.getBatteryVoltage()); - }); - m_simNotifier.startPeriodic(kSimLoopPeriod); - } + /* use the measured time delta, get battery voltage from WPILib */ + updateSimState(deltaTime, RobotController.getBatteryVoltage()); + }); + m_simNotifier.startPeriodic(kSimLoopPeriod); + } - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - */ - @Override - public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); - } + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + */ + @Override + public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) { + super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds)); + } - /** - * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate - * while still accounting for measurement noise. - *

- * Note that the vision measurement standard deviations passed into this method - * will continue to apply to future measurements until a subsequent call to - * {@link #setVisionMeasurementStdDevs(Matrix)} or this method. - * - * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. - * @param timestampSeconds The timestamp of the vision measurement in seconds. - * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement - * in the form [x, y, theta]áµ€, with units in meters and radians. - */ - @Override - public void addVisionMeasurement( - Pose2d visionRobotPoseMeters, - double timestampSeconds, - Matrix visionMeasurementStdDevs - ) { - super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); - } + /** + * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate + * while still accounting for measurement noise. + * + *

Note that the vision measurement standard deviations passed into this method will continue + * to apply to future measurements until a subsequent call to {@link + * #setVisionMeasurementStdDevs(Matrix)} or this method. + * + * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera. + * @param timestampSeconds The timestamp of the vision measurement in seconds. + * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement in the form + * [x, y, theta]áµ€, with units in meters and radians. + */ + @Override + public void addVisionMeasurement( + Pose2d visionRobotPoseMeters, + double timestampSeconds, + Matrix visionMeasurementStdDevs) { + super.addVisionMeasurement( + visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); + } } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 04b22b6..4e8875f 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -107,7 +107,6 @@ public void setPivotAngle(Rotation2d angleSetpoint) { mPivotRight.setControl(follower); } - public void setPivotAngleRot(double rotation) { mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); mPivotRight.setControl(new MotionMagicVoltage(rotation)); @@ -129,8 +128,6 @@ public boolean pivotAtSetpoint() { <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; } - - public Rotation2d getHighAngle(Translation2d location) { // location: the cosmic converter we're shooting on - 1 is blue inner, 2 is blue outer, 3 is red // inner, 4 is red outer @@ -141,8 +138,8 @@ public Rotation2d getHighAngle(Translation2d location) { map.put(76.5, 0.155); map.put(96.5, 0.142); map.put(125.5, 0.13); - map.put(169.5,0.12); - map.put(210.5,0.118); + map.put(169.5, 0.12); + map.put(210.5, 0.118); double distance = Math.sqrt( From f17fdf318d7cc10f3142980c3fb26cd38280aa94 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 12:22:56 -0500 Subject: [PATCH 13/42] drivetrain code in robot container complete --- src/main/java/frc/robot/RobotContainer.java | 42 +++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1e5c1ae..c089dea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,7 +4,10 @@ package frc.robot; +import static edu.wpi.first.units.Units.*; + import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -20,6 +23,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import frc.robot.config.CANMappings; import frc.robot.config.TunerConstants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -29,6 +33,25 @@ @Logged public class RobotContainer { + private double MaxSpeed = + TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed + private double MaxAngularRate = + RotationsPerSecond.of(0.75) + .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity + + /* Setting up bindings for necessary control of the swerve drive platform */ + private final SwerveRequest.FieldCentric drive = + new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1) + .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType( + SwerveModule.DriveRequestType + .OpenLoopVoltage); // Use open-loop control for drive motors + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); + private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); + + private final Telemetry logger = new Telemetry(MaxSpeed); + private CommandXboxController controller = new CommandXboxController(0); private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Intake intake = new Intake(); @@ -65,6 +88,24 @@ public RobotContainer() { } private void configureBindings() { + final SwerveRequest.Idle idle = new SwerveRequest.Idle(); + RobotModeTriggers.disabled() + .whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); + + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest( + () -> + drive + .withVelocityX( + -controller.getLeftY() + * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY( + -controller.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate( + -controller.getRightX() + * MaxAngularRate) // Drive counterclockwise with negative X (left) + )); controller.leftTrigger().onTrue(superstructure.toggleCloseHigh()); // controller.rightTrigger().onTrue(superstructure.action()); // controller.leftBumper().onTrue(superstructure.toggleFarHigh()); @@ -122,6 +163,7 @@ private void configureBindings() { controller.rightBumper().toggleOnTrue(pivot.getCosmicConverter(true)); // auto align with outer cosmic converter controller.rightBumper().toggleOnTrue(pivot.getCosmicConverter(false)); + drivetrain.registerTelemetry(logger::telemeterize); } public Command getAutonomousCommand() { From 13cedf86c87fdb46626ae1bdfafc4b48e192d98b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 13:06:25 -0500 Subject: [PATCH 14/42] new robotcontainer controls for testing --- src/main/java/frc/robot/RobotContainer.java | 73 ++++++++++++++----- src/main/java/frc/robot/subsystems/Pivot.java | 26 +++++++ 2 files changed, 79 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c089dea..8c59b37 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -113,14 +113,14 @@ private void configureBindings() { // controller.rightStick().onTrue(superstructure.toggleIntake()); InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - map.put(59.0, 0.18); - map.put(76.5, 0.155); - map.put(96.5, 0.142); - map.put(125.5, 0.13); - map.put(169.5, 0.12); - map.put(210.5, 0.118); - - // Pivot to angle + map.put(Units.inchesToMeters(59.0), 0.18); + map.put(Units.inchesToMeters(76.5), 0.155); + map.put(Units.inchesToMeters(96.5), 0.142); + map.put(Units.inchesToMeters(125.5), 0.13); + map.put(Units.inchesToMeters(169.5), 0.12); + map.put(Units.inchesToMeters(210.5), 0.118); + + // Pivot to angle based off distance controller .povUp() .whileTrue( @@ -132,24 +132,26 @@ private void configureBindings() { .getState() .Pose .getTranslation() - .getDistance(new Translation2d())))))); - controller.povUp().whileFalse(Commands.run(() -> pivot.setPivotAngleRot(0.0))); + .getDistance(pivot.getCosmicConverterTranslation(false))))))); + + // Pivot default command + pivot.setDefaultCommand(Commands.run(() -> pivot.setPivotAngleRot(0.0))); // Zero pivot controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); - // Intake and shoot + // Shoot controller .rightTrigger() .whileTrue( - Commands.run(() -> shooter.shoot(1)).alongWith(Commands.run(() -> intake.intake()))); - // Stop everything - controller.rightTrigger().whileFalse(Commands.run(() -> intake.stopIntake())); - controller.rightTrigger().whileFalse((Commands.run(() -> shooter.stopShooter()))); - // Intake + Commands.run(() -> shooter.shoot(1)) + .alongWith(Commands.run(() -> intake.runKicker(-0.3)))); + // Shooter default command + shooter.setDefaultCommand(Commands.run(()->shooter.stopShooter())); + // Intake controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); - shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); - // Drive + + // Drivetrain default command drivetrain.setDefaultCommand( Commands.run( () -> @@ -160,9 +162,40 @@ private void configureBindings() { .withRotationalRate( controller.getRightX()))))); // Drive counterclockwise with negative X // auto align with inner cosmic converter - controller.rightBumper().toggleOnTrue(pivot.getCosmicConverter(true)); + controller + .rightBumper() + .toggleOnTrue( + pivot + .getCosmicConverter(true) + .alongWith( + Commands.run( + (() -> + pivot.setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance( + pivot.getCosmicConverterTranslation(true)))))))); + // auto align with outer cosmic converter - controller.rightBumper().toggleOnTrue(pivot.getCosmicConverter(false)); + controller + .rightBumper() + .toggleOnTrue( + pivot + .getCosmicConverter(false) + .alongWith( + Commands.run( + (() -> + pivot.setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance( + pivot.getCosmicConverterTranslation(false)))))))); drivetrain.registerTelemetry(logger::telemeterize); } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 4e8875f..c6a5bdc 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -423,4 +423,30 @@ public Command getCosmicConverter(boolean isInner) { return null; } } + + public Translation2d getCosmicConverterTranslation(boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } + } + } + return cosmicConverter; + } } From 7498eaf4c71efb59836b7d74f34aa003c681e68a Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 13:08:02 -0500 Subject: [PATCH 15/42] new robotcontainer controls for testing --- 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 8c59b37..b1115aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -145,7 +145,7 @@ private void configureBindings() { Commands.run(() -> shooter.shoot(1)) .alongWith(Commands.run(() -> intake.runKicker(-0.3)))); // Shooter default command - shooter.setDefaultCommand(Commands.run(()->shooter.stopShooter())); + shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); // Intake controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); From 37eb0ed3d052cbf377309cf95b2edf3b3ca7a8a7 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 13:12:02 -0500 Subject: [PATCH 16/42] new robotcontainer controls for testing --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/subsystems/Intake.java | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b1115aa..1e9b4a2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -196,6 +196,10 @@ private void configureBindings() { .getTranslation() .getDistance( pivot.getCosmicConverterTranslation(false)))))))); + // Outtake + controller.povDown().toggleOnTrue(Commands.run(() -> intake.outtake())); + // Low score + controller.povRight().toggleOnTrue(Commands.run(() -> pivot.setPivotAngleRot(0.0))); drivetrain.registerTelemetry(logger::telemeterize); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 86e590d..298fd86 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -80,6 +80,11 @@ public void intake() { mKickerIntake.setControl(new DutyCycleOut(-0.3)); } + public void outtake() { + mInitialIntake.setControl(new DutyCycleOut(0.5)); + mKickerIntake.setControl(new DutyCycleOut(0.3)); + } + public void runKicker(double velocity) { mKickerIntake.setControl(new DutyCycleOut(velocity)); } From 62efcc7146e128acc60ccdb3de9db9aa52e0dfa6 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 17:05:50 -0500 Subject: [PATCH 17/42] kjhb --- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 62 +++--- src/main/java/frc/robot/Superstructure.java | 193 ------------------ .../frc/robot/commands/DrivetrainCommand.java | 107 ---------- .../java/frc/robot/config/VisionConfig.java | 4 +- .../CommandSwerveDrivetrainLogger.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 5 + src/main/java/frc/robot/subsystems/Pivot.java | 163 ++++++++------- 8 files changed, 114 insertions(+), 424 deletions(-) delete mode 100644 src/main/java/frc/robot/Superstructure.java delete mode 100644 src/main/java/frc/robot/commands/DrivetrainCommand.java diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0e1daee..bfa3737 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -111,7 +111,7 @@ public void disabledExit() { @Override public void autonomousInit() { - Superstructure.zeroPigeon(); + RobotContainer.zeroPigeon(); m_autonomousCommand = m_robotContainer.getAutonomousCommand(); if (m_autonomousCommand != null) { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1e9b4a2..f36ee4c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,7 +18,6 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.util.Units.*; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; @@ -42,7 +41,7 @@ public class RobotContainer { /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) + .withDeadband(MaxSpeed * 0.50) .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType( SwerveModule.DriveRequestType @@ -53,12 +52,10 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); private CommandXboxController controller = new CommandXboxController(0); - private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + private CommandSwerveDrivetrain drivetrain2 = TunerConstants.createDrivetrain(); private Intake intake = new Intake(); private Pivot pivot = new Pivot(); private Shooter shooter = new Shooter(); - private final Superstructure superstructure = - new Superstructure(intake, pivot, shooter, drivetrain); public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(10.875)); @@ -74,10 +71,10 @@ public class RobotContainer { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation), pigeon2.getRotation2d(), new SwerveModulePosition[] { - drivetrain.getModule(0).getPosition(true), - drivetrain.getModule(1).getPosition(true), - drivetrain.getModule(2).getPosition(true), - drivetrain.getModule(3).getPosition(true) + drivetrain2.getModule(0).getPosition(true), + drivetrain2.getModule(1).getPosition(true), + drivetrain2.getModule(2).getPosition(true), + drivetrain2.getModule(3).getPosition(true) }, new Pose2d(0.0, 0.0, new Rotation2d())); static Field2d m_field = new Field2d(); @@ -90,23 +87,8 @@ public RobotContainer() { private void configureBindings() { final SwerveRequest.Idle idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled() - .whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); + .whileTrue(drivetrain2.applyRequest(() -> idle).ignoringDisable(true)); - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - drivetrain.applyRequest( - () -> - drive - .withVelocityX( - -controller.getLeftY() - * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY( - -controller.getLeftX() * MaxSpeed) // Drive left with negative X (left) - .withRotationalRate( - -controller.getRightX() - * MaxAngularRate) // Drive counterclockwise with negative X (left) - )); - controller.leftTrigger().onTrue(superstructure.toggleCloseHigh()); // controller.rightTrigger().onTrue(superstructure.action()); // controller.leftBumper().onTrue(superstructure.toggleFarHigh()); // controller.rightBumper().onTrue(superstructure.toggleLowScore()); @@ -128,14 +110,14 @@ private void configureBindings() { () -> pivot.setPivotAngleRot( map.get( - drivetrain + drivetrain2 .getState() .Pose .getTranslation() .getDistance(pivot.getCosmicConverterTranslation(false))))))); // Pivot default command - pivot.setDefaultCommand(Commands.run(() -> pivot.setPivotAngleRot(0.0))); + controller.povUp().whileFalse(Commands.run(()-> pivot.setPivotAngleRot(0.0))); // Zero pivot controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); // Shoot @@ -145,22 +127,22 @@ private void configureBindings() { Commands.run(() -> shooter.shoot(1)) .alongWith(Commands.run(() -> intake.runKicker(-0.3)))); // Shooter default command - shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); + //shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); // Intake controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); - intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); + //intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); // Drivetrain default command - drivetrain.setDefaultCommand( + drivetrain2.setDefaultCommand( Commands.run( () -> - drivetrain.setControl( + drivetrain2.setControl( (new SwerveRequest.FieldCentric() - .withVelocityX(controller.getLeftY()) - .withVelocityY(controller.getLeftX()) + .withVelocityX(controller.getLeftY()*2.5) + .withVelocityY(controller.getLeftX()*2.5) .withRotationalRate( - controller.getRightX()))))); // Drive counterclockwise with negative X + controller.getRightX()*2.5))), drivetrain2)); // Drive counterclockwise with negative X // auto align with inner cosmic converter controller .rightBumper() @@ -172,13 +154,12 @@ private void configureBindings() { (() -> pivot.setPivotAngleRot( map.get( - drivetrain + drivetrain2 .getState() .Pose .getTranslation() .getDistance( pivot.getCosmicConverterTranslation(true)))))))); - // auto align with outer cosmic converter controller .rightBumper() @@ -190,7 +171,7 @@ private void configureBindings() { (() -> pivot.setPivotAngleRot( map.get( - drivetrain + drivetrain2 .getState() .Pose .getTranslation() @@ -200,10 +181,15 @@ private void configureBindings() { controller.povDown().toggleOnTrue(Commands.run(() -> intake.outtake())); // Low score controller.povRight().toggleOnTrue(Commands.run(() -> pivot.setPivotAngleRot(0.0))); - drivetrain.registerTelemetry(logger::telemeterize); + drivetrain2.registerTelemetry(logger::telemeterize); } public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } + + public static void zeroPigeon() { + Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); + pigeon.reset(); + } } diff --git a/src/main/java/frc/robot/Superstructure.java b/src/main/java/frc/robot/Superstructure.java deleted file mode 100644 index ea8277e..0000000 --- a/src/main/java/frc/robot/Superstructure.java +++ /dev/null @@ -1,193 +0,0 @@ -package frc.robot; - -import com.ctre.phoenix6.hardware.Pigeon2; -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.commands.DrivetrainCommand; -import frc.robot.commands.IntakeCommand; -import frc.robot.commands.PivotCommand; -import frc.robot.commands.ShooterCommand; -import frc.robot.config.CANMappings; -import frc.robot.config.PivotConfig; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Intake; -import frc.robot.subsystems.Pivot; -import frc.robot.subsystems.Shooter; - -@Logged -public class Superstructure { - private final Intake intake; - private final Pivot pivot; - private final Shooter shooter; - private final CommandSwerveDrivetrain drivetrain; - - private SuperState state = SuperState.IDLE; - - public Superstructure( - Intake intake, Pivot pivot, Shooter shooter, CommandSwerveDrivetrain drivetrain) { - this.intake = intake; - this.pivot = pivot; - this.shooter = shooter; - this.drivetrain = drivetrain; - } - - public enum SuperState { - IDLE, - READY_CLOSE_HIGH, - READY_FAR_HIGH, - READY_LOW_SCORE, - INTAKE - } - - public Command toggleCloseHigh() { - return Commands.runOnce( - () -> { - if (state == SuperState.READY_CLOSE_HIGH) { - setState(SuperState.IDLE); - } else { - setState(SuperState.READY_CLOSE_HIGH); - System.out.println("Toggle close high"); - } - }); - } - - public Command toggleFarHigh() { - return Commands.runOnce( - () -> { - if (state == SuperState.READY_FAR_HIGH) { - setState(SuperState.IDLE); - } else { - setState(SuperState.READY_FAR_HIGH); - } - }); - } - - public Command toggleLowScore() { - return Commands.runOnce( - () -> { - if (state == SuperState.READY_LOW_SCORE) { - setState(SuperState.IDLE); - } else { - setState(SuperState.READY_LOW_SCORE); - } - }); - } - - public Command toggleIntake() { - return Commands.runOnce( - () -> { - if (state == SuperState.INTAKE) { - setState(SuperState.IDLE); - } else { - setState(SuperState.INTAKE); - } - }); - } - - public Command incPivDegUp() { - return Commands.runOnce( - () -> { - switch (state) { - case READY_CLOSE_HIGH: - incrementPivDegreeUp(); - System.out.println("pivot up"); - case READY_FAR_HIGH: - incrementPivDegreeUp(); - System.out.println("pivot up"); - case READY_LOW_SCORE: - incrementPivDegreeUp(); - System.out.println("pivot up"); - case INTAKE: - incrementPivDegreeUp(); - System.out.println("pivot up"); - case IDLE: - incrementPivDegreeUp(); - System.out.println("pivot up"); - } - }); - } - - public Command incPivDegDown() { - return Commands.runOnce( - () -> { - switch (state) { - case READY_CLOSE_HIGH: - case READY_FAR_HIGH: - case READY_LOW_SCORE: - case INTAKE: - case IDLE: - incrementPivDegreeDown(); - System.out.println("Pivot down"); - } - }); - } - - public static void incrementPivDegreeUp() { - PivotConfig.ANGLE_ADD++; - } - - public static void incrementPivDegreeDown() { - PivotConfig.ANGLE_ADD--; - } - - public Command action() { - return Commands.runOnce( - () -> { - switch (state) { - case READY_CLOSE_HIGH: - case READY_FAR_HIGH: - new ShooterCommand(shooter, ShooterCommand.Positions.SHOOT); - case READY_LOW_SCORE: - new IntakeCommand(intake, IntakeCommand.Speeds.OUTTAKE_SCORE); - case INTAKE: - new IntakeCommand(intake, IntakeCommand.Speeds.OUTTAKE_SCORE); - case IDLE: - break; - } - }); - } - - private void setState(SuperState newState) { - state = newState; - - switch (newState) { - case IDLE: - new DrivetrainCommand( - drivetrain, DrivetrainCommand.Position.IDLE, new CommandXboxController(0)); - new PivotCommand(pivot, PivotCommand.Position.IDLE); - new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); - new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); - case READY_CLOSE_HIGH: - new DrivetrainCommand( - drivetrain, pivot.getClosestCosmicConverterDrivetrain(), new CommandXboxController(0)); - new PivotCommand(pivot, pivot.getClosestCosmicConverterPivot()); - new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); - new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); - case READY_FAR_HIGH: - new DrivetrainCommand( - drivetrain, pivot.getFarthestCosmicConverterDrivetrain(), new CommandXboxController(0)); - new PivotCommand(pivot, pivot.getFarthestCosmicConverterPivot()); - new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); - new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); - case READY_LOW_SCORE: - new DrivetrainCommand( - drivetrain, DrivetrainCommand.Position.IDLE, new CommandXboxController(0)); - new PivotCommand(pivot, PivotCommand.Position.OUTTAKE_SCORE); - new IntakeCommand(intake, IntakeCommand.Speeds.IDLE); - new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); - case INTAKE: - new DrivetrainCommand( - drivetrain, DrivetrainCommand.Position.IDLE, new CommandXboxController(0)); - new PivotCommand(pivot, PivotCommand.Position.INTAKE_GROUND); - new IntakeCommand(intake, IntakeCommand.Speeds.INTAKE); - new ShooterCommand(shooter, ShooterCommand.Positions.IDLE); - } - } - - public static void zeroPigeon() { - Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); - pigeon.reset(); - } -} diff --git a/src/main/java/frc/robot/commands/DrivetrainCommand.java b/src/main/java/frc/robot/commands/DrivetrainCommand.java deleted file mode 100644 index 3ad8db9..0000000 --- a/src/main/java/frc/robot/commands/DrivetrainCommand.java +++ /dev/null @@ -1,107 +0,0 @@ -package frc.robot.commands; - -import static edu.wpi.first.units.Units.*; - -import com.ctre.phoenix6.swerve.SwerveModule; -import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.epilogue.Logged; -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.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; -import frc.robot.config.TunerConstants; -import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.Pivot; - -@Logged -public class DrivetrainCommand extends Command { - public static enum Position { - OUTER_COSMIC_CONVERTER, - INNER_COSMIC_CONVERTER, - IDLE - } - - private double MaxSpeed = - TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed - private double MaxAngularRate = - RotationsPerSecond.of(1.5) - .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - - private DrivetrainCommand.Position state; - private CommandSwerveDrivetrain drivetrain; - private CommandXboxController xboxController; - private Pose2d robotPose; - private Translation2d diff; - private Rotation2d targetRotation; - private Translation2d cosmicConverter; - - // Initialize the facing angle request - private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = - new SwerveRequest.FieldCentricFacingAngle() - .withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - - public DrivetrainCommand( - CommandSwerveDrivetrain drivetrain, - DrivetrainCommand.Position state, - CommandXboxController xboxController) { - this.drivetrain = drivetrain; - this.state = state; - this.xboxController = xboxController; - - addRequirements(drivetrain); - } - - @Override - public void initialize() { - switch (state) { - case OUTER_COSMIC_CONVERTER: - cosmicConverter = Pivot.getLocation(0); - drivetrain.setControl( - m_faceAngle - .withVelocityX(0) - .withVelocityY(0) - // Set the desired direction in Radians - .withTargetDirection( - new Rotation2d( - Math.atan2( - cosmicConverter.getX() - drivetrain.getState().Pose.getX(), - cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); - break; - - case INNER_COSMIC_CONVERTER: - cosmicConverter = Pivot.getLocation(1); - drivetrain.setControl( - m_faceAngle - .withVelocityX(0) - .withVelocityY(0) - // Set the desired direction in Radians - .withTargetDirection( - new Rotation2d( - Math.atan2( - cosmicConverter.getX() - drivetrain.getState().Pose.getX(), - cosmicConverter.getY() - drivetrain.getState().Pose.getY())))); - break; - case IDLE: - drivetrain.setDefaultCommand( - // Drivetrain will execute this command periodically - new RunCommand( - () -> - drivetrain.setControl( - (new SwerveRequest.FieldCentric() - .withVelocityX(xboxController.getLeftY()) - .withVelocityY(xboxController.getLeftX()) - .withRotationalRate( - xboxController - .getRightX()))))); // Drive counterclockwise with negative X - // (left) - } - } - - @Override - public void end(boolean interrupted) { - drivetrain.setControl(new SwerveRequest.Idle()); - } -} diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index 52d3046..be2460f 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -81,9 +81,9 @@ public class VisionConfig { public static final PhotonPoseEstimator.PoseStrategy STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; public static final Transform3d LEFT_CAMERA_POSITION = - new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); + new Transform3d(Units.inchesToMeters(9.25), Units.inchesToMeters(10.5), Units.inchesToMeters(7.5), new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); public static final Transform3d RIGHT_CAMERA_POSITION = - new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); + new Transform3d(Units.inchesToMeters(9.25), Units.inchesToMeters(-10.5), Units.inchesToMeters(7.5), new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); public static final Transform3d REAR_CAMERA_POSITION = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); Optional visionEst = Optional.empty(); diff --git a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java index afaefac..1ff4986 100644 --- a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java +++ b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java @@ -13,6 +13,6 @@ public CommandSwerveDrivetrainLogger() { @Override protected void update(EpilogueBackend backend, CommandSwerveDrivetrain drivetrain) { - // backend.log(drivetrain.getState().ModulePositions.); + backend.log(drivetrain.getState()); } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index cd1b65f..b7e213f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,9 +18,12 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.config.TunerConstants.TunerSwerveDrivetrain; + +import java.util.function.BooleanSupplier; import java.util.function.Supplier; /** @@ -273,4 +277,5 @@ public void addVisionMeasurement( super.addVisionMeasurement( visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); } + } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index c6a5bdc..417fe79 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -16,7 +16,6 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.DrivetrainCommand; import frc.robot.commands.PivotCommand; import frc.robot.config.CANMappings; import frc.robot.config.PivotConfig; @@ -204,87 +203,87 @@ public static Translation2d getLocation(int innerouter) { return new Translation2d(0.0, 0.0); } - public DrivetrainCommand.Position getClosestCosmicConverterDrivetrain() { - Translation2d currentLocation = - new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); - // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // innerouter: 0 - outer, 1 - inner - // getAlliance(): blue - 0, red - 1 - - List locations = - new ArrayList<>( - Arrays.asList( - new Translation2d(4.0, 196.125), - new Translation2d(4.0, 20.5), - new Translation2d(644.0, 196.125), - new Translation2d(644.0, 20.5))); // same order as explained above - - if (Pivot.getAlliance() == 1) { // red - if (Math.sqrt( - Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { - return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - } else { - return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - } - } else if (Pivot.getAlliance() == 0) { // blue - if (Math.sqrt( - Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { - return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - } else { - return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - } - } else { - System.out.println("error in getClosestCosmicConverter() in Pivot"); - return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; - } - } - - public DrivetrainCommand.Position getFarthestCosmicConverterDrivetrain() { - Translation2d currentLocation = - new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); - // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // innerouter: 0 - outer, 1 - inner - // getAlliance(): blue - 0, red - 1 - - List locations = - new ArrayList<>( - Arrays.asList( - new Translation2d(4.0, 196.125), - new Translation2d(4.0, 20.5), - new Translation2d(644.0, 196.125), - new Translation2d(644.0, 20.5))); // same order as explained above - - if (Pivot.getAlliance() == 1) { // red - if (Math.sqrt( - Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { - return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - } else { - return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - } - } else if (Pivot.getAlliance() == 0) { // blue - if (Math.sqrt( - Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { - return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - } else { - return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - } - } else { - System.out.println("error in getClosestCosmicConverter() in Pivot"); - return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; - } - } +// public DrivetrainCommand.Position getClosestCosmicConverterDrivetrain() { +// Translation2d currentLocation = +// new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); +// // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer +// // innerouter: 0 - outer, 1 - inner +// // getAlliance(): blue - 0, red - 1 +// +// List locations = +// new ArrayList<>( +// Arrays.asList( +// new Translation2d(4.0, 196.125), +// new Translation2d(4.0, 20.5), +// new Translation2d(644.0, 196.125), +// new Translation2d(644.0, 20.5))); // same order as explained above +// +// if (Pivot.getAlliance() == 1) { // red +// if (Math.sqrt( +// Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) +// > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { +// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer +// } else { +// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner +// } +// } else if (Pivot.getAlliance() == 0) { // blue +// if (Math.sqrt( +// Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) +// > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { +// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer +// } else { +// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner +// } +// } else { +// System.out.println("error in getClosestCosmicConverter() in Pivot"); +// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; + // } + // } + +// public DrivetrainCommand.Position getFarthestCosmicConverterDrivetrain() { +// Translation2d currentLocation = +// new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); +// // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer +// // innerouter: 0 - outer, 1 - inner +// // getAlliance(): blue - 0, red - 1 +// +// List locations = +// new ArrayList<>( +// Arrays.asList( +// new Translation2d(4.0, 196.125), +// new Translation2d(4.0, 20.5), +// new Translation2d(644.0, 196.125), +// new Translation2d(644.0, 20.5))); // same order as explained above +// +// if (Pivot.getAlliance() == 1) { // red +// if (Math.sqrt( +// Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) +// > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { +// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner +// } else { +// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer +// } +// } else if (Pivot.getAlliance() == 0) { // blue +// if (Math.sqrt( +// Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) +// > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) +// + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { +// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner +// } else { +// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer +// } +// } else { +// System.out.println("error in getClosestCosmicConverter() in Pivot"); +// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; +// } +// } public PivotCommand.Position getClosestCosmicConverterPivot() { Translation2d currentLocation = From ae91d90b16997e955b26a7998021f6a1b6e630f0 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 17:07:30 -0500 Subject: [PATCH 18/42] kjhb --- .../java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java index 1ff4986..80e2249 100644 --- a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java +++ b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java @@ -13,6 +13,5 @@ public CommandSwerveDrivetrainLogger() { @Override protected void update(EpilogueBackend backend, CommandSwerveDrivetrain drivetrain) { - backend.log(drivetrain.getState()); } } From 6362a8173961417275140b1bc8d249520ef2735b Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Wed, 3 Dec 2025 21:22:10 -0500 Subject: [PATCH 19/42] spotless --- src/main/java/frc/robot/RobotContainer.java | 22 +-- .../java/frc/robot/config/VisionConfig.java | 12 +- .../CommandSwerveDrivetrainLogger.java | 3 +- .../subsystems/CommandSwerveDrivetrain.java | 5 - src/main/java/frc/robot/subsystems/Pivot.java | 162 +++++++++--------- 5 files changed, 103 insertions(+), 101 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f36ee4c..2a1325a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -117,7 +117,7 @@ private void configureBindings() { .getDistance(pivot.getCosmicConverterTranslation(false))))))); // Pivot default command - controller.povUp().whileFalse(Commands.run(()-> pivot.setPivotAngleRot(0.0))); + controller.povUp().whileFalse(Commands.run(() -> pivot.setPivotAngleRot(0.0))); // Zero pivot controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); // Shoot @@ -127,11 +127,11 @@ private void configureBindings() { Commands.run(() -> shooter.shoot(1)) .alongWith(Commands.run(() -> intake.runKicker(-0.3)))); // Shooter default command - //shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); + // shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); // Intake controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); - //intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); + // intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); // Drivetrain default command drivetrain2.setDefaultCommand( @@ -139,10 +139,10 @@ private void configureBindings() { () -> drivetrain2.setControl( (new SwerveRequest.FieldCentric() - .withVelocityX(controller.getLeftY()*2.5) - .withVelocityY(controller.getLeftX()*2.5) - .withRotationalRate( - controller.getRightX()*2.5))), drivetrain2)); // Drive counterclockwise with negative X + .withVelocityX(controller.getLeftY() * 2.5) + .withVelocityY(controller.getLeftX() * 2.5) + .withRotationalRate(controller.getRightX() * 2.5))), + drivetrain2)); // Drive counterclockwise with negative X // auto align with inner cosmic converter controller .rightBumper() @@ -188,8 +188,8 @@ public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); } - public static void zeroPigeon() { - Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); - pigeon.reset(); - } + public static void zeroPigeon() { + Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); + pigeon.reset(); + } } diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index be2460f..cd6a7df 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -81,9 +81,17 @@ public class VisionConfig { public static final PhotonPoseEstimator.PoseStrategy STRATEGY = PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; public static final Transform3d LEFT_CAMERA_POSITION = - new Transform3d(Units.inchesToMeters(9.25), Units.inchesToMeters(10.5), Units.inchesToMeters(7.5), new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); + new Transform3d( + Units.inchesToMeters(9.25), + Units.inchesToMeters(10.5), + Units.inchesToMeters(7.5), + new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); public static final Transform3d RIGHT_CAMERA_POSITION = - new Transform3d(Units.inchesToMeters(9.25), Units.inchesToMeters(-10.5), Units.inchesToMeters(7.5), new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); + new Transform3d( + Units.inchesToMeters(9.25), + Units.inchesToMeters(-10.5), + Units.inchesToMeters(7.5), + new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); public static final Transform3d REAR_CAMERA_POSITION = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); Optional visionEst = Optional.empty(); diff --git a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java index 80e2249..37dbfc4 100644 --- a/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java +++ b/src/main/java/frc/robot/loggers/CommandSwerveDrivetrainLogger.java @@ -12,6 +12,5 @@ public CommandSwerveDrivetrainLogger() { } @Override - protected void update(EpilogueBackend backend, CommandSwerveDrivetrain drivetrain) { - } + protected void update(EpilogueBackend backend, CommandSwerveDrivetrain drivetrain) {} } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index b7e213f..cd1b65f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -7,7 +7,6 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; -import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -18,12 +17,9 @@ import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.config.TunerConstants.TunerSwerveDrivetrain; - -import java.util.function.BooleanSupplier; import java.util.function.Supplier; /** @@ -277,5 +273,4 @@ public void addVisionMeasurement( super.addVisionMeasurement( visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs); } - } diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 417fe79..daa32a5 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -203,87 +203,87 @@ public static Translation2d getLocation(int innerouter) { return new Translation2d(0.0, 0.0); } -// public DrivetrainCommand.Position getClosestCosmicConverterDrivetrain() { -// Translation2d currentLocation = -// new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); -// // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer -// // innerouter: 0 - outer, 1 - inner -// // getAlliance(): blue - 0, red - 1 -// -// List locations = -// new ArrayList<>( -// Arrays.asList( -// new Translation2d(4.0, 196.125), -// new Translation2d(4.0, 20.5), -// new Translation2d(644.0, 196.125), -// new Translation2d(644.0, 20.5))); // same order as explained above -// -// if (Pivot.getAlliance() == 1) { // red -// if (Math.sqrt( -// Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) -// > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { -// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer -// } else { -// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner -// } -// } else if (Pivot.getAlliance() == 0) { // blue -// if (Math.sqrt( -// Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) -// > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { -// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer -// } else { -// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner -// } -// } else { -// System.out.println("error in getClosestCosmicConverter() in Pivot"); -// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; - // } - // } - -// public DrivetrainCommand.Position getFarthestCosmicConverterDrivetrain() { -// Translation2d currentLocation = -// new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); -// // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer -// // innerouter: 0 - outer, 1 - inner -// // getAlliance(): blue - 0, red - 1 -// -// List locations = -// new ArrayList<>( -// Arrays.asList( -// new Translation2d(4.0, 196.125), -// new Translation2d(4.0, 20.5), -// new Translation2d(644.0, 196.125), -// new Translation2d(644.0, 20.5))); // same order as explained above -// -// if (Pivot.getAlliance() == 1) { // red -// if (Math.sqrt( -// Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) -// > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { -// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner -// } else { -// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer -// } -// } else if (Pivot.getAlliance() == 0) { // blue -// if (Math.sqrt( -// Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) -// > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) -// + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { -// return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner -// } else { -// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer -// } -// } else { -// System.out.println("error in getClosestCosmicConverter() in Pivot"); -// return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; -// } -// } + // public DrivetrainCommand.Position getClosestCosmicConverterDrivetrain() { + // Translation2d currentLocation = + // new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); + // // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer + // // innerouter: 0 - outer, 1 - inner + // // getAlliance(): blue - 0, red - 1 + // + // List locations = + // new ArrayList<>( + // Arrays.asList( + // new Translation2d(4.0, 196.125), + // new Translation2d(4.0, 20.5), + // new Translation2d(644.0, 196.125), + // new Translation2d(644.0, 20.5))); // same order as explained above + // + // if (Pivot.getAlliance() == 1) { // red + // if (Math.sqrt( + // Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) + // > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { + // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + // } else { + // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + // } + // } else if (Pivot.getAlliance() == 0) { // blue + // if (Math.sqrt( + // Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) + // > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { + // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + // } else { + // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + // } + // } else { + // System.out.println("error in getClosestCosmicConverter() in Pivot"); + // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; + // } + // } + + // public DrivetrainCommand.Position getFarthestCosmicConverterDrivetrain() { + // Translation2d currentLocation = + // new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); + // // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer + // // innerouter: 0 - outer, 1 - inner + // // getAlliance(): blue - 0, red - 1 + // + // List locations = + // new ArrayList<>( + // Arrays.asList( + // new Translation2d(4.0, 196.125), + // new Translation2d(4.0, 20.5), + // new Translation2d(644.0, 196.125), + // new Translation2d(644.0, 20.5))); // same order as explained above + // + // if (Pivot.getAlliance() == 1) { // red + // if (Math.sqrt( + // Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) + // > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { + // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + // } else { + // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + // } + // } else if (Pivot.getAlliance() == 0) { // blue + // if (Math.sqrt( + // Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) + // > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) + // + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { + // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner + // } else { + // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer + // } + // } else { + // System.out.println("error in getClosestCosmicConverter() in Pivot"); + // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; + // } + // } public PivotCommand.Position getClosestCosmicConverterPivot() { Translation2d currentLocation = From b4fe62d06b2a83c80aaf68530466b0f826bce946 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 4 Dec 2025 11:31:43 -0500 Subject: [PATCH 20/42] cleaned out commands and robotcontainer --- src/main/java/frc/robot/RobotContainer.java | 148 ++++----- .../frc/robot/commands/IntakeCommand.java | 58 ---- .../java/frc/robot/commands/PivotCommand.java | 78 ----- .../frc/robot/commands/ShooterCommand.java | 45 --- .../java/frc/robot/config/PivotConfig.java | 2 +- .../java/frc/robot/subsystems/Intake.java | 14 +- src/main/java/frc/robot/subsystems/Pivot.java | 286 +++--------------- .../java/frc/robot/subsystems/Shooter.java | 5 + 8 files changed, 122 insertions(+), 514 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/IntakeCommand.java delete mode 100644 src/main/java/frc/robot/commands/PivotCommand.java delete mode 100644 src/main/java/frc/robot/commands/ShooterCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2a1325a..82abde2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -37,25 +37,24 @@ public class RobotContainer { private double MaxAngularRate = RotationsPerSecond.of(0.75) .in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity - /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.50) + .withDeadband(MaxSpeed * 0.1) .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType( SwerveModule.DriveRequestType .OpenLoopVoltage); // Use open-loop control for drive motors private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); - private final Telemetry logger = new Telemetry(MaxSpeed); private CommandXboxController controller = new CommandXboxController(0); - private CommandSwerveDrivetrain drivetrain2 = TunerConstants.createDrivetrain(); + private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Intake intake = new Intake(); private Pivot pivot = new Pivot(); private Shooter shooter = new Shooter(); + public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(10.875)); @@ -71,10 +70,10 @@ public class RobotContainer { m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation), pigeon2.getRotation2d(), new SwerveModulePosition[] { - drivetrain2.getModule(0).getPosition(true), - drivetrain2.getModule(1).getPosition(true), - drivetrain2.getModule(2).getPosition(true), - drivetrain2.getModule(3).getPosition(true) + drivetrain.getModule(0).getPosition(true), + drivetrain.getModule(1).getPosition(true), + drivetrain.getModule(2).getPosition(true), + drivetrain.getModule(3).getPosition(true) }, new Pose2d(0.0, 0.0, new Rotation2d())); static Field2d m_field = new Field2d(); @@ -87,12 +86,8 @@ public RobotContainer() { private void configureBindings() { final SwerveRequest.Idle idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled() - .whileTrue(drivetrain2.applyRequest(() -> idle).ignoringDisable(true)); - - // controller.rightTrigger().onTrue(superstructure.action()); - // controller.leftBumper().onTrue(superstructure.toggleFarHigh()); - // controller.rightBumper().onTrue(superstructure.toggleLowScore()); - // controller.rightStick().onTrue(superstructure.toggleIntake()); + .whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true)); + drivetrain.registerTelemetry(logger::telemeterize); InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); map.put(Units.inchesToMeters(59.0), 0.18); @@ -102,86 +97,61 @@ private void configureBindings() { map.put(Units.inchesToMeters(169.5), 0.12); map.put(Units.inchesToMeters(210.5), 0.118); - // Pivot to angle based off distance - controller - .povUp() - .whileTrue( - (Commands.run( - () -> - pivot.setPivotAngleRot( - map.get( - drivetrain2 - .getState() - .Pose - .getTranslation() - .getDistance(pivot.getCosmicConverterTranslation(false))))))); - - // Pivot default command - controller.povUp().whileFalse(Commands.run(() -> pivot.setPivotAngleRot(0.0))); - // Zero pivot - controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); - // Shoot - controller - .rightTrigger() - .whileTrue( - Commands.run(() -> shooter.shoot(1)) - .alongWith(Commands.run(() -> intake.runKicker(-0.3)))); - // Shooter default command - // shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter())); - - // Intake - controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); - // intake.setDefaultCommand(Commands.run(() -> intake.stopIntake())); - - // Drivetrain default command - drivetrain2.setDefaultCommand( - Commands.run( + // Default commands + pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); + shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter(), shooter)); + intake.setDefaultCommand(Commands.run(() -> intake.stopIntake(), intake)); + drivetrain.setDefaultCommand( + // Drivetrain will execute this command periodically + drivetrain.applyRequest( () -> - drivetrain2.setControl( - (new SwerveRequest.FieldCentric() - .withVelocityX(controller.getLeftY() * 2.5) - .withVelocityY(controller.getLeftX() * 2.5) - .withRotationalRate(controller.getRightX() * 2.5))), - drivetrain2)); // Drive counterclockwise with negative X - // auto align with inner cosmic converter - controller - .rightBumper() - .toggleOnTrue( - pivot - .getCosmicConverter(true) - .alongWith( - Commands.run( - (() -> - pivot.setPivotAngleRot( - map.get( - drivetrain2 - .getState() - .Pose - .getTranslation() - .getDistance( - pivot.getCosmicConverterTranslation(true)))))))); + drive + .withVelocityX( + -controller.getLeftY() + * MaxSpeed) // Drive forward with negative Y (forward) + .withVelocityY( + -controller.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withRotationalRate( + -controller.getRightX() + * MaxAngularRate))); // Drive counterclockwise with negative X (left) + // Testing - Commented out until need to be used + // Pivot to angle based off distance +// controller +// .povUp() +// .whileTrue( +// (Commands.run( +// () -> +// pivot.setPivotAngleRot( +// map.get( +// drivetrain +// .getState() +// .Pose +// .getTranslation() +// .getDistance(pivot.getCosmicConverterTranslation(false))))))); +// +// // Zero pivot +// controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); +// // Shoot +// controller +// .rightTrigger() +// .whileTrue( +// Commands.run(() -> shooter.shoot()).alongWith(Commands.run(() -> intake.runKicker()))); +// // Intake +// controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + + // Specialized commands + // auto align with inner cosmic converter and raise pivot + controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); // auto align with outer cosmic converter controller - .rightBumper() - .toggleOnTrue( - pivot - .getCosmicConverter(false) - .alongWith( - Commands.run( - (() -> - pivot.setPivotAngleRot( - map.get( - drivetrain2 - .getState() - .Pose - .getTranslation() - .getDistance( - pivot.getCosmicConverterTranslation(false)))))))); + .leftTrigger() + .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); + // Intake + controller.leftStick().toggleOnTrue(Commands.run(() -> intake.intake())); // Outtake - controller.povDown().toggleOnTrue(Commands.run(() -> intake.outtake())); + controller.rightStick().toggleOnTrue(Commands.run(() -> intake.outtake())); // Low score - controller.povRight().toggleOnTrue(Commands.run(() -> pivot.setPivotAngleRot(0.0))); - drivetrain2.registerTelemetry(logger::telemeterize); + controller.rightBumper().toggleOnTrue(pivot.lowScore(0.0)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java deleted file mode 100644 index fe787f5..0000000 --- a/src/main/java/frc/robot/commands/IntakeCommand.java +++ /dev/null @@ -1,58 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.IntakeConfig; -import frc.robot.subsystems.Intake; - -@Logged -public class IntakeCommand extends Command { - public static enum Speeds { - INTAKE, - OUTTAKE_SCORE, - SHOOT, - IDLE - } - - private Speeds speed; - Intake intake; - - public IntakeCommand(Intake intake, Speeds speed) { - this.intake = intake; - this.speed = speed; - addRequirements(intake); - } - - @Override - public void initialize() { - switch (speed) { - case INTAKE: - intake.runKicker(IntakeConfig.K_KICKER_INTAKE_VELOCITY); - intake.runInitial(IntakeConfig.K_INITIAL_INTAKE_VELOCITY); - break; - - case OUTTAKE_SCORE: - intake.runKicker(IntakeConfig.K_KICKER_OUTTAKE_VELOCITY); - intake.runInitial(IntakeConfig.K_INITIAL_OUTTAKE_VELOCITY); - break; - - case SHOOT: - intake.runKicker(IntakeConfig.K_KICKER_INTAKE_VELOCITY); - intake.runInitial(IntakeConfig.K_KICKER_INTAKE_VELOCITY); - break; - - case IDLE: - intake.stopIntake(); - break; - - default: - intake.stopIntake(); - break; - } - } - - @Override - public void end(boolean interrupted) { - intake.stopIntake(); - } -} diff --git a/src/main/java/frc/robot/commands/PivotCommand.java b/src/main/java/frc/robot/commands/PivotCommand.java deleted file mode 100644 index 23f6d5a..0000000 --- a/src/main/java/frc/robot/commands/PivotCommand.java +++ /dev/null @@ -1,78 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.PivotConfig; -import frc.robot.subsystems.Pivot; - -@Logged -public class PivotCommand extends Command { - public static enum Position { - INTAKE_GROUND, - INTAKE_STAR_SPIRE, - OUTTAKE_SCORE, - INNER_HIGH_SHOOT, - OUTER_HIGH_SHOOT, - IDLE - } - - private Position pose; - Pivot pivot; - - public PivotCommand(Pivot pivot, Position pose) { - this.pivot = pivot; - this.pose = pose; - addRequirements(pivot); - } - - @Override - public void initialize() { - switch (pose) { - case INTAKE_GROUND: - pivot.setPivotAngle( - new Rotation2d(Units.degreesToRadians(PivotConfig.PIVOT_GROUND_INTAKE_ANGLE))); - break; - - case INTAKE_STAR_SPIRE: - pivot.setPivotAngle( - new Rotation2d(Units.degreesToRadians(PivotConfig.PIVOT_STAR_SPIRE_INTAKE_ANGLE))); - break; - - case OUTTAKE_SCORE: - pivot.setPivotAngle( - new Rotation2d(Units.degreesToRadians(PivotConfig.PIVOT_OUTTAKE_ANGLE))); - break; - - case IDLE: - pivot.setPivotAngle(new Rotation2d((Units.degreesToRadians(PivotConfig.PIVOT_IDLE_ANGLE)))); - break; - - case INNER_HIGH_SHOOT: - // pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(1))); - pivot.setPivotAngle(new Rotation2d(PivotConfig.ANGLE_ADD)); - break; - - case OUTER_HIGH_SHOOT: - // pivot.setPivotAngle(pivot.getHighAngle(Pivot.getLocation(0))); - pivot.setPivotAngle(new Rotation2d(PivotConfig.ANGLE_ADD)); - break; - - default: - pivot.stopPivot(); - break; - } - } - - @Override - public boolean isFinished() { - // The command is finished when the pivot reaches its target angle within tolerance. - return pivot.pivotAtSetpoint(); - } - - @Override - public void end(boolean interrupted) { - pivot.stopPivot(); - } -} diff --git a/src/main/java/frc/robot/commands/ShooterCommand.java b/src/main/java/frc/robot/commands/ShooterCommand.java deleted file mode 100644 index 41fd071..0000000 --- a/src/main/java/frc/robot/commands/ShooterCommand.java +++ /dev/null @@ -1,45 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.config.ShooterConfig; -import frc.robot.subsystems.Shooter; - -@Logged -public class ShooterCommand extends Command { - public static enum Positions { - SHOOT, - IDLE - } - - private Positions pose; - Shooter shooter; - - public ShooterCommand(Shooter shooter, Positions pose) { - this.shooter = shooter; - this.pose = pose; - addRequirements(shooter); - } - - @Override - public void initialize() { - switch (pose) { - case SHOOT: - shooter.shoot(ShooterConfig.K_TOP_AND_BOTTOM_SHOOTER_VELOCITY); - break; - - case IDLE: - shooter.stopShooter(); - break; - - default: - shooter.stopShooter(); - break; - } - } - - @Override - public void end(boolean interrupted) { - shooter.stopShooter(); - } -} diff --git a/src/main/java/frc/robot/config/PivotConfig.java b/src/main/java/frc/robot/config/PivotConfig.java index 8123040..d1d749f 100644 --- a/src/main/java/frc/robot/config/PivotConfig.java +++ b/src/main/java/frc/robot/config/PivotConfig.java @@ -1,7 +1,7 @@ package frc.robot.config; public class PivotConfig { - public static final double K_PIVOT_ANGLE_TOLERANCE = 0.0006; // in rotations + public static final double K_PIVOT_ANGLE_TOLERANCE = 0.005; // in rotations public static final double K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT = 80.0; public static final double K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT = 70.0; diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 298fd86..6d0354a 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -70,9 +70,9 @@ public Intake() { } // Velocity is rotations per second of motor accounting for SensorToMechanismRatio - public void intake(double velocity) { - mInitialIntake.setControl(new DutyCycleOut(velocity)); - mKickerIntake.setControl(new DutyCycleOut(velocity)); + public void intake(double initialVelocity, double kickerVelocity) { + mInitialIntake.setControl(new DutyCycleOut(initialVelocity)); + mKickerIntake.setControl(new DutyCycleOut(kickerVelocity)); } public void intake() { @@ -89,10 +89,18 @@ public void runKicker(double velocity) { mKickerIntake.setControl(new DutyCycleOut(velocity)); } + public void runKicker() { + mKickerIntake.setControl(new DutyCycleOut(-0.3)); + } + public void runInitial(double velocity) { mInitialIntake.setControl(new DutyCycleOut(velocity)); } + public void runInitial() { + mInitialIntake.setControl(new DutyCycleOut(-0.5)); + } + public void stopIntake() { mInitialIntake.stopMotor(); mKickerIntake.stopMotor(); diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index daa32a5..7409872 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -16,14 +16,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.commands.PivotCommand; import frc.robot.config.CANMappings; import frc.robot.config.PivotConfig; import frc.robot.config.TunerConstants; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; import java.util.Optional; +import java.util.function.BooleanSupplier; @Logged public class Pivot extends SubsystemBase { @@ -31,7 +28,9 @@ public class Pivot extends SubsystemBase { protected TalonFX mPivotRight; protected Follower follower; protected CommandSwerveDrivetrain drivetrain; - InterpolatingDoubleTreeMap map1 = new InterpolatingDoubleTreeMap(); + protected Shooter shooter; + protected Intake intake; + private final InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); private double currentAngle; @@ -40,7 +39,8 @@ public Pivot() { mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); drivetrain = TunerConstants.createDrivetrain(); - + shooter = new Shooter(); + intake = new Intake(); TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); @@ -109,7 +109,11 @@ public void setPivotAngle(Rotation2d angleSetpoint) { public void setPivotAngleRot(double rotation) { mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); mPivotRight.setControl(new MotionMagicVoltage(rotation)); - // mPivotRight.setControl(follower); + } + + public void pivotDefault() { + mPivotLeft.setControl(new MotionMagicVoltage(0.0)); + mPivotRight.setControl(new MotionMagicVoltage(0.0)); } public void zeroPivot() { @@ -127,26 +131,6 @@ public boolean pivotAtSetpoint() { <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; } - public Rotation2d getHighAngle(Translation2d location) { - // location: the cosmic converter we're shooting on - 1 is blue inner, 2 is blue outer, 3 is red - // inner, 4 is red outer - // want 5-8 calibrations (distance, angle) - // in, - InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - map.put(59.0, 0.18); - map.put(76.5, 0.155); - map.put(96.5, 0.142); - map.put(125.5, 0.13); - map.put(169.5, 0.12); - map.put(210.5, 0.118); - - double distance = - Math.sqrt( - Math.pow(location.getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(location.getY() - drivetrain.getState().Pose.getY(), 2)); - return Rotation2d.fromDegrees(map.get(distance)); - } - public double getPivotAngleDegrees() { currentAngle = mPivotLeft.getPosition().getValueAsDouble(); currentAngle = currentAngle * 360; @@ -159,217 +143,15 @@ public double getPivotAngleDegrees() { .withDriveRequestType( SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - public static int getAlliance() { - Optional alliance = DriverStation.getAlliance(); - - if (alliance.isPresent()) { - if (alliance.get() == DriverStation.Alliance.Blue) { - return 0; - } - if (alliance.get() == DriverStation.Alliance.Red) { - return 1; - } - } - System.out.println("no alliance detected: likely causing many errors"); - return -1; - } - - public static Translation2d getLocation(int innerouter) { - // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // innerouter: 0 - outer, 1 - inner - // getAlliance(): blue - 0, red - 1 - - List locations = - new ArrayList<>( - Arrays.asList( - new Translation2d(4.0, 196.125), - new Translation2d(4.0, 20.5), - new Translation2d(644.0, 196.125), - new Translation2d(644.0, 20.5))); // same order as explained above - - if (innerouter == 1 & Pivot.getAlliance() == 0) { // blue inner - return locations.get(0); - } - if (innerouter == 0 & Pivot.getAlliance() == 0) { // blue outer - return locations.get(1); - } - if (innerouter == 1 & Pivot.getAlliance() == 1) { // red inner - return locations.get(2); - } - if (innerouter == 0 & Pivot.getAlliance() == 1) { // red outer - return locations.get(3); - } - System.out.println("error in getLocation in pivot subsystem"); - return new Translation2d(0.0, 0.0); - } - - // public DrivetrainCommand.Position getClosestCosmicConverterDrivetrain() { - // Translation2d currentLocation = - // new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); - // // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // // innerouter: 0 - outer, 1 - inner - // // getAlliance(): blue - 0, red - 1 - // - // List locations = - // new ArrayList<>( - // Arrays.asList( - // new Translation2d(4.0, 196.125), - // new Translation2d(4.0, 20.5), - // new Translation2d(644.0, 196.125), - // new Translation2d(644.0, 20.5))); // same order as explained above - // - // if (Pivot.getAlliance() == 1) { // red - // if (Math.sqrt( - // Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) - // > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { - // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - // } else { - // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - // } - // } else if (Pivot.getAlliance() == 0) { // blue - // if (Math.sqrt( - // Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) - // > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { - // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - // } else { - // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - // } - // } else { - // System.out.println("error in getClosestCosmicConverter() in Pivot"); - // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; - // } - // } - - // public DrivetrainCommand.Position getFarthestCosmicConverterDrivetrain() { - // Translation2d currentLocation = - // new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); - // // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // // innerouter: 0 - outer, 1 - inner - // // getAlliance(): blue - 0, red - 1 - // - // List locations = - // new ArrayList<>( - // Arrays.asList( - // new Translation2d(4.0, 196.125), - // new Translation2d(4.0, 20.5), - // new Translation2d(644.0, 196.125), - // new Translation2d(644.0, 20.5))); // same order as explained above - // - // if (Pivot.getAlliance() == 1) { // red - // if (Math.sqrt( - // Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) - // > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { - // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - // } else { - // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - // } - // } else if (Pivot.getAlliance() == 0) { // blue - // if (Math.sqrt( - // Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) - // > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) - // + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { - // return DrivetrainCommand.Position.INNER_COSMIC_CONVERTER; // inner - // } else { - // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; // outer - // } - // } else { - // System.out.println("error in getClosestCosmicConverter() in Pivot"); - // return DrivetrainCommand.Position.OUTER_COSMIC_CONVERTER; - // } - // } - - public PivotCommand.Position getClosestCosmicConverterPivot() { - Translation2d currentLocation = - new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); - // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // innerouter: 0 - outer, 1 - inner - // getAlliance(): blue - 0, red - 1 - - List locations = - new ArrayList<>( - Arrays.asList( - new Translation2d(4.0, 196.125), - new Translation2d(4.0, 20.5), - new Translation2d(644.0, 196.125), - new Translation2d(644.0, 20.5))); // same order as explained above - - if (Pivot.getAlliance() == 1) { // red - if (Math.sqrt( - Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { - return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer - } else { - return PivotCommand.Position.INNER_HIGH_SHOOT; // inner - } - } else if (Pivot.getAlliance() == 0) { // blue - if (Math.sqrt( - Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { - return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer - } else { - return PivotCommand.Position.INNER_HIGH_SHOOT; // inner - } - } else { - System.out.println("error in getClosestCosmicConverter() in Pivot"); - return PivotCommand.Position.OUTER_HIGH_SHOOT; - } - } - - public PivotCommand.Position getFarthestCosmicConverterPivot() { - Translation2d currentLocation = - new Translation2d(drivetrain.getState().Pose.getX(), drivetrain.getState().Pose.getY()); - // ArrayList values: 0 - blue inner, 1 - blue outer, 2 - red inner, 3 - red outer - // innerouter: 0 - outer, 1 - inner - // getAlliance(): blue - 0, red - 1 - - List locations = - new ArrayList<>( - Arrays.asList( - new Translation2d(4.0, 196.125), - new Translation2d(4.0, 20.5), - new Translation2d(644.0, 196.125), - new Translation2d(644.0, 20.5))); // same order as explained above - - if (Pivot.getAlliance() == 1) { // red - if (Math.sqrt( - Math.pow(locations.get(2).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(2).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(3).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(3).getY() - drivetrain.getState().Pose.getY(), 2)) { - return PivotCommand.Position.INNER_HIGH_SHOOT; // inner - } else { - return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer - } - } else if (Pivot.getAlliance() == 0) { // blue - if (Math.sqrt( - Math.pow(locations.get(0).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(0).getY() - drivetrain.getState().Pose.getY(), 2)) - > Math.pow(locations.get(1).getX() - drivetrain.getState().Pose.getX(), 2) - + Math.pow(locations.get(1).getY() - drivetrain.getState().Pose.getY(), 2)) { - return PivotCommand.Position.INNER_HIGH_SHOOT; // inner - } else { - return PivotCommand.Position.OUTER_HIGH_SHOOT; // outer - } - } else { - System.out.println("error in getClosestCosmicConverter() in Pivot"); - return PivotCommand.Position.OUTER_HIGH_SHOOT; - } - } - - public Command getCosmicConverter(boolean isInner) { + public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { Optional alliance1 = DriverStation.getAlliance(); Translation2d cosmicConverter = new Translation2d(); + map.put(Units.inchesToMeters(59.0), 0.18); + map.put(Units.inchesToMeters(76.5), 0.155); + map.put(Units.inchesToMeters(96.5), 0.142); + map.put(Units.inchesToMeters(125.5), 0.13); + map.put(Units.inchesToMeters(169.5), 0.12); + map.put(Units.inchesToMeters(210.5), 0.118); if (alliance1.isPresent()) { if (alliance1.get() == DriverStation.Alliance.Blue) { if (isInner) { @@ -393,7 +175,7 @@ public Command getCosmicConverter(boolean isInner) { Rotation2d heading = drivetrain.getState().Pose.getRotation(); // shooter offset in robot frame (meters) - double shooterOffsetX = 0.20; // forward + double shooterOffsetX = 0.0; // forward double shooterOffsetY = -0.10; // right // convert to field frame @@ -413,9 +195,29 @@ public Command getCosmicConverter(boolean isInner) { Math.atan2(cosmicConverter.getY() - shooterY, cosmicConverter.getX() - shooterX)); return Commands.runOnce( - () -> - drivetrain.setControl( - m_faceAngle.withVelocityX(0.1).withVelocityY(0.1).withTargetDirection(aimAngle))); + () -> + drivetrain.setControl( + m_faceAngle + .withVelocityX(0.1) + .withVelocityY(0.1) + .withTargetDirection(aimAngle))) + .alongWith( + Commands.runOnce( + () -> + setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(getCosmicConverterTranslation(false)))))) + .andThen( + Commands.waitUntil(complete) + .andThen( + Commands.parallel( + Commands.run(() -> shooter.shoot()), + Commands.run(() -> intake.runKicker())) + .until(() -> !complete.getAsBoolean()))); } else { cosmicConverter = null; System.out.println("no alliance detected: likely causing many errors"); @@ -448,4 +250,8 @@ public Translation2d getCosmicConverterTranslation(boolean isInner) { } return cosmicConverter; } + + public Command lowScore(double angle) { + return Commands.run(() -> setPivotAngleRot(angle)); + } } diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java index 5a64a31..2c850fc 100644 --- a/src/main/java/frc/robot/subsystems/Shooter.java +++ b/src/main/java/frc/robot/subsystems/Shooter.java @@ -75,6 +75,11 @@ public void shoot(double velocity) { mBottomShooter.setControl(new DutyCycleOut(velocity)); } + public void shoot() { + mTopShooter.setControl(new DutyCycleOut(-1)); + mBottomShooter.setControl(new DutyCycleOut(1)); + } + public void stopShooter() { mTopShooter.stopMotor(); mBottomShooter.stopMotor(); From 825fabcf261a615a43db08fc355b3f303c82420e Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Thu, 4 Dec 2025 18:22:40 -0500 Subject: [PATCH 21/42] pivot tuning, drivetrain regenerated --- src/main/java/frc/robot/RobotContainer.java | 72 +++++++++++-------- .../java/frc/robot/config/PivotConfig.java | 2 +- .../java/frc/robot/config/TunerConstants.java | 12 ++-- .../subsystems/CommandSwerveDrivetrain.java | 3 +- src/main/java/frc/robot/subsystems/Pivot.java | 7 +- 5 files changed, 52 insertions(+), 44 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 82abde2..1cb63d8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -83,6 +83,8 @@ public RobotContainer() { configureBindings(); } + @Logged Pose2d estimatedPosition = m_odometry.getEstimatedPosition(); + private void configureBindings() { final SwerveRequest.Idle idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled() @@ -109,43 +111,53 @@ private void configureBindings() { .withVelocityX( -controller.getLeftY() * MaxSpeed) // Drive forward with negative Y (forward) - .withVelocityY( - -controller.getLeftX() * MaxSpeed) // Drive left with negative X (left) + .withVelocityY(-controller.getLeftX() * MaxSpeed) // Drive left with negative X .withRotationalRate( -controller.getRightX() - * MaxAngularRate))); // Drive counterclockwise with negative X (left) + * MaxAngularRate))); // Drive counterclockwise with negative X + // Testing - Commented out until need to be used // Pivot to angle based off distance -// controller -// .povUp() -// .whileTrue( -// (Commands.run( -// () -> -// pivot.setPivotAngleRot( -// map.get( -// drivetrain -// .getState() -// .Pose -// .getTranslation() -// .getDistance(pivot.getCosmicConverterTranslation(false))))))); -// -// // Zero pivot -// controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); -// // Shoot -// controller -// .rightTrigger() -// .whileTrue( -// Commands.run(() -> shooter.shoot()).alongWith(Commands.run(() -> intake.runKicker()))); -// // Intake -// controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); + controller + .povUp() + .whileTrue( + (Commands.run( + () -> + pivot.setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(pivot.getCosmicConverterTranslation(false))))))); + // controller + // .povUp() + // .whileTrue( + // (Commands.run( + // () -> + // pivot.setPivotAngleRot( + // map.get(Units.inchesToMeters(132)))))); + + // Zero pivot + controller.povLeft().onTrue(Commands.runOnce(() -> pivot.zeroPivot())); + // + // + // Shoot + controller + .rightTrigger() + .whileTrue( + Commands.run(() -> shooter.shoot()).alongWith(Commands.run(() -> intake.intake()))); + // Intake + // controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); // Specialized commands // auto align with inner cosmic converter and raise pivot - controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); - // auto align with outer cosmic converter - controller - .leftTrigger() - .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); + // controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), + // true)); + // // auto align with outer cosmic converter + // controller + // .leftTrigger() + // .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); // Intake controller.leftStick().toggleOnTrue(Commands.run(() -> intake.intake())); // Outtake diff --git a/src/main/java/frc/robot/config/PivotConfig.java b/src/main/java/frc/robot/config/PivotConfig.java index d1d749f..62d50d0 100644 --- a/src/main/java/frc/robot/config/PivotConfig.java +++ b/src/main/java/frc/robot/config/PivotConfig.java @@ -10,7 +10,7 @@ public class PivotConfig { public static final double K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION = 500.0; public static final double K_LEFT_AND_RIGHT_PIVOT_JERK = 0.0; - public static final double K_LEFT_AND_RIGHT_PIVOT_P = 10; + public static final double K_LEFT_AND_RIGHT_PIVOT_P = 14; public static final double K_LEFT_AND_RIGHT_PIVOT_I = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_D = 0.0; public static final double K_LEFT_AND_RIGHT_PIVOT_S = 0.0; diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 6dc4890..53139ff 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -89,8 +89,8 @@ public class TunerConstants { private static final double kSteerGearRatio = 26.09; private static final Distance kWheelRadius = Inches.of(2); - private static final boolean kInvertLeftSide = false; - private static final boolean kInvertRightSide = true; + private static final boolean kInvertLeftSide = true; + private static final boolean kInvertRightSide = false; private static final int kPigeonId = 0; @@ -137,7 +137,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 51; private static final int kFrontLeftSteerMotorId = 50; private static final int kFrontLeftEncoderId = 52; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.0185546875); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.484619140625); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; @@ -148,7 +148,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 21; private static final int kFrontRightSteerMotorId = 20; private static final int kFrontRightEncoderId = 22; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.43505859375); + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.072509765625); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; @@ -159,7 +159,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 41; private static final int kBackLeftSteerMotorId = 40; private static final int kBackLeftEncoderId = 42; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.31787109375); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.184326171875); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; @@ -170,7 +170,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 31; private static final int kBackRightSteerMotorId = 30; private static final int kBackRightEncoderId = 32; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.2109375); + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.284423828125); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index cd1b65f..decf1e0 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -171,8 +171,7 @@ public CommandSwerveDrivetrain( /** * Returns a command that applies the specified control request to this swerve drivetrain. * - *

//* @param request Function returning the request to apply - * + * @param request Function returning the request to apply * @return Command to run */ public Command applyRequest(Supplier requestSupplier) { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 7409872..0cd31c2 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -176,7 +176,7 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { // shooter offset in robot frame (meters) double shooterOffsetX = 0.0; // forward - double shooterOffsetY = -0.10; // right + double shooterOffsetY = -1; // right // convert to field frame double shooterX = @@ -197,10 +197,7 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { return Commands.runOnce( () -> drivetrain.setControl( - m_faceAngle - .withVelocityX(0.1) - .withVelocityY(0.1) - .withTargetDirection(aimAngle))) + m_faceAngle.withVelocityX(2).withVelocityY(2).withTargetDirection(aimAngle))) .alongWith( Commands.runOnce( () -> From 4927027a9a7867c1c4e731f89d3b5d03e6f5ef5d Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Fri, 5 Dec 2025 15:19:53 -0500 Subject: [PATCH 22/42] auto code --- .../autos/High shoot inner pick 3.auto | 25 ++++++ .../pathplanner/autos/Longhot pick 3.auto | 55 ++++++++++++ .../pathplanner/autos/high low take 3.auto | 31 +++++++ .../autos/take 3 high shoot then there.auto | 73 ++++++++++++++++ .../there and back again high shoot.auto | 31 +++++++ src/main/deploy/pathplanner/navgrid.json | 1 + .../deploy/pathplanner/paths/Longshot 2.path | 70 +++++++++++++++ .../pathplanner/paths/Low goal setup 2.path | 54 ++++++++++++ .../pathplanner/paths/all the way out 1.path | 86 +++++++++++++++++++ .../pathplanner/paths/back again 3.path | 86 +++++++++++++++++++ .../paths/football otherside intake 2.path | 54 ++++++++++++ .../pathplanner/paths/high shoot flat 2.path | 54 ++++++++++++ .../pathplanner/paths/low goal finish.path | 54 ++++++++++++ .../pathplanner/paths/score high 2.path | 54 ++++++++++++ .../deploy/pathplanner/paths/take 3 flat.path | 75 ++++++++++++++++ src/main/deploy/pathplanner/paths/take 3.path | 75 ++++++++++++++++ src/main/deploy/pathplanner/settings.json | 32 +++++++ src/main/java/frc/robot/Robot.java | 11 +++ src/main/java/frc/robot/RobotContainer.java | 57 +++++++++++- .../subsystems/CommandSwerveDrivetrain.java | 38 ++++++++ 20 files changed, 1014 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto create mode 100644 src/main/deploy/pathplanner/autos/Longhot pick 3.auto create mode 100644 src/main/deploy/pathplanner/autos/high low take 3.auto create mode 100644 src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto create mode 100644 src/main/deploy/pathplanner/autos/there and back again high shoot.auto create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/Longshot 2.path create mode 100644 src/main/deploy/pathplanner/paths/Low goal setup 2.path create mode 100644 src/main/deploy/pathplanner/paths/all the way out 1.path create mode 100644 src/main/deploy/pathplanner/paths/back again 3.path create mode 100644 src/main/deploy/pathplanner/paths/football otherside intake 2.path create mode 100644 src/main/deploy/pathplanner/paths/high shoot flat 2.path create mode 100644 src/main/deploy/pathplanner/paths/low goal finish.path create mode 100644 src/main/deploy/pathplanner/paths/score high 2.path create mode 100644 src/main/deploy/pathplanner/paths/take 3 flat.path create mode 100644 src/main/deploy/pathplanner/paths/take 3.path create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto b/src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto new file mode 100644 index 0000000..a2d7242 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/High shoot inner pick 3.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "take 3" + } + }, + { + "type": "path", + "data": { + "pathName": "score high 2" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Longhot pick 3.auto b/src/main/deploy/pathplanner/autos/Longhot pick 3.auto new file mode 100644 index 0000000..b9a5339 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Longhot pick 3.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "high goal shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "take 3" + } + }, + { + "type": "named", + "data": { + "name": "stop intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Longshot 2" + } + }, + { + "type": "named", + "data": { + "name": "longshot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/high low take 3.auto b/src/main/deploy/pathplanner/autos/high low take 3.auto new file mode 100644 index 0000000..0f4deaa --- /dev/null +++ b/src/main/deploy/pathplanner/autos/high low take 3.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "take 3" + } + }, + { + "type": "path", + "data": { + "pathName": "Low goal setup 2" + } + }, + { + "type": "path", + "data": { + "pathName": "low goal finish" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto new file mode 100644 index 0000000..0b963c7 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto @@ -0,0 +1,73 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "high goal shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "take 3 flat" + } + }, + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "path", + "data": { + "pathName": "high shoot flat 2" + } + }, + { + "type": "named", + "data": { + "name": "high goal shoot" + } + }, + { + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "path", + "data": { + "pathName": "all the way out 1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/there and back again high shoot.auto b/src/main/deploy/pathplanner/autos/there and back again high shoot.auto new file mode 100644 index 0000000..c67d19e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/there and back again high shoot.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "all the way out 1" + } + }, + { + "type": "path", + "data": { + "pathName": "football otherside intake 2" + } + }, + { + "type": "path", + "data": { + "pathName": "back again 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Longshot 2.path b/src/main/deploy/pathplanner/paths/Longshot 2.path new file mode 100644 index 0000000..665c0a1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Longshot 2.path @@ -0,0 +1,70 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.029548910017559, + "y": 2.6727494447605586 + }, + "prevControl": null, + "nextControl": { + "x": 5.029548910017559, + "y": 2.6727494447605586 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.053577929308689, + "y": 1.7699595577663172 + }, + "prevControl": { + "x": 3.8630799422428623, + "y": 1.8427224055175209 + }, + "nextControl": { + "x": 2.2440759163745154, + "y": 1.6971967100151135 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.2932306675721814, + "y": 2.1005889667957645 + }, + "prevControl": { + "x": 2.4605355879869943, + "y": 1.9148228137834542 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 133.5222600525553 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.9317723414291 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Low goal setup 2.path b/src/main/deploy/pathplanner/paths/Low goal setup 2.path new file mode 100644 index 0000000..8a00061 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Low goal setup 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.9788515852769675, + "y": 2.6782297740524776 + }, + "prevControl": null, + "nextControl": { + "x": 3.9513717109767534, + "y": 2.9267148973666197 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.893244351311953, + "y": 0.5683707634839654 + }, + "prevControl": { + "x": 3.142398730089056, + "y": 0.5478257997167371 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -0.47602754348356574 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -154.99751127064283 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/all the way out 1.path b/src/main/deploy/pathplanner/paths/all the way out 1.path new file mode 100644 index 0000000..ad0ce8d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/all the way out 1.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6767644557823125, + "y": 0.6788865099611276 + }, + "prevControl": null, + "nextControl": { + "x": 2.5585178267677686, + "y": 0.8830250850321845 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.929469220724004, + "y": 1.5431300868561715 + }, + "prevControl": { + "x": 6.6013347303206995, + "y": 0.6222762542517009 + }, + "nextControl": { + "x": 8.485788056470975, + "y": 1.9288503100333383 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 9.85379084669582, + "y": 3.849962418002915 + }, + "prevControl": { + "x": 9.641671316964286, + "y": 3.448848298038523 + }, + "nextControl": { + "x": 10.065910376427352, + "y": 4.251076537967306 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 13.4638453595724, + "y": 6.89932390366861 + }, + "prevControl": { + "x": 12.4638453595724, + "y": 6.89932390366861 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.38320323473243 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.6352770022539 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/back again 3.path b/src/main/deploy/pathplanner/paths/back again 3.path new file mode 100644 index 0000000..3c07019 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/back again 3.path @@ -0,0 +1,86 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 15.35347956147128, + "y": 6.794882015303399 + }, + "prevControl": null, + "nextControl": { + "x": 13.291499635568512, + "y": 7.536082513362487 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 11.053852344509231, + "y": 4.823725249028183 + }, + "prevControl": { + "x": 12.688312333998129, + "y": 6.57735371673813 + }, + "nextControl": { + "x": 9.051899219509231, + "y": 2.675809721209913 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.3744191873216245, + "y": 0.8991113186318737 + }, + "prevControl": { + "x": 7.723385112977601, + "y": 1.4241678814355665 + }, + "nextControl": { + "x": 5.626398384673274, + "y": 0.6079599825792492 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.8807644709707976, + "y": 0.6066645408136047 + }, + "prevControl": { + "x": 3.906724520169048, + "y": 0.5968894254103005 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 178.81963121952444 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.3933329278013 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/football otherside intake 2.path b/src/main/deploy/pathplanner/paths/football otherside intake 2.path new file mode 100644 index 0000000..29200ab --- /dev/null +++ b/src/main/deploy/pathplanner/paths/football otherside intake 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 13.506267462342079, + "y": 6.9238565962099115 + }, + "prevControl": null, + "nextControl": { + "x": 14.50626746234208, + "y": 6.9238565962099115 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 15.412414965986393, + "y": 6.8083109359815355 + }, + "prevControl": { + "x": 14.412414965986393, + "y": 6.8083109359815355 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.30072257922677 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -179.76827221562849 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/high shoot flat 2.path b/src/main/deploy/pathplanner/paths/high shoot flat 2.path new file mode 100644 index 0000000..f8fd0cd --- /dev/null +++ b/src/main/deploy/pathplanner/paths/high shoot flat 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8983837078668273, + "y": 2.199665418467734 + }, + "prevControl": null, + "nextControl": { + "x": 3.8983837078668278, + "y": 2.199665418467734 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.619250239158163, + "y": 0.6240752920091648 + }, + "prevControl": { + "x": 1.9235272396719103, + "y": 0.5585276862481111 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -179.1356387852817 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -88.28325926782887 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/low goal finish.path b/src/main/deploy/pathplanner/paths/low goal finish.path new file mode 100644 index 0000000..d30ffa1 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/low goal finish.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.8964236364188527, + "y": 0.6204256256073871 + }, + "prevControl": null, + "nextControl": { + "x": 2.4294008898202137, + "y": 0.6153008078231296 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5683840500485906, + "y": 0.6204256256073871 + }, + "prevControl": { + "x": 2.197550337099125, + "y": 0.6510321762633635 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/score high 2.path b/src/main/deploy/pathplanner/paths/score high 2.path new file mode 100644 index 0000000..57b3c80 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/score high 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.978519421161321, + "y": 2.7376871507531586 + }, + "prevControl": null, + "nextControl": { + "x": 3.84197650924759, + "y": 2.5282688296680176 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.5968078079446062, + "y": 0.6112199344023327 + }, + "prevControl": { + "x": 1.863816030994844, + "y": 0.8564793673544928 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -178.75297650895942 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -148.72053264780297 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/take 3 flat.path b/src/main/deploy/pathplanner/paths/take 3 flat.path new file mode 100644 index 0000000..0ffb4ca --- /dev/null +++ b/src/main/deploy/pathplanner/paths/take 3 flat.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.571468431122449, + "y": 0.768713177235181 + }, + "prevControl": null, + "nextControl": { + "x": 2.8711705695423007, + "y": 0.8517906782938883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 0.9189798206609033 + }, + "prevControl": { + "x": 2.8546664756846116, + "y": 0.6689868103988942 + }, + "nextControl": { + "x": 2.8630504165726527, + "y": 1.790136649285827 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 2.17686106017928 + }, + "prevControl": { + "x": 2.907278601544784, + "y": 1.8958659961380382 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3315587734241905, + "rotationDegrees": -85.69322225973595 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.78832381236471 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.8782176560898 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/take 3.path b/src/main/deploy/pathplanner/paths/take 3.path new file mode 100644 index 0000000..17dd9c3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/take 3.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.6015235433759782, + "y": 0.6497773688686587 + }, + "prevControl": null, + "nextControl": { + "x": 2.3003049992963907, + "y": 0.6643084286264951 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.9091277234265154, + "y": 0.8755345900405657 + }, + "prevControl": { + "x": 2.944079035349721, + "y": 0.3675726922113227 + }, + "nextControl": { + "x": 2.869788703095269, + "y": 1.4472648721572674 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.980649981950894, + "y": 2.6848905275845403 + }, + "prevControl": { + "x": 2.965196582294745, + "y": 2.484849458220909 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.384529386712096, + "rotationDegrees": -89.44080869749423 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -149.21236118610597 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.91420959211533 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..f5c5ca6 --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.051, + "driveGearing": 7.03, + "maxDriveSpeed": 4.54, + "driveMotorType": "krakenX60", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index bfa3737..ecb63b8 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,9 +6,11 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.config.TunerConstants; @@ -43,6 +45,15 @@ public void robotPeriodic() { drivetrain.getModule(3).getPosition(true) }); + + + Command selectedAuto = m_robotContainer.getAutonomousCommand(); + if (selectedAuto != null) { + SmartDashboard.putString("Selected Auto", selectedAuto.getName()); + } else { + SmartDashboard.putString("Selected Auto", "None"); + } + // Updates the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE_STRATEGY // (not in use) VisionConfig.photonPoseEstimatorLeft.setReferencePose( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1cb63d8..fe342c9 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -9,6 +9,10 @@ import com.ctre.phoenix6.hardware.Pigeon2; import com.ctre.phoenix6.swerve.SwerveModule; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.util.PathPlannerLogging; + import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -19,6 +23,8 @@ import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.smartdashboard.Field2d; +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.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -48,7 +54,7 @@ public class RobotContainer { private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); private final Telemetry logger = new Telemetry(MaxSpeed); - + private final SendableChooser autoChooser; private CommandXboxController controller = new CommandXboxController(0); private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Intake intake = new Intake(); @@ -80,9 +86,25 @@ public class RobotContainer { // links xbox controller to controls public RobotContainer() { + + SmartDashboard.putData("Field", m_field); + + SmartDashboard.putData("Field", m_field); + + PathPlannerLogging.setLogActivePathCallback((poses) -> { + m_field.getObject("path").setPoses(poses); + }); + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + configureBindings(); } + public void updateTelemetry() { + m_field.setRobotPose(drivetrain.getState().Pose); + } + @Logged Pose2d estimatedPosition = m_odometry.getEstimatedPosition(); private void configureBindings() { @@ -99,6 +121,33 @@ private void configureBindings() { map.put(Units.inchesToMeters(169.5), 0.12); map.put(Units.inchesToMeters(210.5), 0.118); + NamedCommands.registerCommand( + "idle", + Commands.run(() -> intake.stopIntake(), intake) + .alongWith( + Commands.run(() -> shooter.stopShooter(), shooter) + .alongWith(Commands.run(() -> pivot.pivotDefault(), pivot)))); + NamedCommands.registerCommand( + "high goal shoot", + Commands.run(() -> intake.intake(), intake) + .alongWith( + Commands.run(() -> shooter.shoot(), shooter) + .alongWith( + Commands.run( + () -> + pivot.setPivotAngleRot( + m_odometry + .getEstimatedPosition() + .getTranslation() + .getDistance(pivot.getCosmicConverterTranslation(false))), + pivot)))); + NamedCommands.registerCommand( + "intake", + Commands.run(() -> intake.intake(), intake) + .alongWith( + Commands.run(() -> shooter.stopShooter(), shooter) + .alongWith(Commands.run(() -> pivot.setPivotAngleRot(0.0), pivot)))); + // Default commands pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); shooter.setDefaultCommand(Commands.run(() -> shooter.stopShooter(), shooter)); @@ -164,12 +213,16 @@ private void configureBindings() { controller.rightStick().toggleOnTrue(Commands.run(() -> intake.outtake())); // Low score controller.rightBumper().toggleOnTrue(pivot.lowScore(0.0)); + + } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.getSelected(); } + + public static void zeroPigeon() { Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); pigeon.reset(); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index decf1e0..1f37096 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -7,9 +7,14 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; 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; @@ -37,6 +42,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg; /* Keep track if we've ever applied the operator perspective before or not */ private boolean m_hasAppliedOperatorPerspective = false; + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = + new SwerveRequest.ApplyRobotSpeeds(); /* Swerve requests to apply during SysId characterization */ private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = @@ -113,6 +120,37 @@ public CommandSwerveDrivetrain( if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); + } + + private void configureAutoBuilder() { + try { + var config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, // Supplier of current robot pose + this::resetPose, // Consumer for seeding pose against auto + () -> getState().Speeds, // Supplier of current robot speeds + // Consumer of ChassisSpeeds and feedforwards to drive the robot + (speeds, feedforwards) -> + setControl( + m_pathApplyRobotSpeeds + .withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())), + new PPHolonomicDriveController( + // PID constants for translation + new PIDConstants(1, 0, 0), + // PID constants for rotation + new PIDConstants(1, 0, 0)), + config, + // Assume the path needs to be flipped for Red vs Blue, this is normally the case + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this // Subsystem for requirements + ); + } catch (Exception ex) { + DriverStation.reportError( + "Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + } } /** From 8b42521ecfa2a6a9a8a6bd1a075f7b8616faed44 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 5 Dec 2025 16:26:03 -0500 Subject: [PATCH 23/42] testing --- src/main/java/frc/robot/Robot.java | 24 +++++++++++-------- src/main/java/frc/robot/RobotContainer.java | 18 ++++++-------- .../java/frc/robot/config/TunerConstants.java | 4 ++-- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ecb63b8..9beb29e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,10 +6,10 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -25,6 +25,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @Logged private final RobotContainer m_robotContainer; + @Logged private Field2d field = new Field2d(); public Robot() { m_robotContainer = new RobotContainer(); @@ -45,14 +46,12 @@ public void robotPeriodic() { drivetrain.getModule(3).getPosition(true) }); - - - Command selectedAuto = m_robotContainer.getAutonomousCommand(); - if (selectedAuto != null) { - SmartDashboard.putString("Selected Auto", selectedAuto.getName()); - } else { - SmartDashboard.putString("Selected Auto", "None"); - } + Command selectedAuto = m_robotContainer.getAutonomousCommand(); + if (selectedAuto != null) { + SmartDashboard.putString("Selected Auto", selectedAuto.getName()); + } else { + SmartDashboard.putString("Selected Auto", "None"); + } // Updates the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE_STRATEGY // (not in use) @@ -84,9 +83,12 @@ public void robotPeriodic() { } m_robotContainer.m_odometry.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds); + System.out.println("VISION WORKING\nVISION WORKING"); + //System.out.println((pose.estimatedPose.getX(), pose.estimatedPose.getY()); }); + } else { + System.out.println("Left cam NOT WORKING\nLeft cam NOT WORKING\nLeft cam NOT WORKING\n"); } - results = Vision.rightCameraApril.getAllUnreadResults(); if (!results.isEmpty()) { @@ -102,6 +104,8 @@ public void robotPeriodic() { m_robotContainer.m_odometry.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds); }); + + field.setRobotPose(m_robotContainer.m_odometry.getEstimatedPosition()); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe342c9..cdc7547 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,7 +12,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.util.PathPlannerLogging; - import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; @@ -46,7 +45,7 @@ public class RobotContainer { /* Setting up bindings for necessary control of the swerve drive platform */ private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() - .withDeadband(MaxSpeed * 0.1) + .withDeadband(MaxSpeed * 0.2) .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband .withDriveRequestType( SwerveModule.DriveRequestType @@ -70,7 +69,7 @@ public class RobotContainer { new Translation2d(Units.inchesToMeters(-10.875), Units.inchesToMeters(10.875)); Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-10.855), Units.inchesToMeters(-10.875)); - SwerveDrivePoseEstimator m_odometry = + public SwerveDrivePoseEstimator m_odometry = new SwerveDrivePoseEstimator( new SwerveDriveKinematics( m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation), @@ -90,10 +89,11 @@ public RobotContainer() { SmartDashboard.putData("Field", m_field); SmartDashboard.putData("Field", m_field); - - PathPlannerLogging.setLogActivePathCallback((poses) -> { - m_field.getObject("path").setPoses(poses); - }); + + PathPlannerLogging.setLogActivePathCallback( + (poses) -> { + m_field.getObject("path").setPoses(poses); + }); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); @@ -213,16 +213,12 @@ private void configureBindings() { controller.rightStick().toggleOnTrue(Commands.run(() -> intake.outtake())); // Low score controller.rightBumper().toggleOnTrue(pivot.lowScore(0.0)); - - } public Command getAutonomousCommand() { return autoChooser.getSelected(); } - - public static void zeroPigeon() { Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); pigeon.reset(); diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 53139ff..4f4a905 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -55,7 +55,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(120.0); + private static final Current kSlipCurrent = Amps.of(60); //changed from 120 // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -67,7 +67,7 @@ public class TunerConstants { // Swerve azimuth does not require much torque output, so we can set a relatively // low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(60)) + .withStatorCurrentLimit(Amps.of(30)) //changed (originally 60) .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs From 319884fe8d606b0bfefd9eb0ad49b6fc5bd22550 Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Fri, 5 Dec 2025 16:42:47 -0500 Subject: [PATCH 24/42] update to vision --- src/main/java/frc/robot/Robot.java | 20 +++++++------------ src/main/java/frc/robot/RobotContainer.java | 2 +- .../java/frc/robot/config/VisionConfig.java | 2 +- 3 files changed, 9 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index ecb63b8..8006c9d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -23,7 +23,6 @@ @Logged public class Robot extends TimedRobot { private Command m_autonomousCommand; - CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @Logged private final RobotContainer m_robotContainer; public Robot() { @@ -36,14 +35,6 @@ public Robot() { public void robotPeriodic() { // loop continuously runs as long as the robot is active CommandScheduler.getInstance().run(); - m_robotContainer.m_odometry.update( - new Rotation2d(RobotContainer.pigeon2.getYaw().getValueAsDouble()), - new SwerveModulePosition[] { - drivetrain.getModule(0).getPosition(true), - drivetrain.getModule(1).getPosition(true), - drivetrain.getModule(2).getPosition(true), - drivetrain.getModule(3).getPosition(true) - }); @@ -57,11 +48,14 @@ public void robotPeriodic() { // Updates the stored reference pose for use when using the CLOSEST_TO_REFERENCE_POSE_STRATEGY // (not in use) VisionConfig.photonPoseEstimatorLeft.setReferencePose( - m_robotContainer.m_odometry.getEstimatedPosition()); + m_robotContainer.drivetrain.getState().Pose); VisionConfig.photonPoseEstimatorRight.setReferencePose( - m_robotContainer.m_odometry.getEstimatedPosition()); + m_robotContainer.drivetrain.getState().Pose); // Puts the pose data from one camera into a list + + + List results = Vision.leftCameraApril.getAllUnreadResults(); // If there is pose data from the cameras, get the latest estimated pose and update the 'vision' @@ -82,7 +76,7 @@ public void robotPeriodic() { && result.targets.get(0).bestCameraToTarget.getTranslation().getNorm() > 4) { return; } - m_robotContainer.m_odometry.addVisionMeasurement( + m_robotContainer.drivetrain.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds); }); } @@ -99,7 +93,7 @@ public void robotPeriodic() { && result.targets.get(0).bestCameraToTarget.getTranslation().getNorm() > 4) { return; } - m_robotContainer.m_odometry.addVisionMeasurement( + m_robotContainer.drivetrain.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds); }); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fe342c9..25d79e1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -56,7 +56,7 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); private final SendableChooser autoChooser; private CommandXboxController controller = new CommandXboxController(0); - private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); + public CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Intake intake = new Intake(); private Pivot pivot = new Pivot(); private Shooter shooter = new Shooter(); diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index cd6a7df..8fef1d7 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -79,7 +79,7 @@ public class VisionConfig { public static final AprilTagFieldLayout FIELD_LAYOUT = new AprilTagFieldLayout(APRIL_TAG_LIST, Units.inchesToMeters(648), Units.inchesToMeters(324)); public static final PhotonPoseEstimator.PoseStrategy STRATEGY = - PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; + PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY; public static final Transform3d LEFT_CAMERA_POSITION = new Transform3d( Units.inchesToMeters(9.25), From 2bbd638306a7b7840a8c42f854b32a2b200d34cc Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Fri, 5 Dec 2025 16:55:04 -0500 Subject: [PATCH 25/42] update to vision 2 --- src/main/java/frc/robot/config/VisionConfig.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/config/VisionConfig.java b/src/main/java/frc/robot/config/VisionConfig.java index 8fef1d7..209d863 100644 --- a/src/main/java/frc/robot/config/VisionConfig.java +++ b/src/main/java/frc/robot/config/VisionConfig.java @@ -85,13 +85,13 @@ public class VisionConfig { Units.inchesToMeters(9.25), Units.inchesToMeters(10.5), Units.inchesToMeters(7.5), - new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); + new Rotation3d(0.0, 0.0, 0.0)); public static final Transform3d RIGHT_CAMERA_POSITION = new Transform3d( Units.inchesToMeters(9.25), Units.inchesToMeters(-10.5), Units.inchesToMeters(7.5), - new Rotation3d(0.0, Units.degreesToRadians(30), 0.0)); + new Rotation3d(0.0, 0.0, 0.0)); public static final Transform3d REAR_CAMERA_POSITION = new Transform3d(0.0, 0.0, 0.0, new Rotation3d(0.0, 0.0, 0.0)); Optional visionEst = Optional.empty(); From 23fd9e745c445641bb6031786bb205b8f8b7e7af Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 5 Dec 2025 18:31:51 -0500 Subject: [PATCH 26/42] testing --- src/main/java/frc/robot/Robot.java | 12 +- src/main/java/frc/robot/RobotContainer.java | 66 +-- .../java/frc/robot/config/TunerConstants.java | 7 +- .../java/frc/robot/subsystems/Intake.java | 6 +- src/main/java/frc/robot/subsystems/Pivot.java | 449 +++++++++--------- 5 files changed, 275 insertions(+), 265 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 742bd84..dcd248e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -6,16 +6,12 @@ import edu.wpi.first.epilogue.Epilogue; import edu.wpi.first.epilogue.Logged; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import frc.robot.config.TunerConstants; import frc.robot.config.VisionConfig; -import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.Vision; import java.util.List; import org.photonvision.targeting.PhotonPipelineResult; @@ -25,7 +21,6 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; @Logged private final RobotContainer m_robotContainer; @Logged private Field2d field = new Field2d(); - public Robot() { m_robotContainer = new RobotContainer(); @@ -36,7 +31,6 @@ public Robot() { public void robotPeriodic() { // loop continuously runs as long as the robot is active CommandScheduler.getInstance().run(); - Command selectedAuto = m_robotContainer.getAutonomousCommand(); if (selectedAuto != null) { SmartDashboard.putString("Selected Auto", selectedAuto.getName()); @@ -53,8 +47,6 @@ public void robotPeriodic() { // Puts the pose data from one camera into a list - - List results = Vision.leftCameraApril.getAllUnreadResults(); // If there is pose data from the cameras, get the latest estimated pose and update the 'vision' @@ -77,8 +69,8 @@ public void robotPeriodic() { } m_robotContainer.drivetrain.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds); - System.out.println("VISION WORKING\nVISION WORKING"); - //System.out.println((pose.estimatedPose.getX(), pose.estimatedPose.getY()); + System.out.println("VISION WORKING\nVISION WORKING"); + // System.out.println((pose.estimatedPose.getX(), pose.estimatedPose.getY()); }); } else { System.out.println("Left cam NOT WORKING\nLeft cam NOT WORKING\nLeft cam NOT WORKING\n"); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f4738a1..714d6c4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -59,8 +59,13 @@ public class RobotContainer { private Intake intake = new Intake(); private Pivot pivot = new Pivot(); private Shooter shooter = new Shooter(); + private final SwerveRequest.FieldCentricFacingAngle m_default = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType( + SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); + + public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(10.875)); Translation2d m_frontRightLocation = @@ -106,6 +111,7 @@ public void updateTelemetry() { } @Logged Pose2d estimatedPosition = m_odometry.getEstimatedPosition(); + @Logged private double goalAngle = drivetrain.getState().; private void configureBindings() { final SwerveRequest.Idle idle = new SwerveRequest.Idle(); @@ -167,18 +173,18 @@ private void configureBindings() { // Testing - Commented out until need to be used // Pivot to angle based off distance - controller - .povUp() - .whileTrue( - (Commands.run( - () -> - pivot.setPivotAngleRot( - map.get( - drivetrain - .getState() - .Pose - .getTranslation() - .getDistance(pivot.getCosmicConverterTranslation(false))))))); + // controller + // .povUp() + // .whileTrue( + // (Commands.run( + // () -> + // pivot.setPivotAngleRot( + // map.get( + // drivetrain + // .getState() + // .Pose + // .getTranslation() + // .getDistance(pivot.getCosmicConverterTranslation(false))))))); // controller // .povUp() // .whileTrue( @@ -192,27 +198,33 @@ private void configureBindings() { // // // Shoot - controller - .rightTrigger() - .whileTrue( - Commands.run(() -> shooter.shoot()).alongWith(Commands.run(() -> intake.intake()))); - // Intake + // controller + // .rightTrigger() + // .whileTrue( + // Commands.run((() -> intake.runKicker(-0.7))) + // .withTimeout(0.5) + // .andThen( + // Commands.run(() -> shooter.shoot()) + // .alongWith(Commands.run(() -> intake.intake())))); + // // Intake // controller.leftTrigger().whileTrue(Commands.run(() -> intake.intake())); // Specialized commands // auto align with inner cosmic converter and raise pivot - // controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), - // true)); - // // auto align with outer cosmic converter - // controller - // .leftTrigger() - // .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); + controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); + // auto align with outer cosmic converter + controller + .leftTrigger().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); + controller.rightTrigger().toggleOnFalse(pivot.defaults()); // Intake - controller.leftStick().toggleOnTrue(Commands.run(() -> intake.intake())); + controller + .a() + .toggleOnTrue(Commands.run(() -> intake.intake()).alongWith(pivot.lowScore(0.31))); + // Outtake - controller.rightStick().toggleOnTrue(Commands.run(() -> intake.outtake())); + controller.b().toggleOnTrue(Commands.run(() -> intake.outtake())); // Low score - controller.rightBumper().toggleOnTrue(pivot.lowScore(0.0)); + controller.x().toggleOnTrue(pivot.lowScore(0.0)); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 4f4a905..9b8fcef 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -23,7 +23,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(100) + .withKP(50) .withKI(0) .withKD(0.5) .withKS(0.1) @@ -40,6 +40,7 @@ public class TunerConstants { private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; // The closed-loop output type to use for the drive motors; // This affects the PID/FF gains for the drive motors + // This affects the PID/FF gains for the drive motors private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; // The type of motor used for the drive motor @@ -55,7 +56,7 @@ public class TunerConstants { // The stator current at which the wheels start to slip; // This needs to be tuned to your individual robot - private static final Current kSlipCurrent = Amps.of(60); //changed from 120 + private static final Current kSlipCurrent = Amps.of(60); // changed from 120 // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. @@ -67,7 +68,7 @@ public class TunerConstants { // Swerve azimuth does not require much torque output, so we can set a relatively // low // stator current limit to help avoid brownouts without impacting performance. - .withStatorCurrentLimit(Amps.of(30)) //changed (originally 60) + .withStatorCurrentLimit(Amps.of(30)) // changed (originally 60) .withStatorCurrentLimitEnable(true)); private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6d0354a..6456633 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -77,12 +77,12 @@ public void intake(double initialVelocity, double kickerVelocity) { public void intake() { mInitialIntake.setControl(new DutyCycleOut(-0.5)); - mKickerIntake.setControl(new DutyCycleOut(-0.3)); + mKickerIntake.setControl(new DutyCycleOut(-0.7)); } public void outtake() { mInitialIntake.setControl(new DutyCycleOut(0.5)); - mKickerIntake.setControl(new DutyCycleOut(0.3)); + mKickerIntake.setControl(new DutyCycleOut(0.7)); } public void runKicker(double velocity) { @@ -90,7 +90,7 @@ public void runKicker(double velocity) { } public void runKicker() { - mKickerIntake.setControl(new DutyCycleOut(-0.3)); + mKickerIntake.setControl(new DutyCycleOut(-0.7)); } public void runInitial(double velocity) { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 0cd31c2..e7cdd08 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -24,231 +24,236 @@ @Logged public class Pivot extends SubsystemBase { - protected TalonFX mPivotLeft; - protected TalonFX mPivotRight; - protected Follower follower; - protected CommandSwerveDrivetrain drivetrain; - protected Shooter shooter; - protected Intake intake; - private final InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - - private double currentAngle; - - public Pivot() { - mPivotLeft = new TalonFX(CANMappings.K_PIVOT_LEFT_ID); - mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); - - drivetrain = TunerConstants.createDrivetrain(); - shooter = new Shooter(); - intake = new Intake(); - TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); - TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); - - leftPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - leftPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; - leftPivotConfig.CurrentLimits.StatorCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; - leftPivotConfig.CurrentLimits.SupplyCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; - ; - - rightPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - rightPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; - rightPivotConfig.CurrentLimits.StatorCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; - ; - rightPivotConfig.CurrentLimits.SupplyCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; - ; - - leftPivotConfig.MotionMagic.MotionMagicCruiseVelocity = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; - rightPivotConfig.MotionMagic.MotionMagicCruiseVelocity = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; - leftPivotConfig.MotionMagic.MotionMagicAcceleration = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; - rightPivotConfig.MotionMagic.MotionMagicAcceleration = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; - - leftPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; - leftPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; - leftPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; - leftPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; - leftPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; - leftPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; - leftPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; - - rightPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; - rightPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; - rightPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; - rightPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; - rightPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; - rightPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; - rightPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; - - leftPivotConfig.Feedback.SensorToMechanismRatio = - PivotConfig.K_LEFT_PIVOT_GEAR_RATIO; // gear ratio - rightPivotConfig.Feedback.SensorToMechanismRatio = PivotConfig.K_RIGHT_PIVOT_GEAR_RATIO; - - leftPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - rightPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - // leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - mPivotLeft.getConfigurator().apply(leftPivotConfig); - mPivotRight.getConfigurator().apply(rightPivotConfig); - - // follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, false); - } - - public void setPivotAngle(Rotation2d angleSetpoint) { - mPivotLeft.setControl(new MotionMagicVoltage(angleSetpoint.getRotations())); - mPivotRight.setControl(follower); - } - - public void setPivotAngleRot(double rotation) { - mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); - mPivotRight.setControl(new MotionMagicVoltage(rotation)); - } - - public void pivotDefault() { - mPivotLeft.setControl(new MotionMagicVoltage(0.0)); - mPivotRight.setControl(new MotionMagicVoltage(0.0)); - } - - public void zeroPivot() { - mPivotLeft.setPosition(0.0); - mPivotRight.setPosition(0.0); - } - - public void stopPivot() { - mPivotLeft.stopMotor(); - mPivotRight.stopMotor(); - } - - public boolean pivotAtSetpoint() { - return Math.abs(mPivotLeft.getClosedLoopError().getValueAsDouble()) - <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; - } - - public double getPivotAngleDegrees() { - currentAngle = mPivotLeft.getPosition().getValueAsDouble(); - currentAngle = currentAngle * 360; - - return currentAngle; - } - - private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = - new SwerveRequest.FieldCentricFacingAngle() - .withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - - public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { - Optional alliance1 = DriverStation.getAlliance(); - Translation2d cosmicConverter = new Translation2d(); - map.put(Units.inchesToMeters(59.0), 0.18); - map.put(Units.inchesToMeters(76.5), 0.155); - map.put(Units.inchesToMeters(96.5), 0.142); - map.put(Units.inchesToMeters(125.5), 0.13); - map.put(Units.inchesToMeters(169.5), 0.12); - map.put(Units.inchesToMeters(210.5), 0.118); - if (alliance1.isPresent()) { - if (alliance1.get() == DriverStation.Alliance.Blue) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); - } - } - if (alliance1.get() == DriverStation.Alliance.Red) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); - } - } - - Rotation2d heading = drivetrain.getState().Pose.getRotation(); - - // shooter offset in robot frame (meters) - double shooterOffsetX = 0.0; // forward - double shooterOffsetY = -1; // right - - // convert to field frame - double shooterX = - drivetrain.getState().Pose.getX() - + shooterOffsetX * heading.getCos() - - shooterOffsetY * heading.getSin(); - - double shooterY = - drivetrain.getState().Pose.getY() - + shooterOffsetX * heading.getSin() - + shooterOffsetY * heading.getCos(); - - // compute target angle - Rotation2d aimAngle = - new Rotation2d( - Math.atan2(cosmicConverter.getY() - shooterY, cosmicConverter.getX() - shooterX)); - - return Commands.runOnce( - () -> - drivetrain.setControl( - m_faceAngle.withVelocityX(2).withVelocityY(2).withTargetDirection(aimAngle))) - .alongWith( - Commands.runOnce( - () -> - setPivotAngleRot( - map.get( - drivetrain - .getState() - .Pose - .getTranslation() - .getDistance(getCosmicConverterTranslation(false)))))) - .andThen( - Commands.waitUntil(complete) - .andThen( - Commands.parallel( - Commands.run(() -> shooter.shoot()), - Commands.run(() -> intake.runKicker())) - .until(() -> !complete.getAsBoolean()))); - } else { - cosmicConverter = null; - System.out.println("no alliance detected: likely causing many errors"); - return null; + protected TalonFX mPivotLeft; + protected TalonFX mPivotRight; + protected Follower follower; + protected CommandSwerveDrivetrain drivetrain; + protected Shooter shooter; + protected Intake intake; + private final InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); + + private double currentAngle; + + public Pivot() { + mPivotLeft = new TalonFX(CANMappings.K_PIVOT_LEFT_ID); + mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); + + drivetrain = TunerConstants.createDrivetrain(); + shooter = new Shooter(); + intake = new Intake(); + TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); + TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); + + leftPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + leftPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + leftPivotConfig.CurrentLimits.StatorCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; + leftPivotConfig.CurrentLimits.SupplyCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; + ; + + rightPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + rightPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + rightPivotConfig.CurrentLimits.StatorCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; + ; + rightPivotConfig.CurrentLimits.SupplyCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; + ; + + leftPivotConfig.MotionMagic.MotionMagicCruiseVelocity = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; + rightPivotConfig.MotionMagic.MotionMagicCruiseVelocity = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; + leftPivotConfig.MotionMagic.MotionMagicAcceleration = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; + rightPivotConfig.MotionMagic.MotionMagicAcceleration = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; + + leftPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; + leftPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; + leftPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; + leftPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; + leftPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; + leftPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; + leftPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; + + rightPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; + rightPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; + rightPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; + rightPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; + rightPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; + rightPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; + rightPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; + + leftPivotConfig.Feedback.SensorToMechanismRatio = + PivotConfig.K_LEFT_PIVOT_GEAR_RATIO; // gear ratio + rightPivotConfig.Feedback.SensorToMechanismRatio = PivotConfig.K_RIGHT_PIVOT_GEAR_RATIO; + + leftPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + rightPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + // leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + mPivotLeft.getConfigurator().apply(leftPivotConfig); + mPivotRight.getConfigurator().apply(rightPivotConfig); + + // follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, false); } - } - - public Translation2d getCosmicConverterTranslation(boolean isInner) { - Optional alliance1 = DriverStation.getAlliance(); - Translation2d cosmicConverter = new Translation2d(); - if (alliance1.isPresent()) { - if (alliance1.get() == DriverStation.Alliance.Blue) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); - } - } - if (alliance1.get() == DriverStation.Alliance.Red) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + + public void setPivotAngle(Rotation2d angleSetpoint) { + mPivotLeft.setControl(new MotionMagicVoltage(angleSetpoint.getRotations())); + mPivotRight.setControl(follower); + } + + public void setPivotAngleRot(double rotation) { + mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); + mPivotRight.setControl(new MotionMagicVoltage(rotation)); + } + + public void pivotDefault() { + mPivotLeft.setControl(new MotionMagicVoltage(0.0)); + mPivotRight.setControl(new MotionMagicVoltage(0.0)); + } + + public void zeroPivot() { + mPivotLeft.setPosition(0.0); + mPivotRight.setPosition(0.0); + } + + public void stopPivot() { + mPivotLeft.stopMotor(); + mPivotRight.stopMotor(); + } + + public boolean pivotAtSetpoint() { + return Math.abs(mPivotLeft.getClosedLoopError().getValueAsDouble()) + <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; + } + + public double getPivotAngleDegrees() { + currentAngle = mPivotLeft.getPosition().getValueAsDouble(); + currentAngle = currentAngle * 360; + + return currentAngle; + } + + private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType( + SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle + + public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + map.put(Units.inchesToMeters(59.0), 0.18); + map.put(Units.inchesToMeters(76.5), 0.155); + map.put(Units.inchesToMeters(96.5), 0.142); + map.put(Units.inchesToMeters(125.5), 0.13); + map.put(Units.inchesToMeters(169.5), 0.12); + map.put(Units.inchesToMeters(210.5), 0.118); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } + } + + Rotation2d heading = drivetrain.getState().Pose.getRotation(); + + // shooter offset in robot frame (meters) + double shooterOffsetX = 0.0; // forward + double shooterOffsetY = Units.inchesToMeters(-1); // right + + // convert to field frame + double shooterX = + drivetrain.getState().Pose.getX() + + shooterOffsetX * heading.getCos() + - shooterOffsetY * heading.getSin(); + + double shooterY = + drivetrain.getState().Pose.getY() + + shooterOffsetX * heading.getSin() + + shooterOffsetY * heading.getCos(); + + // compute target angle + Rotation2d aimAngle = + new Rotation2d( + Math.atan2( + cosmicConverter.getY() - drivetrain.getState().Pose.getY(), + cosmicConverter.getX() - drivetrain.getState().Pose.getX())); + + return Commands.runOnce( + () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle))) + .alongWith( + Commands.runOnce( + () -> + setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(getCosmicConverterTranslation(false)))))) + .andThen( + Commands.waitUntil(complete) + .andThen( + Commands.parallel( + Commands.run((() -> intake.runKicker(-0.7))) + .withTimeout(0.5) + .andThen( + Commands.run(() -> shooter.shoot()) + .alongWith(Commands.run(() -> intake.intake())))) + .until(() -> !complete.getAsBoolean()))); } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + cosmicConverter = null; + System.out.println("no alliance detected: likely causing many errors"); + return null; } - } } - return cosmicConverter; - } - public Command lowScore(double angle) { - return Commands.run(() -> setPivotAngleRot(angle)); - } + public Translation2d getCosmicConverterTranslation(boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } + } + } + return cosmicConverter; + } + public Command defaults(){ + return Commands.run(()->drivetrain.setControl(m_faceAngle.withTargetDirection(drivetrain.getState().Pose.getRotation()))); + } + public Command lowScore(double angle) { + return Commands.run(() -> setPivotAngleRot(angle)); + } } From 8b3ace72bea719f3209210be09b35596c039b314 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 5 Dec 2025 18:52:56 -0500 Subject: [PATCH 27/42] testing --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- src/main/java/frc/robot/subsystems/Pivot.java | 5 ++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 714d6c4..96018fa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,8 +111,8 @@ public void updateTelemetry() { } @Logged Pose2d estimatedPosition = m_odometry.getEstimatedPosition(); - @Logged private double goalAngle = drivetrain.getState().; - + @Logged private double goalAngle = drivetrain.getState().ModuleTargets[1].angle.getRotations(); +@Logged private double actualAngle = drivetrain.getState().ModuleStates[1].angle.getRotations(); private void configureBindings() { final SwerveRequest.Idle idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled() diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index e7cdd08..18a2bc0 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -139,8 +139,7 @@ public double getPivotAngleDegrees() { } private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = - new SwerveRequest.FieldCentricFacingAngle() - .withDriveRequestType( + new SwerveRequest.FieldCentricFacingAngle().withDriveRequestType( SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { @@ -197,7 +196,7 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { cosmicConverter.getX() - drivetrain.getState().Pose.getX())); return Commands.runOnce( - () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle))) + () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain) .alongWith( Commands.runOnce( () -> From 6efd179297d549a6700343b6eee300b3f8c82167 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 5 Dec 2025 18:57:19 -0500 Subject: [PATCH 28/42] testing --- src/main/java/frc/robot/config/TunerConstants.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/config/TunerConstants.java b/src/main/java/frc/robot/config/TunerConstants.java index 9b8fcef..28f0a07 100644 --- a/src/main/java/frc/robot/config/TunerConstants.java +++ b/src/main/java/frc/robot/config/TunerConstants.java @@ -23,7 +23,7 @@ public class TunerConstants { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(50) + .withKP(100) .withKI(0) .withKD(0.5) .withKS(0.1) diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 18a2bc0..05c2754 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -195,7 +195,7 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { cosmicConverter.getY() - drivetrain.getState().Pose.getY(), cosmicConverter.getX() - drivetrain.getState().Pose.getX())); - return Commands.runOnce( + return Commands.run( () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain) .alongWith( Commands.runOnce( @@ -207,7 +207,7 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { .Pose .getTranslation() .getDistance(getCosmicConverterTranslation(false)))))) - .andThen( + .withDeadline( Commands.waitUntil(complete) .andThen( Commands.parallel( From 46cba71ad978e9be6ea2aaf6544cc339cec3a5df Mon Sep 17 00:00:00 2001 From: Brayden Zee Date: Fri, 5 Dec 2025 19:00:03 -0500 Subject: [PATCH 29/42] Simple turn --- src/main/java/frc/robot/subsystems/Pivot.java | 42 +++++++++---------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 05c2754..e6a2d7c 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -196,27 +196,27 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { cosmicConverter.getX() - drivetrain.getState().Pose.getX())); return Commands.run( - () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain) - .alongWith( - Commands.runOnce( - () -> - setPivotAngleRot( - map.get( - drivetrain - .getState() - .Pose - .getTranslation() - .getDistance(getCosmicConverterTranslation(false)))))) - .withDeadline( - Commands.waitUntil(complete) - .andThen( - Commands.parallel( - Commands.run((() -> intake.runKicker(-0.7))) - .withTimeout(0.5) - .andThen( - Commands.run(() -> shooter.shoot()) - .alongWith(Commands.run(() -> intake.intake())))) - .until(() -> !complete.getAsBoolean()))); + () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain).until(complete); + // .alongWith( + // Commands.runOnce( + // () -> + // setPivotAngleRot( + // map.get( + // drivetrain + // .getState() + // .Pose + // .getTranslation() + // .getDistance(getCosmicConverterTranslation(false)))))) + // .withDeadline( + // Commands.waitUntil(complete) + // .andThen( + // Commands.parallel( + // Commands.run((() -> intake.runKicker(-0.7))) + // .withTimeout(0.5) + // .andThen( + // Commands.run(() -> shooter.shoot()) + // .alongWith(Commands.run(() -> intake.intake())))) + // .until(() -> !complete.getAsBoolean()))); } else { cosmicConverter = null; System.out.println("no alliance detected: likely causing many errors"); From 2636bb320de88ee8fe5186ca11e89926d9504894 Mon Sep 17 00:00:00 2001 From: Brayden Zee Date: Fri, 5 Dec 2025 19:03:23 -0500 Subject: [PATCH 30/42] Register the drivetrain --- src/main/java/frc/robot/RobotContainer.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 96018fa..27758ce 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ 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.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; @@ -91,6 +92,8 @@ public class RobotContainer { // links xbox controller to controls public RobotContainer() { + CommandScheduler.getInstance().registerSubsystem(drivetrain); + SmartDashboard.putData("Field", m_field); SmartDashboard.putData("Field", m_field); From d733930021fd80d2e5f41a2a5d9f9567f912666f Mon Sep 17 00:00:00 2001 From: Brayden Zee Date: Fri, 5 Dec 2025 19:08:11 -0500 Subject: [PATCH 31/42] Remove duplicated drive trains --- 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 27758ce..218eced 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -58,8 +58,8 @@ public class RobotContainer { private CommandXboxController controller = new CommandXboxController(0); public CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); private Intake intake = new Intake(); - private Pivot pivot = new Pivot(); private Shooter shooter = new Shooter(); + private Pivot pivot = new Pivot(drivetrain, shooter, intake); private final SwerveRequest.FieldCentricFacingAngle m_default = new SwerveRequest.FieldCentricFacingAngle() .withDriveRequestType( From 5f60dceae709e358e4373b7a0caf928a0fb5d816 Mon Sep 17 00:00:00 2001 From: Brayden Zee Date: Fri, 5 Dec 2025 19:09:18 -0500 Subject: [PATCH 32/42] Actually fix things --- src/main/java/frc/robot/subsystems/Pivot.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index e6a2d7c..0a665bd 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -34,13 +34,13 @@ public class Pivot extends SubsystemBase { private double currentAngle; - public Pivot() { + public Pivot(CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake) { mPivotLeft = new TalonFX(CANMappings.K_PIVOT_LEFT_ID); mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); - drivetrain = TunerConstants.createDrivetrain(); - shooter = new Shooter(); - intake = new Intake(); + this.drivetrain = drivetrain; + this.shooter = shooter; + this.intake = intake; TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); From e9395f06290c91666f1817607d1418c961d3669a Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 5 Dec 2025 20:31:54 -0500 Subject: [PATCH 33/42] testing --- .../java/frc/robot/subsystems/Intake.java | 2 +- src/main/java/frc/robot/subsystems/Pivot.java | 45 +++++++++---------- 2 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index 6456633..d26aa06 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -77,7 +77,7 @@ public void intake(double initialVelocity, double kickerVelocity) { public void intake() { mInitialIntake.setControl(new DutyCycleOut(-0.5)); - mKickerIntake.setControl(new DutyCycleOut(-0.7)); + mKickerIntake.setControl(new DutyCycleOut(-1)); } public void outtake() { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 0a665bd..092550e 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -140,7 +140,7 @@ public double getPivotAngleDegrees() { private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = new SwerveRequest.FieldCentricFacingAngle().withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle + SwerveModule.DriveRequestType.OpenLoopVoltage).withHeadingPID(6, 0, 0); // Or OpenLoopDutyCycle public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { Optional alliance1 = DriverStation.getAlliance(); @@ -193,30 +193,29 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { new Rotation2d( Math.atan2( cosmicConverter.getY() - drivetrain.getState().Pose.getY(), - cosmicConverter.getX() - drivetrain.getState().Pose.getX())); + cosmicConverter.getX() - drivetrain.getState().Pose.getX())+Math.PI/2+Units.degreesToRadians(8)); return Commands.run( - () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain).until(complete); - // .alongWith( - // Commands.runOnce( - // () -> - // setPivotAngleRot( - // map.get( - // drivetrain - // .getState() - // .Pose - // .getTranslation() - // .getDistance(getCosmicConverterTranslation(false)))))) - // .withDeadline( - // Commands.waitUntil(complete) - // .andThen( - // Commands.parallel( - // Commands.run((() -> intake.runKicker(-0.7))) - // .withTimeout(0.5) - // .andThen( - // Commands.run(() -> shooter.shoot()) - // .alongWith(Commands.run(() -> intake.intake())))) - // .until(() -> !complete.getAsBoolean()))); + () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain) + .alongWith(run( + () -> + setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(getCosmicConverterTranslation(false)))-0.01))) + .withDeadline( + Commands.waitUntil(complete) + .andThen( + Commands.parallel( + Commands.run((() -> intake.runKicker(0.2))) + .withTimeout(0.25) + .andThen( + Commands.run(() -> shooter.shoot()) + .alongWith(Commands.run(() -> intake.intake())))) + .until(() -> !complete.getAsBoolean()))); } else { cosmicConverter = null; System.out.println("no alliance detected: likely causing many errors"); From 389f0070cc1d748f18ba64491be6fa3714b0efa8 Mon Sep 17 00:00:00 2001 From: Brayden Zee Date: Fri, 5 Dec 2025 20:35:18 -0500 Subject: [PATCH 34/42] Recompute angle constantly --- src/main/java/frc/robot/subsystems/Pivot.java | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index 092550e..a65f821 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -170,6 +170,8 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); } } + + final Translation2d cosmicConverterFinal = cosmicConverter; Rotation2d heading = drivetrain.getState().Pose.getRotation(); @@ -188,15 +190,11 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { + shooterOffsetX * heading.getSin() + shooterOffsetY * heading.getCos(); - // compute target angle - Rotation2d aimAngle = - new Rotation2d( - Math.atan2( - cosmicConverter.getY() - drivetrain.getState().Pose.getY(), - cosmicConverter.getX() - drivetrain.getState().Pose.getX())+Math.PI/2+Units.degreesToRadians(8)); - return Commands.run( - () -> drivetrain.setControl(m_faceAngle.withTargetDirection(aimAngle)), drivetrain) + () -> drivetrain.setControl(m_faceAngle.withTargetDirection(new Rotation2d( + Math.atan2( + cosmicConverterFinal.getY() - drivetrain.getState().Pose.getY(), + cosmicConverterFinal.getX() - drivetrain.getState().Pose.getX())+Math.PI/2+Units.degreesToRadians(8)))), drivetrain) .alongWith(run( () -> setPivotAngleRot( @@ -217,7 +215,6 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { .alongWith(Commands.run(() -> intake.intake())))) .until(() -> !complete.getAsBoolean()))); } else { - cosmicConverter = null; System.out.println("no alliance detected: likely causing many errors"); return null; } From b09ba741ee4c172182d2853031ee95823209f703 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Fri, 5 Dec 2025 22:42:10 -0500 Subject: [PATCH 35/42] testing --- src/main/java/frc/robot/Robot.java | 1 + src/main/java/frc/robot/RobotContainer.java | 42 +- src/main/java/frc/robot/subsystems/Pivot.java | 443 +++++++++--------- 3 files changed, 250 insertions(+), 236 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index dcd248e..da07bf1 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -21,6 +21,7 @@ public class Robot extends TimedRobot { private Command m_autonomousCommand; @Logged private final RobotContainer m_robotContainer; @Logged private Field2d field = new Field2d(); + public Robot() { m_robotContainer = new RobotContainer(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 218eced..1eb1c2e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -60,13 +61,12 @@ public class RobotContainer { private Intake intake = new Intake(); private Shooter shooter = new Shooter(); private Pivot pivot = new Pivot(drivetrain, shooter, intake); - private final SwerveRequest.FieldCentricFacingAngle m_default = - new SwerveRequest.FieldCentricFacingAngle() - .withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - + private final SwerveRequest.FieldCentricFacingAngle m_default = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType( + SwerveModule.DriveRequestType.OpenLoopVoltage); // Or OpenLoopDutyCycle - public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); + public static Pigeon2 pigeon2 = new Pigeon2(CANMappings.PIGEON_CAN_ID); Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(10.875), Units.inchesToMeters(10.875)); Translation2d m_frontRightLocation = @@ -92,7 +92,7 @@ public class RobotContainer { // links xbox controller to controls public RobotContainer() { - CommandScheduler.getInstance().registerSubsystem(drivetrain); + CommandScheduler.getInstance().registerSubsystem(drivetrain); SmartDashboard.putData("Field", m_field); @@ -114,8 +114,9 @@ public void updateTelemetry() { } @Logged Pose2d estimatedPosition = m_odometry.getEstimatedPosition(); - @Logged private double goalAngle = drivetrain.getState().ModuleTargets[1].angle.getRotations(); -@Logged private double actualAngle = drivetrain.getState().ModuleStates[1].angle.getRotations(); + @Logged private double goalAngle = drivetrain.getState().ModuleTargets[1].angle.getRotations(); + @Logged private double actualAngle = drivetrain.getState().ModuleStates[1].angle.getRotations(); + private void configureBindings() { final SwerveRequest.Idle idle = new SwerveRequest.Idle(); RobotModeTriggers.disabled() @@ -138,18 +139,12 @@ private void configureBindings() { .alongWith(Commands.run(() -> pivot.pivotDefault(), pivot)))); NamedCommands.registerCommand( "high goal shoot", - Commands.run(() -> intake.intake(), intake) - .alongWith( - Commands.run(() -> shooter.shoot(), shooter) - .alongWith( - Commands.run( - () -> - pivot.setPivotAngleRot( - m_odometry - .getEstimatedPosition() - .getTranslation() - .getDistance(pivot.getCosmicConverterTranslation(false))), - pivot)))); + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + .withTimeout(1) + .andThen( + Commands.run(() -> shooter.shoot(), shooter)) + .withTimeout(1) + .andThen(Commands.run(() -> intake.intake(), intake))); NamedCommands.registerCommand( "intake", Commands.run(() -> intake.intake(), intake) @@ -217,7 +212,8 @@ private void configureBindings() { controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); // auto align with outer cosmic converter controller - .leftTrigger().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); + .leftTrigger() + .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); controller.rightTrigger().toggleOnFalse(pivot.defaults()); // Intake controller @@ -231,7 +227,7 @@ private void configureBindings() { } public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return new PathPlannerAuto("take 3 high shoot then there"); } public static void zeroPigeon() { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index a65f821..abda2ba 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -18,237 +18,254 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.config.CANMappings; import frc.robot.config.PivotConfig; -import frc.robot.config.TunerConstants; import java.util.Optional; import java.util.function.BooleanSupplier; @Logged public class Pivot extends SubsystemBase { - protected TalonFX mPivotLeft; - protected TalonFX mPivotRight; - protected Follower follower; - protected CommandSwerveDrivetrain drivetrain; - protected Shooter shooter; - protected Intake intake; - private final InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - - private double currentAngle; - - public Pivot(CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake) { - mPivotLeft = new TalonFX(CANMappings.K_PIVOT_LEFT_ID); - mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); - - this.drivetrain = drivetrain; - this.shooter = shooter; - this.intake = intake; - TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); - TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); - - leftPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - leftPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; - leftPivotConfig.CurrentLimits.StatorCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; - leftPivotConfig.CurrentLimits.SupplyCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; - ; - - rightPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; - rightPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; - rightPivotConfig.CurrentLimits.StatorCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; - ; - rightPivotConfig.CurrentLimits.SupplyCurrentLimit = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; - ; - - leftPivotConfig.MotionMagic.MotionMagicCruiseVelocity = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; - rightPivotConfig.MotionMagic.MotionMagicCruiseVelocity = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; - leftPivotConfig.MotionMagic.MotionMagicAcceleration = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; - rightPivotConfig.MotionMagic.MotionMagicAcceleration = - PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; - - leftPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; - leftPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; - leftPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; - leftPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; - leftPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; - leftPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; - leftPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; - - rightPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; - rightPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; - rightPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; - rightPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; - rightPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; - rightPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; - rightPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; - - leftPivotConfig.Feedback.SensorToMechanismRatio = - PivotConfig.K_LEFT_PIVOT_GEAR_RATIO; // gear ratio - rightPivotConfig.Feedback.SensorToMechanismRatio = PivotConfig.K_RIGHT_PIVOT_GEAR_RATIO; - - leftPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - rightPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - // leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - mPivotLeft.getConfigurator().apply(leftPivotConfig); - mPivotRight.getConfigurator().apply(rightPivotConfig); - - // follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, false); - } + protected TalonFX mPivotLeft; + protected TalonFX mPivotRight; + protected Follower follower; + protected CommandSwerveDrivetrain drivetrain; + protected Shooter shooter; + protected Intake intake; + private final InterpolatingDoubleTreeMap map = new InterpolatingDoubleTreeMap(); - public void setPivotAngle(Rotation2d angleSetpoint) { - mPivotLeft.setControl(new MotionMagicVoltage(angleSetpoint.getRotations())); - mPivotRight.setControl(follower); - } + private double currentAngle; - public void setPivotAngleRot(double rotation) { - mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); - mPivotRight.setControl(new MotionMagicVoltage(rotation)); - } + public Pivot(CommandSwerveDrivetrain drivetrain, Shooter shooter, Intake intake) { + mPivotLeft = new TalonFX(CANMappings.K_PIVOT_LEFT_ID); + mPivotRight = new TalonFX(CANMappings.K_PIVOT_RIGHT_ID); - public void pivotDefault() { - mPivotLeft.setControl(new MotionMagicVoltage(0.0)); - mPivotRight.setControl(new MotionMagicVoltage(0.0)); - } + this.drivetrain = drivetrain; + this.shooter = shooter; + this.intake = intake; + TalonFXConfiguration leftPivotConfig = new TalonFXConfiguration(); + TalonFXConfiguration rightPivotConfig = new TalonFXConfiguration(); - public void zeroPivot() { - mPivotLeft.setPosition(0.0); - mPivotRight.setPosition(0.0); - } + leftPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + leftPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + leftPivotConfig.CurrentLimits.StatorCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; + leftPivotConfig.CurrentLimits.SupplyCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; + ; - public void stopPivot() { - mPivotLeft.stopMotor(); - mPivotRight.stopMotor(); - } + rightPivotConfig.CurrentLimits.SupplyCurrentLimitEnable = true; + rightPivotConfig.CurrentLimits.StatorCurrentLimitEnable = true; + rightPivotConfig.CurrentLimits.StatorCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_STATOR_CURRENT_LIMIT; + ; + rightPivotConfig.CurrentLimits.SupplyCurrentLimit = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_SUPPLY_CURRENT_LIMIT; + ; - public boolean pivotAtSetpoint() { - return Math.abs(mPivotLeft.getClosedLoopError().getValueAsDouble()) - <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; - } + leftPivotConfig.MotionMagic.MotionMagicCruiseVelocity = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; + rightPivotConfig.MotionMagic.MotionMagicCruiseVelocity = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_MAX_CRUISE_VELOCITY; + leftPivotConfig.MotionMagic.MotionMagicAcceleration = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; + rightPivotConfig.MotionMagic.MotionMagicAcceleration = + PivotConfig.K_LEFT_AND_RIGHT_PIVOT_TARGET_ACCELERATION; - public double getPivotAngleDegrees() { - currentAngle = mPivotLeft.getPosition().getValueAsDouble(); - currentAngle = currentAngle * 360; + leftPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; + leftPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; + leftPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; + leftPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; + leftPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; + leftPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; + leftPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; - return currentAngle; - } + rightPivotConfig.Slot0.kP = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_P; + rightPivotConfig.Slot0.kI = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_I; + rightPivotConfig.Slot0.kD = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_D; + rightPivotConfig.Slot0.kS = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_S; + rightPivotConfig.Slot0.kG = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_G; + rightPivotConfig.Slot0.kV = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_V; + rightPivotConfig.Slot0.kA = PivotConfig.K_LEFT_AND_RIGHT_PIVOT_A; + + leftPivotConfig.Feedback.SensorToMechanismRatio = + PivotConfig.K_LEFT_PIVOT_GEAR_RATIO; // gear ratio + rightPivotConfig.Feedback.SensorToMechanismRatio = PivotConfig.K_RIGHT_PIVOT_GEAR_RATIO; + + leftPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + rightPivotConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + // leftPivotConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; + + mPivotLeft.getConfigurator().apply(leftPivotConfig); + mPivotRight.getConfigurator().apply(rightPivotConfig); + + // follower = new Follower(CANMappings.K_PIVOT_LEFT_ID, false); + } + + public void setPivotAngle(Rotation2d angleSetpoint) { + mPivotLeft.setControl(new MotionMagicVoltage(angleSetpoint.getRotations())); + mPivotRight.setControl(follower); + } + + public void setPivotAngleRot(double rotation) { + mPivotLeft.setControl(new MotionMagicVoltage(-rotation)); + mPivotRight.setControl(new MotionMagicVoltage(rotation)); + } + + public void pivotDefault() { + mPivotLeft.setControl(new MotionMagicVoltage(0.0)); + mPivotRight.setControl(new MotionMagicVoltage(0.0)); + } + + public void zeroPivot() { + mPivotLeft.setPosition(0.0); + mPivotRight.setPosition(0.0); + } + + public void stopPivot() { + mPivotLeft.stopMotor(); + mPivotRight.stopMotor(); + } + + public boolean pivotAtSetpoint() { + return Math.abs(mPivotLeft.getClosedLoopError().getValueAsDouble()) + <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; + } - private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = - new SwerveRequest.FieldCentricFacingAngle().withDriveRequestType( - SwerveModule.DriveRequestType.OpenLoopVoltage).withHeadingPID(6, 0, 0); // Or OpenLoopDutyCycle - - public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { - Optional alliance1 = DriverStation.getAlliance(); - Translation2d cosmicConverter = new Translation2d(); - map.put(Units.inchesToMeters(59.0), 0.18); - map.put(Units.inchesToMeters(76.5), 0.155); - map.put(Units.inchesToMeters(96.5), 0.142); - map.put(Units.inchesToMeters(125.5), 0.13); - map.put(Units.inchesToMeters(169.5), 0.12); - map.put(Units.inchesToMeters(210.5), 0.118); - if (alliance1.isPresent()) { - if (alliance1.get() == DriverStation.Alliance.Blue) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); - } - } - if (alliance1.get() == DriverStation.Alliance.Red) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); - } - } - - final Translation2d cosmicConverterFinal = cosmicConverter; - - Rotation2d heading = drivetrain.getState().Pose.getRotation(); - - // shooter offset in robot frame (meters) - double shooterOffsetX = 0.0; // forward - double shooterOffsetY = Units.inchesToMeters(-1); // right - - // convert to field frame - double shooterX = - drivetrain.getState().Pose.getX() - + shooterOffsetX * heading.getCos() - - shooterOffsetY * heading.getSin(); - - double shooterY = - drivetrain.getState().Pose.getY() - + shooterOffsetX * heading.getSin() - + shooterOffsetY * heading.getCos(); - - return Commands.run( - () -> drivetrain.setControl(m_faceAngle.withTargetDirection(new Rotation2d( - Math.atan2( - cosmicConverterFinal.getY() - drivetrain.getState().Pose.getY(), - cosmicConverterFinal.getX() - drivetrain.getState().Pose.getX())+Math.PI/2+Units.degreesToRadians(8)))), drivetrain) - .alongWith(run( - () -> - setPivotAngleRot( - map.get( - drivetrain - .getState() - .Pose - .getTranslation() - .getDistance(getCosmicConverterTranslation(false)))-0.01))) - .withDeadline( - Commands.waitUntil(complete) - .andThen( - Commands.parallel( - Commands.run((() -> intake.runKicker(0.2))) - .withTimeout(0.25) - .andThen( - Commands.run(() -> shooter.shoot()) - .alongWith(Commands.run(() -> intake.intake())))) - .until(() -> !complete.getAsBoolean()))); + public double getPivotAngleDegrees() { + currentAngle = mPivotLeft.getPosition().getValueAsDouble(); + currentAngle = currentAngle * 360; + + return currentAngle; + } + + private final SwerveRequest.FieldCentricFacingAngle m_faceAngle = + new SwerveRequest.FieldCentricFacingAngle() + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage) + .withHeadingPID(6, 0, 0); // Or OpenLoopDutyCycle + + public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + map.put(Units.inchesToMeters(59.0), 0.18); + map.put(Units.inchesToMeters(76.5), 0.155); + map.put(Units.inchesToMeters(96.5), 0.142); + map.put(Units.inchesToMeters(125.5), 0.13); + map.put(Units.inchesToMeters(169.5), 0.12); + map.put(Units.inchesToMeters(210.5), 0.118); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); + } + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); } else { - System.out.println("no alliance detected: likely causing many errors"); - return null; + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); } + } + + final Translation2d cosmicConverterFinal = cosmicConverter; + + Rotation2d heading = drivetrain.getState().Pose.getRotation(); + + // shooter offset in robot frame (meters) + double shooterOffsetX = 0.0; // forward + double shooterOffsetY = Units.inchesToMeters(-1); // right + + // convert to field frame + double shooterX = + drivetrain.getState().Pose.getX() + + shooterOffsetX * heading.getCos() + - shooterOffsetY * heading.getSin(); + + double shooterY = + drivetrain.getState().Pose.getY() + + shooterOffsetX * heading.getSin() + + shooterOffsetY * heading.getCos(); + + return Commands.run( + () -> + drivetrain.setControl( + m_faceAngle.withTargetDirection( + new Rotation2d( + Math.atan2( + cosmicConverterFinal.getY() + - drivetrain.getState().Pose.getY(), + cosmicConverterFinal.getX() + - drivetrain.getState().Pose.getX()) + + Units.degreesToRadians(-3.0)))), + drivetrain) + .alongWith( + run( + () -> + setPivotAngleRot( + map.get( + drivetrain + .getState() + .Pose + .getTranslation() + .getDistance(getCosmicConverterTranslation(false))) + - 0.01))) + .withDeadline( + Commands.waitUntil(complete) + .andThen( + Commands.parallel( + Commands.run((() -> intake.runKicker(0.2))) + .withTimeout(0.25) + .andThen( + Commands.run(() -> shooter.shoot()) + .withTimeout(1) + .andThen( + Commands.run(() -> intake.intake()) + .alongWith(Commands.run(() -> shooter.shoot()))))) + .until(() -> !complete.getAsBoolean()))); + } else { + System.out.println("no alliance detected: likely causing many errors"); + return null; } + } - public Translation2d getCosmicConverterTranslation(boolean isInner) { - Optional alliance1 = DriverStation.getAlliance(); - Translation2d cosmicConverter = new Translation2d(); - if (alliance1.isPresent()) { - if (alliance1.get() == DriverStation.Alliance.Blue) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); - } - } - if (alliance1.get() == DriverStation.Alliance.Red) { - if (isInner) { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); - } else { - cosmicConverter = - new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); - } - } + public Translation2d getCosmicConverterTranslation(boolean isInner) { + Optional alliance1 = DriverStation.getAlliance(); + Translation2d cosmicConverter = new Translation2d(); + if (alliance1.isPresent()) { + if (alliance1.get() == DriverStation.Alliance.Blue) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(4.0), Units.inchesToMeters(20.5)); } - return cosmicConverter; - } - public Command defaults(){ - return Commands.run(()->drivetrain.setControl(m_faceAngle.withTargetDirection(drivetrain.getState().Pose.getRotation()))); - } - public Command lowScore(double angle) { - return Commands.run(() -> setPivotAngleRot(angle)); + } + if (alliance1.get() == DriverStation.Alliance.Red) { + if (isInner) { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(196.125)); + } else { + cosmicConverter = + new Translation2d(Units.inchesToMeters(644.0), Units.inchesToMeters(20.5)); + } + } } + return cosmicConverter; + } + + public Command defaults() { + return Commands.run( + () -> + drivetrain.setControl( + m_faceAngle.withTargetDirection(drivetrain.getState().Pose.getRotation()))); + } + + public Command lowScore(double angle) { + return Commands.run(() -> setPivotAngleRot(angle)); + } } From cd38e54fff09fe45a50b6848569bb6df69028c06 Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Sat, 6 Dec 2025 08:33:20 -0500 Subject: [PATCH 36/42] test2 --- .../deploy/pathplanner/autos/New Auto.auto | 32 ++++++++ .../autos/take 3 high shoot then there.auto | 77 ++++++++++++------- .../paths/Copy of take 3 flat.path | 75 ++++++++++++++++++ src/main/java/frc/robot/Robot.java | 2 - src/main/java/frc/robot/RobotContainer.java | 44 ++++++++--- 5 files changed, 189 insertions(+), 41 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Copy of take 3 flat.path diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..fa4a3f3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,32 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "wait 5 seconds" + } + }, + { + "type": "named", + "data": { + "name": "high goal shoot" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto index 0b963c7..b1d798d 100644 --- a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto +++ b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto @@ -5,33 +5,66 @@ "data": { "commands": [ { - "type": "named", + "type": "deadline", "data": { - "name": "high goal shoot" + "commands": [ + { + "type": "named", + "data": { + "name": "wait 5 seconds" + } + }, + { + "type": "named", + "data": { + "name": "high goal shoot" + } + } + ] } }, { - "type": "wait", + "type": "deadline", "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "idle" + "commands": [ + { + "type": "path", + "data": { + "pathName": "take 3 flat" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + } + ] } }, { - "type": "named", + "type": "path", "data": { - "name": "intake" + "pathName": "high shoot flat 2" } }, { - "type": "path", + "type": "deadline", "data": { - "pathName": "take 3 flat" + "commands": [ + { + "type": "named", + "data": { + "name": "wait 5 seconds" + } + }, + { + "type": "named", + "data": { + "name": "high goal shoot" + } + } + ] } }, { @@ -43,25 +76,13 @@ { "type": "path", "data": { - "pathName": "high shoot flat 2" - } - }, - { - "type": "named", - "data": { - "name": "high goal shoot" + "pathName": "all the way out 1" } }, { "type": "named", "data": { - "name": "idle" - } - }, - { - "type": "path", - "data": { - "pathName": "all the way out 1" + "name": null } } ] diff --git a/src/main/deploy/pathplanner/paths/Copy of take 3 flat.path b/src/main/deploy/pathplanner/paths/Copy of take 3 flat.path new file mode 100644 index 0000000..73f3603 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Copy of take 3 flat.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.571468431122449, + "y": 0.768713177235181 + }, + "prevControl": null, + "nextControl": { + "x": 2.8711705695423007, + "y": 0.8517906782938883 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 0.9189798206609033 + }, + "prevControl": { + "x": 2.8546664756846116, + "y": 0.6689868103988942 + }, + "nextControl": { + "x": 2.8630504165726527, + "y": 1.790136649285827 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.856535919489323, + "y": 7.559346286257558 + }, + "prevControl": { + "x": 2.907278601544784, + "y": 7.278351222216316 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.3315587734241905, + "rotationDegrees": -85.69322225973595 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -88.78832381236471 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 179.8782176560898 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index da07bf1..21c3ef0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -70,11 +70,9 @@ public void robotPeriodic() { } m_robotContainer.drivetrain.addVisionMeasurement( pose.estimatedPose.toPose2d(), pose.timestampSeconds); - System.out.println("VISION WORKING\nVISION WORKING"); // System.out.println((pose.estimatedPose.getX(), pose.estimatedPose.getY()); }); } else { - System.out.println("Left cam NOT WORKING\nLeft cam NOT WORKING\nLeft cam NOT WORKING\n"); } results = Vision.rightCameraApril.getAllUnreadResults(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1eb1c2e..a0ec32a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -131,26 +131,34 @@ private void configureBindings() { map.put(Units.inchesToMeters(169.5), 0.12); map.put(Units.inchesToMeters(210.5), 0.118); + NamedCommands.registerCommand("wait 5 seconds", Commands.waitSeconds(5)); + NamedCommands.registerCommand( "idle", - Commands.run(() -> intake.stopIntake(), intake) + Commands.runOnce(() -> intake.stopIntake(), intake) .alongWith( - Commands.run(() -> shooter.stopShooter(), shooter) - .alongWith(Commands.run(() -> pivot.pivotDefault(), pivot)))); + Commands.runOnce(() -> shooter.stopShooter(), shooter) + .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot)))); + NamedCommands.registerCommand( "high goal shoot", - Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> System.out.println("Pivot going"))) .withTimeout(1) .andThen( - Commands.run(() -> shooter.shoot(), shooter)) - .withTimeout(1) - .andThen(Commands.run(() -> intake.intake(), intake))); + Commands.run(() -> shooter.shoot(), shooter) + .alongWith(Commands.run(() -> System.out.println("Shooter going"))) + // Timeout applied to shooter command only + .alongWith( + Commands.run(() -> intake.intake(), intake) + .alongWith(Commands.run(() -> System.out.println("Intake going"))) + ))); + NamedCommands.registerCommand( "intake", - Commands.run(() -> intake.intake(), intake) + Commands.run(() -> intake.intake(), intake).alongWith(Commands.run((() -> System.out.println("Intake intaking off ground"))) .alongWith( - Commands.run(() -> shooter.stopShooter(), shooter) - .alongWith(Commands.run(() -> pivot.setPivotAngleRot(0.0), pivot)))); + Commands.run(() -> shooter.stopShooter(), shooter).alongWith(Commands.run(() -> System.out.println("Shooter stopped"))) + .alongWith(Commands.run(() -> pivot.lowScore(0.31), pivot).alongWith(Commands.run(() -> System.out.println("Pivot to intake position"))))))); // Default commands pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); @@ -209,6 +217,9 @@ private void configureBindings() { // Specialized commands // auto align with inner cosmic converter and raise pivot + + // Intake + controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); // auto align with outer cosmic converter controller @@ -228,7 +239,18 @@ private void configureBindings() { public Command getAutonomousCommand() { return new PathPlannerAuto("take 3 high shoot then there"); - } + +// Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> System.out.println("Pivot going"))) +// .withTimeout(1) +// .andThen( +// Commands.run(() -> shooter.shoot(), shooter) +// .alongWith(Commands.run(() -> System.out.println("Shooter going"))) +// // Timeout applied to shooter command only +// .alongWith( +// Commands.run(() -> intake.intake(), intake) +// .alongWith(Commands.run(() -> System.out.println("Intake going"))) +// )).withDeadline(Commands.waitSeconds(5)); +} public static void zeroPigeon() { Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); From 91fbab3c16571026f7afd063c2b40f8c9d8f00a6 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Sat, 6 Dec 2025 08:46:52 -0500 Subject: [PATCH 37/42] testing --- src/main/java/frc/robot/RobotContainer.java | 69 ++++++++++++--------- 1 file changed, 41 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a0ec32a..dfac0ff 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -142,23 +142,32 @@ private void configureBindings() { NamedCommands.registerCommand( "high goal shoot", - Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> System.out.println("Pivot going"))) + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + .alongWith(Commands.run(() -> System.out.println("Pivot going"))) .withTimeout(1) .andThen( Commands.run(() -> shooter.shoot(), shooter) .alongWith(Commands.run(() -> System.out.println("Shooter going"))) - // Timeout applied to shooter command only - .alongWith( - Commands.run(() -> intake.intake(), intake) - .alongWith(Commands.run(() -> System.out.println("Intake going"))) - ))); - + // Timeout applied to shooter command only + .alongWith( + Commands.run(() -> intake.intake(), intake) + .alongWith(Commands.run(() -> System.out.println("Intake going")))))); + NamedCommands.registerCommand( "intake", - Commands.run(() -> intake.intake(), intake).alongWith(Commands.run((() -> System.out.println("Intake intaking off ground"))) + Commands.run(() -> intake.intake(), intake) .alongWith( - Commands.run(() -> shooter.stopShooter(), shooter).alongWith(Commands.run(() -> System.out.println("Shooter stopped"))) - .alongWith(Commands.run(() -> pivot.lowScore(0.31), pivot).alongWith(Commands.run(() -> System.out.println("Pivot to intake position"))))))); + Commands.run((() -> System.out.println("Intake intaking off ground"))) + .alongWith( + Commands.run(() -> shooter.stopShooter(), shooter) + .alongWith(Commands.run(() -> System.out.println("Shooter stopped"))) + .alongWith( + Commands.run(() -> pivot.lowScore(0.31), pivot) + .alongWith( + Commands.run( + () -> + System.out.println( + "Pivot to intake position"))))))); // Default commands pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); @@ -217,40 +226,44 @@ private void configureBindings() { // Specialized commands // auto align with inner cosmic converter and raise pivot - - // Intake - controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); - // auto align with outer cosmic converter + // auto align with outer cosmic converter and raise pivot controller .leftTrigger() .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); + // Default drive controller.rightTrigger().toggleOnFalse(pivot.defaults()); // Intake controller .a() .toggleOnTrue(Commands.run(() -> intake.intake()).alongWith(pivot.lowScore(0.31))); - // Outtake controller.b().toggleOnTrue(Commands.run(() -> intake.outtake())); // Low score - controller.x().toggleOnTrue(pivot.lowScore(0.0)); + controller + .x() + .toggleOnTrue( + pivot + .lowScore(0.0) + .until(controller.rightTrigger()) + .andThen(pivot.lowScore(0.0).alongWith(Commands.run(() -> intake.outtake())))); } public Command getAutonomousCommand() { return new PathPlannerAuto("take 3 high shoot then there"); - -// Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> System.out.println("Pivot going"))) -// .withTimeout(1) -// .andThen( -// Commands.run(() -> shooter.shoot(), shooter) -// .alongWith(Commands.run(() -> System.out.println("Shooter going"))) -// // Timeout applied to shooter command only -// .alongWith( -// Commands.run(() -> intake.intake(), intake) -// .alongWith(Commands.run(() -> System.out.println("Intake going"))) -// )).withDeadline(Commands.waitSeconds(5)); -} + + // Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> + // System.out.println("Pivot going"))) + // .withTimeout(1) + // .andThen( + // Commands.run(() -> shooter.shoot(), shooter) + // .alongWith(Commands.run(() -> System.out.println("Shooter going"))) + // // Timeout applied to shooter command only + // .alongWith( + // Commands.run(() -> intake.intake(), intake) + // .alongWith(Commands.run(() -> System.out.println("Intake going"))) + // )).withDeadline(Commands.waitSeconds(5)); + } public static void zeroPigeon() { Pigeon2 pigeon = new Pigeon2(CANMappings.PIGEON_CAN_ID); From 60a17a78afccd20e5d7a310e3808c810b27b4989 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 9 Dec 2025 11:01:22 -0500 Subject: [PATCH 38/42] comp changes --- simgui-ds.json | 5 + src/main/java/frc/robot/RobotContainer.java | 116 ++++++++++++++---- src/main/java/frc/robot/subsystems/Pivot.java | 20 ++- 3 files changed, 113 insertions(+), 28 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 49cf3aa..8d4d057 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -1,4 +1,9 @@ { + "System Joysticks": { + "window": { + "enabled": false + } + }, "keyboardJoysticks": [ { "axisConfig": [ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dfac0ff..77d02b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.epilogue.Logged; import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; @@ -52,6 +51,15 @@ public class RobotContainer { .withDriveRequestType( SwerveModule.DriveRequestType .OpenLoopVoltage); // Use open-loop control for drive motors + private final SwerveRequest.RobotCentric intakeDrive = + new SwerveRequest.RobotCentric() + .withDeadband(MaxSpeed * 0.2) + .withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.RobotCentric autoDrive = + new SwerveRequest.RobotCentric() + .withDriveRequestType(SwerveModule.DriveRequestType.OpenLoopVoltage); + private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt(); private final Telemetry logger = new Telemetry(MaxSpeed); @@ -226,43 +234,107 @@ private void configureBindings() { // Specialized commands // auto align with inner cosmic converter and raise pivot - controller.leftBumper().toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), true)); + controller + .leftBumper() + .toggleOnTrue( + pivot + .getCosmicConverter(controller.rightTrigger(), true) + .alongWith(Commands.run(() -> System.out.println("inner")))); // auto align with outer cosmic converter and raise pivot controller .leftTrigger() - .toggleOnTrue(pivot.getCosmicConverter(controller.rightTrigger(), false)); - // Default drive - controller.rightTrigger().toggleOnFalse(pivot.defaults()); + .toggleOnTrue( + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + .alongWith(Commands.run(() -> System.out.println("Pivot going"))) + .withTimeout(1) + .andThen( + Commands.run(() -> shooter.shoot(), shooter) + .alongWith(Commands.run(() -> System.out.println("Shooter going"))) + // Timeout applied to shooter command only + .alongWith( + Commands.run(() -> intake.intake(), intake) + .alongWith(Commands.run(() -> System.out.println("Intake going"))))) + .withDeadline(Commands.waitSeconds(5))); // Intake controller .a() - .toggleOnTrue(Commands.run(() -> intake.intake()).alongWith(pivot.lowScore(0.31))); + .toggleOnTrue( + Commands.run(() -> intake.intake()) + .alongWith(pivot.lowScore(0.31)) + .alongWith(Commands.run(() -> System.out.println("intake"))) + .alongWith( + Commands.runOnce(() -> drivetrain.removeDefaultCommand()) + .andThen( + Commands.run( + () -> + drivetrain.setDefaultCommand( + drivetrain.applyRequest( + () -> + intakeDrive + .withVelocityX( + -controller.getLeftY() + * MaxSpeed) // Drive forward with + // negative Y + // (forward) + .withVelocityY( + -controller.getLeftX() + * MaxSpeed) // Drive left with negative + // X + .withRotationalRate( + -controller.getRightX() + * MaxAngularRate))))))); // Drive + // counterclockwise with + // negative X) + // counterclockwise + // with negative + // X)))); // Outtake - controller.b().toggleOnTrue(Commands.run(() -> intake.outtake())); + controller + .b() + .toggleOnTrue( + Commands.run(() -> intake.outtake()) + .alongWith(Commands.run(() -> System.out.println("outtake")))); + controller.povRight().onTrue(Commands.runOnce(() -> zeroPigeon())); // Low score controller .x() .toggleOnTrue( pivot - .lowScore(0.0) + .lowScore(0.5) .until(controller.rightTrigger()) - .andThen(pivot.lowScore(0.0).alongWith(Commands.run(() -> intake.outtake())))); + .andThen( + pivot + .lowScore(0.05) + .alongWith(Commands.run(() -> intake.outtake())) + .alongWith(Commands.run(() -> System.out.println("low"))))); + controller.povDown().toggleOnTrue(Commands.run(() -> pivot.movePivot(0.05), pivot)); } public Command getAutonomousCommand() { - return new PathPlannerAuto("take 3 high shoot then there"); - - // Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> - // System.out.println("Pivot going"))) - // .withTimeout(1) - // .andThen( - // Commands.run(() -> shooter.shoot(), shooter) - // .alongWith(Commands.run(() -> System.out.println("Shooter going"))) - // // Timeout applied to shooter command only - // .alongWith( - // Commands.run(() -> intake.intake(), intake) - // .alongWith(Commands.run(() -> System.out.println("Intake going"))) - // )).withDeadline(Commands.waitSeconds(5)); + return (Commands.run(() -> pivot.setPivotAngleRot(0.1525), pivot)) + .withTimeout(2) + .andThen( + Commands.run(() -> shooter.shoot(), shooter) + .alongWith(Commands.run(() -> intake.intake(), intake))).withDeadline(Commands.waitSeconds(10)); + // .withDeadline(Commands.waitSeconds(10)); + // return Commands.waitSeconds(12).andThen( + // Commands.run(() -> drivetrain.setControl(autoDrive.withVelocityX(1))) + // .withTimeout(5) + // .andThen( + // Commands.run( + // () -> + // drivetrain.setControl( + // (drive + // .withVelocityX( + // -controller.getLeftY() + // * MaxSpeed) // Drive forward with negative Y + // // (forward) + // .withVelocityY( + // -controller.getLeftX() + // * MaxSpeed) // Drive left with negative X + // .withRotationalRate( + // -controller.getRightX() + // * MaxAngularRate)))))); // Drive)))); } public static void zeroPigeon() { diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java index abda2ba..273b456 100644 --- a/src/main/java/frc/robot/subsystems/Pivot.java +++ b/src/main/java/frc/robot/subsystems/Pivot.java @@ -1,6 +1,7 @@ package frc.robot.subsystems; import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.MotionMagicVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -125,6 +126,11 @@ public void stopPivot() { mPivotRight.stopMotor(); } + public void movePivot(double speed) { + mPivotLeft.setControl(new DutyCycleOut(speed)); + mPivotRight.setControl(new DutyCycleOut(-speed)); + } + public boolean pivotAtSetpoint() { return Math.abs(mPivotLeft.getClosedLoopError().getValueAsDouble()) <= PivotConfig.K_PIVOT_ANGLE_TOLERANCE; @@ -212,23 +218,25 @@ public Command getCosmicConverter(BooleanSupplier complete, boolean isInner) { .Pose .getTranslation() .getDistance(getCosmicConverterTranslation(false))) - - 0.01))) + + 0.05))) .withDeadline( Commands.waitUntil(complete) .andThen( Commands.parallel( - Commands.run((() -> intake.runKicker(0.2))) + Commands.run((() -> intake.runKicker(0.2)), intake) .withTimeout(0.25) .andThen( - Commands.run(() -> shooter.shoot()) + Commands.run(() -> shooter.shoot(), shooter) .withTimeout(1) .andThen( - Commands.run(() -> intake.intake()) - .alongWith(Commands.run(() -> shooter.shoot()))))) + Commands.run(() -> intake.intake(), intake) + .alongWith( + Commands.run( + () -> shooter.shoot(), shooter))))) .until(() -> !complete.getAsBoolean()))); } else { System.out.println("no alliance detected: likely causing many errors"); - return null; + return Commands.none(); } } From 25fc0826fffd5ed8499f5d51fa65601872d14d5e Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 9 Dec 2025 11:38:19 -0500 Subject: [PATCH 39/42] spotless --- src/main/java/frc/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77d02b5..03e2fb4 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -315,7 +315,8 @@ public Command getAutonomousCommand() { .withTimeout(2) .andThen( Commands.run(() -> shooter.shoot(), shooter) - .alongWith(Commands.run(() -> intake.intake(), intake))).withDeadline(Commands.waitSeconds(10)); + .alongWith(Commands.run(() -> intake.intake(), intake))) + .withDeadline(Commands.waitSeconds(10)); // .withDeadline(Commands.waitSeconds(10)); // return Commands.waitSeconds(12).andThen( // Commands.run(() -> drivetrain.setControl(autoDrive.withVelocityX(1))) From 51ec53d1e0b0654bf01fc9efae5218fa81a31fc0 Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Mon, 15 Dec 2025 14:49:01 -0500 Subject: [PATCH 40/42] added a bunch of deadlines --- .../autos/take 3 high shoot then there.auto | 54 ++++++------------- src/main/java/frc/robot/RobotContainer.java | 14 ++--- 2 files changed, 24 insertions(+), 44 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto index b1d798d..3b2cacc 100644 --- a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto +++ b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto @@ -5,26 +5,19 @@ "data": { "commands": [ { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "wait 5 seconds" - } - }, - { - "type": "named", - "data": { - "name": "high goal shoot" - } - } - ] + "name": "high goal shoot" } }, { - "type": "deadline", + "type": "named", + "data": { + "name": "idle" + } + }, + { + "type": "parallel", "data": { "commands": [ { @@ -42,6 +35,12 @@ ] } }, + { + "type": "named", + "data": { + "name": "idle" + } + }, { "type": "path", "data": { @@ -49,22 +48,9 @@ } }, { - "type": "deadline", + "type": "named", "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "wait 5 seconds" - } - }, - { - "type": "named", - "data": { - "name": "high goal shoot" - } - } - ] + "name": "high goal shoot" } }, { @@ -78,12 +64,6 @@ "data": { "pathName": "all the way out 1" } - }, - { - "type": "named", - "data": { - "name": null - } } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a0ec32a..f0f634e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -135,30 +135,30 @@ private void configureBindings() { NamedCommands.registerCommand( "idle", - Commands.runOnce(() -> intake.stopIntake(), intake) + Commands.runOnce(() -> intake.stopIntake(), intake).withDeadline(Commands.waitSeconds(5)) .alongWith( Commands.runOnce(() -> shooter.stopShooter(), shooter) - .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot)))); + .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot))).withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "high goal shoot", - Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).alongWith(Commands.run(() -> System.out.println("Pivot going"))) + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).withDeadline(Commands.waitSeconds(5)).alongWith(Commands.run(() -> System.out.println("Pivot going"))).withDeadline(Commands.waitSeconds(5)) .withTimeout(1) .andThen( Commands.run(() -> shooter.shoot(), shooter) - .alongWith(Commands.run(() -> System.out.println("Shooter going"))) + .alongWith(Commands.run(() -> System.out.println("Shooter going"))).withDeadline(Commands.waitSeconds(5)) // Timeout applied to shooter command only .alongWith( Commands.run(() -> intake.intake(), intake) .alongWith(Commands.run(() -> System.out.println("Intake going"))) - ))); + )).withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "intake", - Commands.run(() -> intake.intake(), intake).alongWith(Commands.run((() -> System.out.println("Intake intaking off ground"))) + Commands.run(() -> intake.intake(), intake).withDeadline(Commands.waitSeconds(5)).alongWith(Commands.run((() -> System.out.println("Intake intaking off ground"))) .alongWith( Commands.run(() -> shooter.stopShooter(), shooter).alongWith(Commands.run(() -> System.out.println("Shooter stopped"))) - .alongWith(Commands.run(() -> pivot.lowScore(0.31), pivot).alongWith(Commands.run(() -> System.out.println("Pivot to intake position"))))))); + .alongWith(Commands.run(() -> pivot.lowScore(0.31), pivot).alongWith(Commands.run(() -> System.out.println("Pivot to intake position")))))).withDeadline(Commands.waitSeconds(5))); // Default commands pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); From 8878d9ee15b9cef0ce64e9a324df1814c61a1810 Mon Sep 17 00:00:00 2001 From: Yaypixel Date: Mon, 15 Dec 2025 14:55:32 -0500 Subject: [PATCH 41/42] added a bunch of deadlines --- .../autos/take 3 high shoot then there.auto | 8 +------- src/main/java/frc/robot/RobotContainer.java | 12 ++++++------ 2 files changed, 7 insertions(+), 13 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto index 3b2cacc..a72f949 100644 --- a/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto +++ b/src/main/deploy/pathplanner/autos/take 3 high shoot then there.auto @@ -38,7 +38,7 @@ { "type": "named", "data": { - "name": "idle" + "name": "high goal shoot" } }, { @@ -47,12 +47,6 @@ "pathName": "high shoot flat 2" } }, - { - "type": "named", - "data": { - "name": "high goal shoot" - } - }, { "type": "named", "data": { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 03e2fb4..54203bc 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,14 +143,14 @@ private void configureBindings() { NamedCommands.registerCommand( "idle", - Commands.runOnce(() -> intake.stopIntake(), intake) + Commands.runOnce(() -> intake.stopIntake(), intake).withDeadline(Commands.waitSeconds(5)) .alongWith( Commands.runOnce(() -> shooter.stopShooter(), shooter) - .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot)))); + .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot))).withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "high goal shoot", - Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).withDeadline(Commands.waitSeconds(5)) .alongWith(Commands.run(() -> System.out.println("Pivot going"))) .withTimeout(1) .andThen( @@ -159,11 +159,11 @@ private void configureBindings() { // Timeout applied to shooter command only .alongWith( Commands.run(() -> intake.intake(), intake) - .alongWith(Commands.run(() -> System.out.println("Intake going")))))); + .alongWith(Commands.run(() -> System.out.println("Intake going"))))).withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "intake", - Commands.run(() -> intake.intake(), intake) + Commands.run(() -> intake.intake(), intake).withDeadline(Commands.waitSeconds(5)) .alongWith( Commands.run((() -> System.out.println("Intake intaking off ground"))) .alongWith( @@ -175,7 +175,7 @@ private void configureBindings() { Commands.run( () -> System.out.println( - "Pivot to intake position"))))))); + "Pivot to intake position")))))).withDeadline(Commands.waitSeconds(5))); // Default commands pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot)); From d13c3696cd3cffb50004f516b952ac9473bcc1a1 Mon Sep 17 00:00:00 2001 From: sophiep29 <186753525+sophiep29@users.noreply.github.com> Date: Tue, 6 Jan 2026 20:34:54 -0500 Subject: [PATCH 42/42] spotless --- src/main/java/frc/robot/RobotContainer.java | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 54203bc..0b4072b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -143,14 +143,17 @@ private void configureBindings() { NamedCommands.registerCommand( "idle", - Commands.runOnce(() -> intake.stopIntake(), intake).withDeadline(Commands.waitSeconds(5)) + Commands.runOnce(() -> intake.stopIntake(), intake) + .withDeadline(Commands.waitSeconds(5)) .alongWith( Commands.runOnce(() -> shooter.stopShooter(), shooter) - .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot))).withDeadline(Commands.waitSeconds(5))); + .alongWith(Commands.runOnce(() -> pivot.pivotDefault(), pivot))) + .withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "high goal shoot", - Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot).withDeadline(Commands.waitSeconds(5)) + Commands.run(() -> pivot.setPivotAngleRot(0.14), pivot) + .withDeadline(Commands.waitSeconds(5)) .alongWith(Commands.run(() -> System.out.println("Pivot going"))) .withTimeout(1) .andThen( @@ -159,11 +162,13 @@ private void configureBindings() { // Timeout applied to shooter command only .alongWith( Commands.run(() -> intake.intake(), intake) - .alongWith(Commands.run(() -> System.out.println("Intake going"))))).withDeadline(Commands.waitSeconds(5))); + .alongWith(Commands.run(() -> System.out.println("Intake going"))))) + .withDeadline(Commands.waitSeconds(5))); NamedCommands.registerCommand( "intake", - Commands.run(() -> intake.intake(), intake).withDeadline(Commands.waitSeconds(5)) + Commands.run(() -> intake.intake(), intake) + .withDeadline(Commands.waitSeconds(5)) .alongWith( Commands.run((() -> System.out.println("Intake intaking off ground"))) .alongWith( @@ -174,8 +179,8 @@ private void configureBindings() { .alongWith( Commands.run( () -> - System.out.println( - "Pivot to intake position")))))).withDeadline(Commands.waitSeconds(5))); + System.out.println("Pivot to intake position")))))) + .withDeadline(Commands.waitSeconds(5))); // Default commands pivot.setDefaultCommand(Commands.run(() -> pivot.pivotDefault(), pivot));