drivetrain + autoBuilder/chooser + tournerAZero/180

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<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("TournerAZero", new TournerAZero(drivetrain));
-    NamedCommands.registerCommand("TournerA180", new TournerA180());
+    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();
   }
 }

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. */
+  CommandSwerveDrivetrain drivetrain;
-  public TournerA180() {
+  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;
   }
 }

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

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;
-
+    private final SwerveRequest.ApplyRobotSpeeds m_pathApplyRobotSpeeds = new SwerveRequest.ApplyRobotSpeeds();
-    /* Swerve requests to apply during SysId characterization */
+     private void configureAutoBuilder() {
-    private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
+        try {
-    private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
+            RobotConfig config = RobotConfig.fromGUISettings();
-    private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
+            AutoBuilder.configure(
-
+                () -> getState().Pose,  
-    /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
+                this::resetPose,         
-    private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
+                () -> getState().Speeds, 
-        new SysIdRoutine.Config(
+                (speeds, feedforwards) -> setControl(
-            null,        // Use default ramp rate (1 V/s)
+                    m_pathApplyRobotSpeeds.withSpeeds(ChassisSpeeds.discretize(speeds, 0.020))
-            Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
+                        .withWheelForceFeedforwardsX(feedforwards.robotRelativeForcesXNewtons())
-            null,        // Use default timeout (10 s)
+                        .withWheelForceFeedforwardsY(feedforwards.robotRelativeForcesYNewtons())
-            // Log state with SignalLogger class
+                ),
-            state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
+                new PPHolonomicDriveController(
-        ),
+                    new PIDConstants(10, 0, 0),
-        new SysIdRoutine.Mechanism(
+                    new PIDConstants(7, 0, 0)
-            output -> setControl(m_translationCharacterization.withVolts(output)),
+                ),
-            null,
+                config,
-            this
+                () -> DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red,
-        )
+                this 
-    );
+            );
-
+        } catch (Exception ex) {
-    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
+            DriverStation.reportError("Failed to load PathPlanner config and configure AutoBuilder", ex.getStackTrace());
-    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;
-
     /**
      * 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() {
         /*