sdesharnais/Rebuilt-2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

drivetrain + autoBuilder/chooser + tournerAZero/180

Browse Source
This commit is contained in:
Antoine PerreaultE
2026-03-10 18:52:29 -04:00
parent c0a2c67261
commit d1f4e11cfb
4 changed files with 101 additions and 102 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/main/java/frc/robot/RobotContainer.java
View File

@@ -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<Command> 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();
}
}

31
src/main/java/frc/robot/commands/ModeAuto/TournerA180.java
View File

@@ -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;
}
}

29
src/main/java/frc/robot/commands/ModeAuto/TournerAZero.java
View File

@@ -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;
}
}

115
src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
View File

@@ -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())
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 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 PPHolonomicDriveController(
new PIDConstants(10, 0, 0),
new PIDConstants(7, 0, 0)
),
new SysIdRoutine.Mechanism(
volts -> setControl(m_steerCharacterization.withVolts(volts)),
null,
config,
() -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
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;
} catch (Exception ex) {
DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
}
}
/**
* Constructs a CTRE SwerveDrivetrain using the specified constants.
* <p>
@@ -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() {
/*