Port drive subsystem to command robot

src/main/java/frc/robot/Robot.java

@@ -4,8 +4,6 @@
 
 package frc.robot;
 
-import com.ctre.phoenix6.SignalLogger;
-
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
@@ -24,8 +22,6 @@ public void robotInit() {
     m_robotContainer = new RobotContainer();
 
     m_robotContainer.drivetrain.getDaqThread().setThreadPriority(99);
-
-    SignalLogger.start();
   }
 
   @Override

src/main/java/frc/robot/RobotContainer.java

@@ -4,79 +4,59 @@
 
 package frc.robot;
 
+import com.ctre.phoenix6.mechanisms.swerve.SwerveModule.DriveRequestType;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
-import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.drive.DriveConstants;
+import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.led.LEDSubsystem;
 
 public class RobotContainer {
-  final double MaxSpeed = 6; // 6 meters per second desired top speed
-  final double MaxAngularRate = Math.PI; // Half a rotation per second max angular velocity
+  private class Controller {
+    public static final CommandController driver = new CommandController(0);
+    public static final CommandController operator = new CommandController(1);
+  }
 
-  /* Setting up bindings for necessary control of the swerve drive platform */
-  CommandController driver = new CommandController(0); // Driver Controller
-  CommandController operator = new CommandController(1); // Operator Controller
-  CommandSwerveDrivetrain drivetrain = TunerConstants.DriveTrain; // My drivetrain
   private final LEDSubsystem ledSubsystem;
-  SwerveRequest.FieldCentric drive =
+
+  // Set up the base for the drive and drivetrain
+  final DriveSubsystem drivetrain = DriveConstants.DriveTrain;
+  private final SwerveRequest.FieldCentric drive =
       new SwerveRequest.FieldCentric()
-          .withIsOpenLoop(true)
-          .withDeadband(MaxSpeed * 0.1)
-          .withRotationalDeadband(MaxAngularRate * 0.1); // I want field-centric
-  // driving in open loop
-  SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
-  SwerveRequest.RobotCentric forwardStraight =
-      new SwerveRequest.RobotCentric().withIsOpenLoop(true);
-  SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+          .withDeadband(DriveConstants.kMaxAngularRate * 0.1)
+          .withRotationalDeadband(DriveConstants.kMaxAngularRate * 0.1)
+          .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
 
   /* Path follower */
-  // Command runAuto = drivetrain.getAutoPath("Tests");
-
-  Telemetry logger = new Telemetry(MaxSpeed);
+  private Command runAuto = drivetrain.getAutoPath("Tests");
 
-  Pose2d odomStart = new Pose2d(0, 0, new Rotation2d(0, 0));
+  private final Telemetry logger = new Telemetry(DriveConstants.kMaxSpeed);
 
   private void configureBindings() {
     ledSubsystem.setDefaultCommand(new InstantCommand(() -> ledSubsystem.periodic(), ledSubsystem));
+    drivetrain.registerTelemetry(logger::telemeterize);
 
-    drivetrain.setDefaultCommand( // Drivetrain will execute this command periodically
+    drivetrain.setDefaultCommand(
         drivetrain
             .applyRequest(
                 () ->
                     drive
-                        .withVelocityX(-driver.getLeftY() * MaxSpeed) // Drive forward with
+                        .withVelocityX(
+                            -Controller.driver.getLeftY()
+                                * DriveConstants.kMaxSpeed) // Drive forward with
                         // negative Y (forward)
                         .withVelocityY(
-                            -driver.getLeftX() * MaxSpeed) // Drive left with negative X (left)
+                            -Controller.driver.getLeftX()
+                                * DriveConstants.kMaxSpeed) // Drive left with negative X (left)
                         .withRotationalRate(
-                            -driver.getRightX()
-                                * MaxAngularRate) // Drive counterclockwise with negative X (left)
+                            -Controller.driver.getRightX()
+                                * DriveConstants
+                                    .kMaxAngularRate) // Drive counterclockwise with negative X
+                // (left)
                 )
             .ignoringDisable(true));
-
-    driver.greenButton().whileTrue(drivetrain.applyRequest(() -> brake));
-    driver
-        .yellowButton()
-        .whileTrue(
-            drivetrain.applyRequest(
-                () ->
-                    point.withModuleDirection(
-                        new Rotation2d(-driver.getLeftY(), -driver.getLeftX()))));
-
-    // if (Utils.isSimulation()) {
-    //   drivetrain.seedFieldRelative(new Pose2d(new Translation2d(), Rotation2d.fromDegrees(90)));
-    // }
-    drivetrain.registerTelemetry(logger::telemeterize);
-    driver.POVUp()
-        .whileTrue(
-            drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
-    driver.POVDown()
-        .whileTrue(
-            drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
   }
 
   public RobotContainer() {
@@ -85,11 +65,6 @@ public RobotContainer() {
   }
 
   public Command getAutonomousCommand() {
-    /* First put the drivetrain into auto run mode, then run the auto */
-    return new InstantCommand(() -> {});
-  }
-
-  public boolean seedPoseButtonDown() {
-    return driver.leftBumper().getAsBoolean();
+    return runAuto;
   }
 }

src/main/java/frc/robot/Telemetry.java

@@ -1,5 +1,6 @@
 package frc.robot;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.Utils;
 import com.ctre.phoenix6.mechanisms.swerve.SwerveDrivetrain.SwerveDriveState;
 
@@ -10,8 +11,6 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.networktables.StringPublisher;
-import edu.wpi.first.wpilibj.DataLogManager;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
 import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -20,8 +19,6 @@
 
 public class Telemetry {
   private final double MaxSpeed;
-  private int logEntry;
-  private int odomEntry;
 
   /**
    * Construct a telemetry object, with the specified max speed of the robot
@@ -30,36 +27,36 @@ public class Telemetry {
    */
   public Telemetry(double maxSpeed) {
     MaxSpeed = maxSpeed;
-    logEntry = DataLogManager.getLog().start("odometry", "double[]");
-    odomEntry = DataLogManager.getLog().start("odom period", "double");
+    SignalLogger.start();
   }
 
   /* What to publish over networktables for telemetry */
-  NetworkTableInstance inst = NetworkTableInstance.getDefault();
+  private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
 
   /* Robot pose for field positioning */
-  NetworkTable table = inst.getTable("Pose");
-  DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
-  StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+  private final NetworkTable table = inst.getTable("Pose");
+  private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+  private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
 
   /* Robot speeds for general checking */
-  NetworkTable driveStats = inst.getTable("Drive");
-  DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
-  DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
-  DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
-  DoublePublisher odomPeriod = driveStats.getDoubleTopic("Odometry Period").publish();
+  private final NetworkTable driveStats = inst.getTable("Drive");
+  private final DoublePublisher velocityX = driveStats.getDoubleTopic("Velocity X").publish();
+  private final DoublePublisher velocityY = driveStats.getDoubleTopic("Velocity Y").publish();
+  private final DoublePublisher speed = driveStats.getDoubleTopic("Speed").publish();
+  private final DoublePublisher odomFreq =
+      driveStats.getDoubleTopic("Odometry Frequency").publish();
 
   /* Keep a reference of the last pose to calculate the speeds */
-  Pose2d m_lastPose = new Pose2d();
-  double lastTime = Utils.getCurrentTimeSeconds();
+  private Pose2d m_lastPose = new Pose2d();
+  private double lastTime = Utils.getCurrentTimeSeconds();
 
   /* Mechanisms to represent the swerve module states */
-  Mechanism2d[] m_moduleMechanisms =
+  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 */
-  MechanismLigament2d[] m_moduleSpeeds =
+  private final MechanismLigament2d[] m_moduleSpeeds =
       new MechanismLigament2d[] {
         m_moduleMechanisms[0]
             .getRoot("RootSpeed", 0.5, 0.5)
@@ -75,7 +72,7 @@ public Telemetry(double maxSpeed) {
             .append(new MechanismLigament2d("Speed", 0.5, 0)),
       };
   /* A direction changing and length constant ligament for module direction */
-  MechanismLigament2d[] m_moduleDirections =
+  private final MechanismLigament2d[] m_moduleDirections =
       new MechanismLigament2d[] {
         m_moduleMechanisms[0]
             .getRoot("RootDirection", 0.5, 0.5)
@@ -110,7 +107,7 @@ public void telemeterize(SwerveDriveState state) {
     speed.set(velocities.getNorm());
     velocityX.set(velocities.getX());
     velocityY.set(velocities.getY());
-    odomPeriod.set(1.0 / state.OdometryPeriod);
+    odomFreq.set(1.0 / state.OdometryPeriod);
 
     /* Telemeterize the module's states */
     for (int i = 0; i < 4; ++i) {
@@ -121,12 +118,8 @@ public void telemeterize(SwerveDriveState state) {
       SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
     }
 
-    DataLogManager.getLog()
-        .appendDoubleArray(
-            logEntry,
-            new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()},
-            (long) (Timer.getFPGATimestamp() * 1000000));
-    DataLogManager.getLog()
-        .appendDouble(odomEntry, state.OdometryPeriod, (long) (Timer.getFPGATimestamp() * 1000000));
+    SignalLogger.writeDoubleArray(
+        "odometry", new double[] {pose.getX(), pose.getY(), pose.getRotation().getDegrees()});
+    SignalLogger.writeDouble("odom period", state.OdometryPeriod, "seconds");
   }
 }