From d1f4e11cfb7ba494eabe33f10b3dee8ad0bda887 Mon Sep 17 00:00:00 2001 From: Antoine PerreaultE Date: Tue, 10 Mar 2026 18:52:29 -0400 Subject: [PATCH] drivetrain + autoBuilder/chooser + tournerAZero/180 --- src/main/java/frc/robot/RobotContainer.java | 20 ++- .../robot/commands/ModeAuto/TournerA180.java | 31 ++++- .../robot/commands/ModeAuto/TournerAZero.java | 29 ++++- .../subsystems/CommandSwerveDrivetrain.java | 123 +++++------------- 4 files changed, 101 insertions(+), 102 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 239a253..cd1656d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,9 +8,12 @@ import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; +import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.cameraserver.CameraServer; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -41,6 +44,7 @@ import frc.robot.subsystems.LimeLight3; import frc.robot.subsystems.Limelight3G; public class RobotContainer { + private final SendableChooser autoChooser; Balayeuse balayeuse = new Balayeuse(); Grimpeur grimpeur = new Grimpeur(); Lanceur lanceur = new Lanceur(); @@ -60,12 +64,11 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); - private final CommandXboxController joystick = new CommandXboxController(0); - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); public RobotContainer() { + autoChooser = AutoBuilder.buildAutoChooser(); CameraServer.startAutomaticCapture(); configureBindings(); NamedCommands.registerCommand("GrimperMur", new GrimperMur()); @@ -76,13 +79,20 @@ public class RobotContainer { NamedCommands.registerCommand("Limelighter", new Limelighter()); NamedCommands.registerCommand("DescendreBalayeuse", new DescendreBalyeuse(balayeuse)); NamedCommands.registerCommand("Aspirer", new AspirerAuto(balayeuse)); - NamedCommands.registerCommand("TournerAZero", new TournerAZero()); - NamedCommands.registerCommand("TournerA180", new TournerA180()); + NamedCommands.registerCommand("TournerAZero", new TournerAZero(drivetrain)); + NamedCommands.registerCommand("TournerA180", new TournerA180(drivetrain)); NamedCommands.registerCommand("MonterGrimpeur", new MonterGrimpeur(grimpeur)); NamedCommands.registerCommand("DescendreGrimpeur", new DescendreGrimpeur(grimpeur)); } private void configureBindings() { + drivetrain.setDefaultCommand( + drivetrain.applyRequest(() -> + drive.withVelocityY(MathUtil.applyDeadband(-manette.getLeftX()*Math.abs(-manette.getLeftX())*MaxSpeed, 0.05)) + .withVelocityX(MathUtil.applyDeadband(-manette.getLeftY()*Math.abs(-manette.getLeftY())*MaxSpeed, 0.05)) + .withRotationalRate(MathUtil.applyDeadband(-manette.getRightX()*Math.abs(-manette.getRightX())*MaxAngularRate, 0.05)) + ) + ); manette.a().whileTrue(new Lancer(lanceur,limeLight3G,balayeuse,led)); manette.y().whileTrue(new ModeOposer(lanceur, balayeuse)); manette.x().whileTrue(new LancerBaseVitesse(lanceur, limeLight3, balayeuse, led)); @@ -94,6 +104,6 @@ public class RobotContainer { } public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + return autoChooser.getSelected(); } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java b/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java index 3471f10..b19d21f 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerA180.java @@ -4,12 +4,28 @@ package frc.robot.commands.ModeAuto; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import static edu.wpi.first.units.Units.*; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class TournerA180 extends Command { - /** Creates a new TournerA180. */ - public TournerA180() { + CommandSwerveDrivetrain drivetrain; + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Pigeon2 pigeon2; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); + /** Creates a new TournerAZero. */ + public TournerA180(CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -19,7 +35,14 @@ public class TournerA180 extends Command { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ + drivetrain.setControl(drive.withRotationalRate(-0.5)); + } + else if(pigeon2.getYaw().getValueAsDouble() >180){ + drivetrain.setControl(drive.withRotationalRate(0.5)); + } + } // Called once the command ends or is interrupted. @Override @@ -28,6 +51,6 @@ public class TournerA180 extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return pigeon2.getYaw().getValueAsDouble() > 165 && pigeon2.getYaw().getValueAsDouble() < 195; } } diff --git a/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java b/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java index 6b2886c..9e45d08 100644 --- a/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java +++ b/src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java @@ -4,12 +4,28 @@ package frc.robot.commands.ModeAuto; +import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; +import com.ctre.phoenix6.hardware.Pigeon2; +import com.ctre.phoenix6.swerve.SwerveRequest; + import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.generated.TunerConstants; +import frc.robot.subsystems.CommandSwerveDrivetrain; +import static edu.wpi.first.units.Units.*; /* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ public class TournerAZero extends Command { + CommandSwerveDrivetrain drivetrain; + private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); + private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond); + Pigeon2 pigeon2; + private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric() + .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) + .withDriveRequestType(DriveRequestType.OpenLoopVoltage); /** Creates a new TournerAZero. */ - public TournerAZero() { + public TournerAZero(CommandSwerveDrivetrain drivetrain) { + this.drivetrain = drivetrain; + addRequirements(drivetrain); // Use addRequirements() here to declare subsystem dependencies. } @@ -19,7 +35,14 @@ public class TournerAZero extends Command { // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if(pigeon2.getYaw().getValueAsDouble() >0 && pigeon2.getYaw().getValueAsDouble() <180){ + drivetrain.setControl(drive.withRotationalRate(0.5)); + } + else if(pigeon2.getYaw().getValueAsDouble() >180){ + drivetrain.setControl(drive.withRotationalRate(-0.5)); + } + } // Called once the command ends or is interrupted. @Override @@ -28,6 +51,6 @@ public class TournerAZero extends Command { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return pigeon2.getYaw().getValueAsDouble() > 345 || pigeon2.getYaw().getValueAsDouble() < 15; } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 5aa885f..ed7478d 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -10,10 +10,15 @@ import com.ctre.phoenix6.Utils; 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; @@ -44,74 +49,31 @@ 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; - - /* 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 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 -> { - /* 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 - ) - ); - - /* The SysId routine to test */ - private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation; - + private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds(); + private void configureAutoBuilder() { + try { + RobotConfig config = RobotConfig.fromGUISettings(); + AutoBuilder.configure( + () -> getState().Pose, + this::resetPose, + () -> getState().Speeds, + (speeds, feedforwards) -> setControl( + m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020)) + .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons()) + .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons()) + ), + new PPHolonomicDriveController( + new PIDConstants(10, 0, 0), + new PIDConstants(7, 0, 0) + ), + config, + () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red, + this + ); + } catch (Exception ex) { + DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace()); + } + } /** * Constructs a CTRE SwerveDrivetrain using the specified constants. *

@@ -130,6 +92,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); } /** @@ -154,6 +117,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); } /** @@ -186,6 +150,7 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su if (Utils.isSimulation()) { startSimThread(); } + configureAutoBuilder(); } /** @@ -198,28 +163,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su return run(() -> this.setControl(request.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 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() { /*