Feature/feb7 changes

build.gradle

@@ -121,3 +121,5 @@ wpi.java.configureTestTasks(test)
 tasks.withType(JavaCompile) {
     options.compilerArgs.add '-XDstringConcat=inline'
 }
+
+compileJava.dependsOn 'spotlessApply'

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

@@ -5,7 +5,6 @@
 package frc.robot;
 
 import com.ctre.phoenix6.SignalLogger;
-
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;

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

@@ -4,12 +4,9 @@
 
 package frc.robot;
 
-import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.subsystems.drive.DrivetrainSubsystem;
 import frc.robot.subsystems.vision.VisionSubsystem;
@@ -28,7 +25,10 @@ public class RobotContainer {
   // The controllers are defined here
   private static final CommandXboxController joystick = new CommandXboxController(0);
 
-  //private static JoystickButton driver_x = new JoystickButton(joystick, XboxController.Button.kX.value);
+  private final Telemetry logger = new Telemetry(DriveConstants.MAX_DRIVE_SPEED);
+
+  // private static JoystickButton driver_x = new JoystickButton(joystick,
+  // XboxController.Button.kX.value);
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
@@ -47,21 +47,35 @@ public RobotContainer() {
    */
   private void configureBindings() {
     joystick.x().onTrue(drivetrain.sysIdSteer());
-    joystick.y().whileTrue(drivetrain.driveForward());
+    joystick.y().onTrue(drivetrain.sysIdTranslation());
+    joystick.a().onTrue(drivetrain.driveForward());
+    joystick.leftBumper().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
+    drivetrain.registerTelemetry(logger::telemeterize);
   }
 
-  //Subsystem Default Commands
-  private void configureSubsystemDefaultCommands(){
+  // Subsystem Default Commands
+  private void configureSubsystemDefaultCommands() {
 
     drivetrain.setDefaultCommand(
-      // Drivetrain will execute this command periodically
-      drivetrain.applyRequest(() ->
-        DriveConstants.DEFAULT_DRIVE_REQUEST.withVelocityX(-1 * Math.copySign(Math.pow(joystick.getLeftY(),2), joystick.getLeftY()) * DriveConstants.MAX_DRIVE_SPEED) // Drive forward with negative Y (forward)
-          .withVelocityY(-1 * Math.copySign(Math.pow(joystick.getLeftX(), 2), joystick.getLeftX()) * DriveConstants.MAX_DRIVE_SPEED) // Drive left with negative X (left)
-          .withRotationalRate(-1 * Math.copySign(Math.pow(joystick.getRightX(), 2), joystick.getRightX()) * DriveConstants.MAX_ANGULAR_SPEED) // Drive counterclockwise with negative X (left)
-        )
-    );
-
+        // Drivetrain will execute this command periodically
+        drivetrain.applyRequest(
+            () ->
+                DriveConstants.DEFAULT_DRIVE_REQUEST
+                    .withVelocityX(
+                        -1
+                            * Math.copySign(Math.pow(joystick.getLeftY(), 2), joystick.getLeftY())
+                            * DriveConstants
+                                .MAX_DRIVE_SPEED) // Drive forward with negative Y (forward)
+                    .withVelocityY(
+                        -1
+                            * Math.copySign(Math.pow(joystick.getLeftX(), 2), joystick.getLeftX())
+                            * DriveConstants.MAX_DRIVE_SPEED) // Drive left with negative X (left)
+                    .withRotationalRate(
+                        -1
+                            * Math.copySign(Math.pow(joystick.getRightX(), 2), joystick.getRightX())
+                            * DriveConstants
+                                .MAX_ANGULAR_SPEED) // Drive counterclockwise with negative X (left)
+            ));
   }
 
   /**

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

@@ -0,0 +1,142 @@
+package frc.robot;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Telemetry {
+  private final double MaxSpeed;
+
+  /**
+   * Construct a telemetry object, with the specified max speed of the robot
+   *
+   * @param maxSpeed Maximum speed in meters per second
+   */
+  public Telemetry(double maxSpeed) {
+    MaxSpeed = maxSpeed;
+    SignalLogger.start();
+
+    /* Set up the module state Mechanism2d telemetry */
+    for (int i = 0; i < 4; ++i) {
+      SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
+    }
+  }
+
+  /* What to publish over networktables for telemetry */
+  private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+  /* Robot swerve drive state */
+  private final NetworkTable driveStateTable = inst.getTable("DriveState");
+  private final StructPublisher<Pose2d> drivePose =
+      driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
+  private final StructPublisher<ChassisSpeeds> driveSpeeds =
+      driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
+  private final StructArrayPublisher<SwerveModuleState> driveModuleStates =
+      driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
+  private final StructArrayPublisher<SwerveModuleState> driveModuleTargets =
+      driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
+  private final StructArrayPublisher<SwerveModulePosition> driveModulePositions =
+      driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
+  private final DoublePublisher driveTimestamp =
+      driveStateTable.getDoubleTopic("Timestamp").publish();
+  private final DoublePublisher driveOdometryFrequency =
+      driveStateTable.getDoubleTopic("OdometryFrequency").publish();
+
+  /* Robot pose for field positioning */
+  private final NetworkTable table = inst.getTable("Pose");
+  private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+  private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+
+  /* Mechanisms to represent the swerve module states */
+  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 */
+  private final MechanismLigament2d[] m_moduleSpeeds =
+      new MechanismLigament2d[] {
+        m_moduleMechanisms[0]
+            .getRoot("RootSpeed", 0.5, 0.5)
+            .append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[1]
+            .getRoot("RootSpeed", 0.5, 0.5)
+            .append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[2]
+            .getRoot("RootSpeed", 0.5, 0.5)
+            .append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[3]
+            .getRoot("RootSpeed", 0.5, 0.5)
+            .append(new MechanismLigament2d("Speed", 0.5, 0)),
+      };
+  /* A direction changing and length constant ligament for module direction */
+  private final MechanismLigament2d[] m_moduleDirections =
+      new MechanismLigament2d[] {
+        m_moduleMechanisms[0]
+            .getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[1]
+            .getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[2]
+            .getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[3]
+            .getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+      };
+
+  private final double[] m_poseArray = new double[3];
+
+  /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
+  public void telemeterize(SwerveDriveState state) {
+    /* Telemeterize the swerve drive state */
+    drivePose.set(state.Pose);
+    driveSpeeds.set(state.Speeds);
+    driveModuleStates.set(state.ModuleStates);
+    driveModuleTargets.set(state.ModuleTargets);
+    driveModulePositions.set(state.ModulePositions);
+    driveTimestamp.set(state.Timestamp);
+    driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
+
+    /* Also write to log file */
+    SignalLogger.writeStruct("DriveState/Pose", Pose2d.struct, state.Pose);
+    SignalLogger.writeStruct("DriveState/Speeds", ChassisSpeeds.struct, state.Speeds);
+    SignalLogger.writeStructArray(
+        "DriveState/ModuleStates", SwerveModuleState.struct, state.ModuleStates);
+    SignalLogger.writeStructArray(
+        "DriveState/ModuleTargets", SwerveModuleState.struct, state.ModuleTargets);
+    SignalLogger.writeStructArray(
+        "DriveState/ModulePositions", SwerveModulePosition.struct, state.ModulePositions);
+    SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
+
+    /* Telemeterize the pose to a Field2d */
+    fieldTypePub.set("Field2d");
+
+    m_poseArray[0] = state.Pose.getX();
+    m_poseArray[1] = state.Pose.getY();
+    m_poseArray[2] = state.Pose.getRotation().getDegrees();
+    fieldPub.set(m_poseArray);
+
+    /* Telemeterize each module state to a Mechanism2d */
+    for (int i = 0; i < 4; ++i) {
+      m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
+      m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
+      m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
+    }
+  }
+}

src/main/java/frc/robot/commands/DriveToPose.java

@@ -1,22 +1,22 @@
 package frc.robot.commands;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.drive.DriveConstants;
 import frc.robot.subsystems.drive.DrivetrainSubsystem;
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.geometry.Pose2d;
 
 public class DriveToPose extends Command {
   private DrivetrainSubsystem drivetrain;
   private Pose2d targetPose;
   private PIDController xControl;
   private PIDController yControl;
   private PIDController rotControl;
- 
+
   public DriveToPose(DrivetrainSubsystem subsystem, Pose2d pose) {
-      drivetrain = subsystem;
-      targetPose = pose;
-      addRequirements(drivetrain);
+    drivetrain = subsystem;
+    targetPose = pose;
+    addRequirements(drivetrain);
   }
 
   @Override
@@ -29,16 +29,21 @@ public void initialize() {
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    drivetrain.setControl(DriveConstants.AUTO_DRIVE_REQUEST
-      .withVelocityX(xControl.calculate(drivetrain.getFieldPose().getX(), targetPose.getX()))
-      .withVelocityY(yControl.calculate(drivetrain.getFieldPose().getY(), targetPose.getY()))
-      .withRotationalRate(rotControl.calculate(drivetrain.getFieldPose().getRotation().getDegrees(), targetPose.getRotation().getDegrees())));
+    drivetrain.setControl(
+        DriveConstants.AUTO_DRIVE_REQUEST
+            .withVelocityX(xControl.calculate(drivetrain.getFieldPose().getX(), targetPose.getX()))
+            .withVelocityY(yControl.calculate(drivetrain.getFieldPose().getY(), targetPose.getY()))
+            .withRotationalRate(
+                rotControl.calculate(
+                    drivetrain.getFieldPose().getRotation().getDegrees(),
+                    targetPose.getRotation().getDegrees())));
   }
 
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    drivetrain.setControl(DriveConstants.AUTO_DRIVE_REQUEST.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
+    drivetrain.setControl(
+        DriveConstants.AUTO_DRIVE_REQUEST.withVelocityX(0).withVelocityY(0).withRotationalRate(0));
   }
 
   // Returns true when the command should end.

src/main/java/frc/robot/generated/TunerConstants.java

@@ -5,7 +5,6 @@
 import com.ctre.phoenix6.CANBus;
 import com.ctre.phoenix6.configs.*;
 import com.ctre.phoenix6.hardware.*;
-import com.ctre.phoenix6.signals.*;
 import com.ctre.phoenix6.swerve.*;
 import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
 import edu.wpi.first.math.Matrix;
@@ -97,8 +96,8 @@ public class TunerConstants {
               .withSteerMotorGearRatio(kSteerGearRatio)
               .withCouplingGearRatio(kCoupleRatio)
               .withWheelRadius(kWheelRadius)
-              //.withSteerMotorGains(steerGains)
-              //.withDriveMotorGains(driveGains)
+              // .withSteerMotorGains(steerGains)
+              // .withDriveMotorGains(driveGains)
               .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
               .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
               .withSlipCurrent(kSlipCurrent)

src/main/java/frc/robot/statemachines/DriveState.java

@@ -22,7 +22,8 @@ private DriveState() {
     concurrentQueueMap.put(
         CameraConstants.photonCameraName_FrontLeft, new ConcurrentLinkedQueue<VisionMeasurement>());
     concurrentQueueMap.put(
-        CameraConstants.photonCameraName_FrontRight, new ConcurrentLinkedQueue<VisionMeasurement>());
+        CameraConstants.photonCameraName_FrontRight,
+        new ConcurrentLinkedQueue<VisionMeasurement>());
   }
 
   public static synchronized DriveState getInstance() {
@@ -68,4 +69,4 @@ public boolean hasDriveStats() {
   public SwerveDriveState getCurrentDriveStats() {
     return currentDriveStats;
   }
-}
+}

src/main/java/frc/robot/subsystems/Vision/camconfig.java

@@ -1,3 +1 @@
-public class camconfig {
-
-}
+public class camconfig {}

src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -2,30 +2,30 @@
 
 import static edu.wpi.first.units.Units.*;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import com.ctre.phoenix6.swerve.SwerveRequest.FieldCentric;
 import com.ctre.phoenix6.swerve.SwerveRequest.ForwardPerspectiveValue;
-import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
-import com.ctre.phoenix6.swerve.SwerveModule.SteerRequestType;
-
 import frc.robot.generated.TunerConstants;
 
 public class DriveConstants {
-    public static final double MAX_DRIVE_SPEED = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
-    public static final double MAX_ANGULAR_SPEED = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
-    public static final double DEADBAND_FACTOR = 0.1;
+  public static final double MAX_DRIVE_SPEED = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  public static final double MAX_ANGULAR_SPEED = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+  public static final double DEADBAND_FACTOR = 0.1;
 
-    public static final SwerveRequest.FieldCentric DEFAULT_DRIVE_REQUEST = new FieldCentric()
-        .withDeadband(MAX_DRIVE_SPEED * DEADBAND_FACTOR)
-        .withRotationalDeadband(MAX_ANGULAR_SPEED * DEADBAND_FACTOR)
-        .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective);
+  public static final SwerveRequest.FieldCentric DEFAULT_DRIVE_REQUEST =
+      new FieldCentric()
+          .withDeadband(MAX_DRIVE_SPEED * DEADBAND_FACTOR)
+          .withRotationalDeadband(MAX_ANGULAR_SPEED * DEADBAND_FACTOR)
+          .withForwardPerspective(ForwardPerspectiveValue.OperatorPerspective);
 
-    public static final SwerveRequest.FieldCentric AUTO_DRIVE_REQUEST = new FieldCentric()
-        .withDriveRequestType(DriveRequestType.Velocity) //Closed Loop Translation
-        .withSteerRequestType(SteerRequestType.Position) //Closed Loop Steer (Default)
-        .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); //Prevents "flips"
+  public static final SwerveRequest.FieldCentric AUTO_DRIVE_REQUEST =
+      new FieldCentric()
+          .withDriveRequestType(DriveRequestType.Velocity) // Closed Loop Translation
+          .withSteerRequestType(SteerRequestType.Position) // Closed Loop Steer (Default)
+          .withForwardPerspective(ForwardPerspectiveValue.BlueAlliance); // Prevents "flips"
 
-    public static final double TRANSLATION_ALIGN_TOLERANCE = 0.3; //probably meters
-    public static final double ROTATION_ALIGN_TOLERANCE = 0.3; //probably radians?
-
+  public static final double TRANSLATION_ALIGN_TOLERANCE = 0.01; // meters
+  public static final double ROTATION_ALIGN_TOLERANCE = 1; // degrees
 }