logging setup

Author: NoahSimon8

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

@@ -17,12 +17,15 @@ public RobotContainer() {
     DriverStation.silenceJoystickConnectionWarning(true);
     DriverStation.removeRefreshedDataEventHandle(44000);
 
+    // data logs
     DataLogManager.start();
     DataLogManager.logNetworkTables(true);
     DriverStation.startDataLog(DataLogManager.getLog(), true);
 
+    // When to go into brownout protection
     RobotController.setBrownoutVoltage(7.0);
 
+    // robot setup
     JoystickIO.getButtonBindings();
     AutoUtils.initAuto();
   }

Template/src/main/java/frc/robot/commands/auto/AutoController.java

@@ -14,21 +14,8 @@ public class AutoController {
     private static PIDController yController = new PIDController(AutoConstants.kPDrive, 0, AutoConstants.kDDrive);
     private static PIDController turnController = new PIDController(AutoConstants.kPTurn, 0, AutoConstants.kDTurn);
 
-    private static ShuffleData<Double[]> setpointPositionLog = new ShuffleData<Double[]>(
-            "Auto",
-            "setpoint position",
-            new Double[] { 0.0, 0.0, 0.0 });
-    private static ShuffleData<Double[]> setpointVelocityLog = new ShuffleData<Double[]>(
-            "Auto",
-            "setpoint velocity",
-            new Double[] { 0.0, 0.0, 0.0 });
-    private static ShuffleData<Double[]> setpointAccelerationLog = new ShuffleData<Double[]>(
-            "Auto",
-            "setpoint acceleration",
-            new Double[] { 0.0, 0.0, 0.0 });
-
     public static void choreoController(Pose2d curPose, SwerveSample sample) {
-        logSetpoints(sample);
+        Robot.swerve.logSetpoints(sample);
         ChassisSpeeds speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
                 new ChassisSpeeds(
                         xController.calculate(curPose.getX(), sample.x) + sample.vx,
@@ -39,17 +26,4 @@ public static void choreoController(Pose2d curPose, SwerveSample sample) {
         Robot.swerve.setChassisSpeeds(speeds);
     }
 
-    private static void logSetpoints(SwerveSample sample) {
-        Double[] positions = new Double[] { sample.x, sample.y, sample.heading };
-        positions[2] = Units.radiansToDegrees(positions[2]);
-        setpointPositionLog.set(positions);
-
-        Double[] velocities = new Double[] { sample.vx, sample.vy, sample.omega };
-        velocities[2] = Units.radiansToDegrees(velocities[2]);
-        setpointVelocityLog.set(velocities);
-
-        Double[] accelerations = new Double[] { sample.ax, sample.ay, sample.alpha };
-        accelerations[2] = Units.radiansToDegrees(accelerations[2]);
-        setpointAccelerationLog.set(accelerations);
-    }
 }

Template/src/main/java/frc/robot/commands/auto/AutoUtils.java

@@ -3,9 +3,11 @@
 import choreo.Choreo;
 import choreo.auto.AutoChooser;
 import choreo.auto.AutoFactory;
+import choreo.auto.AutoTrajectory;
 import choreo.auto.AutoFactory.AutoBindings;
 import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.Robot;
 import frc.robot.utils.UtilityFunctions;
@@ -24,6 +26,23 @@ public static void initAuto() {
 
     }
 
+    public static AutoChooser getChooser() {
+        return chooser;
+    }
+
+    public static Command makeStartingTrajectoryCommand(AutoTrajectory trajectory) {
+        return Commands.runOnce(
+                () -> Robot.swerve.setOdometry(trajectory.getInitialPose().orElseGet(() -> Robot.swerve.getPose())))
+                .andThen(trajectory.cmd());
+    }
+
+    public static Command addResetLoggingCommand(Command cmd) {
+        return cmd.andThen(
+                Commands.runOnce(() -> Robot.swerve.logSetpoints(
+                        new SwerveSample(-100, 0, -100, 0, 0, 0, 0, 0,
+                                0, 0, new double[] { 0.0, 0.0, 0.0, 0.0 }, new double[] { 0.0, 0.0, 0.0, 0.0 }))));
+    }
+
     private static void setupFactory() {
         // what commands run on what markers
         AutoBindings bindings = new AutoFactory.AutoBindings();
@@ -50,10 +69,4 @@ private static void setupChooser() {
 
     }
 
-
-
-    public static AutoChooser getChooser() {
-        return chooser;
-    }
-
 }

Template/src/main/java/frc/robot/commands/auto/Autos.java

@@ -4,7 +4,9 @@
 import choreo.auto.AutoFactory;
 import choreo.auto.AutoLoop;
 import choreo.auto.AutoTrajectory;
+import choreo.trajectory.SwerveSample;
 import frc.robot.Robot;
+import frc.robot.subsystems.swerve.Swerve;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 
@@ -21,15 +23,14 @@ public static Command getMyRoutine(AutoFactory factory) {
         // instaniate our auto loop and trajectories
         AutoLoop loop = factory.newLoop("auto");
         AutoTrajectory trajectory = factory.trajectory("trajectoryName", loop);
-        
-        // create our trajectory commands, setting odometry if needed
-        Command trajectoryCommand = Commands.runOnce(
-                () -> Robot.swerve.setOdometry(trajectory.getInitialPose().orElseGet(() -> Robot.swerve.getPose())));
-        trajectoryCommand = trajectoryCommand.andThen(trajectory.cmd());
+
+        // create our trajectory commands, setting odometry and resetting logging when finished
+        Command trajectoryCommand = AutoUtils.addResetLoggingCommand(
+                AutoUtils.makeStartingTrajectoryCommand(trajectory));
 
         // set the command to begin when the loop enables
         loop.enabled().onTrue(trajectoryCommand);
-        
+
         // our final, total command
         Command cmd;
 

Template/src/main/java/frc/robot/subsystems/swerve/Swerve.java

@@ -4,6 +4,7 @@
 
 package frc.robot.subsystems.swerve;
 
+import choreo.trajectory.SwerveSample;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
@@ -44,6 +45,7 @@ public class Swerve extends SubsystemBase {
   // equivilant to a odometer, but also intakes vision
   private SwerveDrivePoseEstimator swerveDrivePoseEstimator;
 
+  // Logging
   private ShuffleData<Double[]> odometryLog = new ShuffleData<Double[]>(
       this.getName(),
       "odometry",
@@ -98,6 +100,19 @@ public class Swerve extends SubsystemBase {
       "heading",
       0.0);
 
+  private ShuffleData<Double[]> setpointPositionLog = new ShuffleData<Double[]>(
+      this.getName(),
+      "setpoint position",
+      new Double[] { 0.0, 0.0, 0.0 });
+  private ShuffleData<Double[]> setpointVelocityLog = new ShuffleData<Double[]>(
+      this.getName(),
+      "setpoint velocity",
+      new Double[] { 0.0, 0.0, 0.0 });
+  private ShuffleData<Double[]> setpointAccelerationLog = new ShuffleData<Double[]>(
+      this.getName(),
+      "setpoint acceleration",
+      new Double[] { 0.0, 0.0, 0.0 });
+
   public Swerve() {
 
     // if simulation
@@ -320,16 +335,22 @@ public void resetGyro() {
     }
   }
 
-  @Override
-  public void periodic() {
-    gyro.updateData(gyroData);
-    updateOdometry();
+  public void logSetpoints(SwerveSample sample) {
+    // setpoint logging for automated driving
+    Double[] positions = new Double[] { sample.x, sample.y, sample.heading };
+    positions[2] = Units.radiansToDegrees(positions[2]);
+    setpointPositionLog.set(positions);
 
-    // periodic method for individual modules
-    for (int i = 0; i < 4; i++) {
-      modules[i].periodic();
-    }
+    Double[] velocities = new Double[] { sample.vx, sample.vy, sample.omega };
+    velocities[2] = Units.radiansToDegrees(velocities[2]);
+    setpointVelocityLog.set(velocities);
 
+    Double[] accelerations = new Double[] { sample.ax, sample.ay, sample.alpha };
+    accelerations[2] = Units.radiansToDegrees(accelerations[2]);
+    setpointAccelerationLog.set(accelerations);
+  }
+
+  private void logData() {
     // logging of our module states
     Double[] realStates = {
         modules[0].getState().angle.getDegrees(),
@@ -378,6 +399,19 @@ public void periodic() {
     velocityLog.set(robotVelocity);
     accelerationLog.set((robotVelocity - prevVelocity) / .02);
     prevVelocity = robotVelocity;
+  }
+
+  @Override
+  public void periodic() {
+    gyro.updateData(gyroData);
+    updateOdometry();
+
+    // periodic method for individual modules
+    for (int i = 0; i < 4; i++) {
+      modules[i].periodic();
+    }
+
+    logData();
 
   }