Merge branch 'dcmp' of https://github.com/team4909/2024-Crescendo int…

…o dcmp
team4909 · Apr 9, 2024 · 69911b5 · 69911b5
2 parents fd485ca + 414d15a
commit 69911b5
Show file tree

Hide file tree

Showing 6 changed files with 27 additions and 25 deletions.
diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml
@@ -11,8 +11,10 @@ jobs:
     container: wpilib/roborio-cross-ubuntu:2024-22.04
     steps:
       - name: Checkout repository
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
       - name: Grant execute permission
         run: chmod +x gradlew
+      - name: Check formatting
+        run: ./gradlew spotlessCheck
       - name: Build robot code
         run: ./gradlew build
diff --git a/src/main/java/frc/robot/arm/ArmVisualizer.java b/src/main/java/frc/robot/arm/ArmVisualizer.java
@@ -46,7 +46,7 @@ public ArmVisualizer(String logKey, double ligamentWidth, Color color) {
   public void update(double elbowAngleRad, double wristAngleRad) {
     m_elbowLigament.setAngle(Units.radiansToDegrees(elbowAngleRad) - kShoulderAngleDegrees);
     m_wristLigament.setAngle(Units.radiansToDegrees(wristAngleRad - elbowAngleRad));
-    Logger.recordOutput("Mechanism2d/" + m_logKey, m_mechanism);
+    Logger.recordOutput("Arm/Mechanism2d/" + m_logKey, m_mechanism);
 
     m_elbowPose =
         new Pose3d(origin.getX(), 0.0, origin.getY(), new Rotation3d(0.0, -elbowAngleRad, 0.0));
@@ -55,7 +55,7 @@ public void update(double elbowAngleRad, double wristAngleRad) {
             new Transform3d(
                 new Translation3d(ArmConstants.kElbowLengthMeters, 0.0, 0.0),
                 new Rotation3d(0.0, -(wristAngleRad - elbowAngleRad), 0.0)));
-    Logger.recordOutput("Mechanism3d/" + m_logKey, m_elbowPose, m_wristPose);
+    Logger.recordOutput("Arm/Mechanism3d/" + m_logKey, m_elbowPose, m_wristPose);
   }
 
   public Pose3d getWristPose() {

diff --git a/src/main/java/frc/robot/drivetrain/DriveToPose.java b/src/main/java/frc/robot/drivetrain/DriveToPose.java
@@ -72,15 +72,17 @@ public void initialize() {
   @Override
   public void execute() {
     Pose2d currentPose = PoseEstimation.getInstance().getPose();
-    double currentDistance = currentPose.getTranslation().getDistance(m_goalPose.getTranslation());
-    double ffScaler = MathUtil.clamp((currentDistance - 0.2) / (0.8 - 0.2), 0.0, 1.0);
+    double distanceToGoalPose =
+        currentPose.getTranslation().getDistance(m_goalPose.getTranslation());
+    double ffScaler = MathUtil.clamp((distanceToGoalPose - 0.2) / (0.8 - 0.2), 0.0, 1.0);
     m_translationController.reset(
         m_lastSetpointTranslation.getDistance(m_goalPose.getTranslation()),
         m_translationController.getSetpoint().velocity);
     double driveVelocityScalar =
         m_translationController.getSetpoint().velocity * ffScaler
-            + m_translationController.calculate(currentDistance, 0.0);
-    if (currentDistance < m_translationController.getPositionTolerance()) driveVelocityScalar = 0.0;
+            + m_translationController.calculate(distanceToGoalPose, 0.0);
+    if (distanceToGoalPose < m_translationController.getPositionTolerance())
+      driveVelocityScalar = 0.0;
     m_lastSetpointTranslation =
         new Pose2d(
                 m_goalPose.getTranslation(),
@@ -95,9 +97,9 @@ public void execute() {
         m_thetaController.getSetpoint().velocity * ffScaler
             + m_thetaController.calculate(
                 currentPose.getRotation().getRadians(), m_goalPose.getRotation().getRadians());
-    double thetaErrorAbs =
+    double thetaErrorAbsolute =
         Math.abs(currentPose.getRotation().minus(m_goalPose.getRotation()).getRadians());
-    if (thetaErrorAbs < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0;
+    if (thetaErrorAbsolute < m_thetaController.getPositionTolerance()) thetaVelocity = 0.0;
 
     Translation2d driveVelocity =
         new Pose2d(
@@ -110,15 +112,11 @@ public void execute() {
         ChassisSpeeds.fromFieldRelativeSpeeds(
             driveVelocity.getX(), driveVelocity.getY(), thetaVelocity, currentPose.getRotation()));
 
-    Logger.recordOutput("DriveToPose/DistanceMeasured", currentDistance);
+    Logger.recordOutput("Drivetrain/DriveToPose/DistanceToGoalPose", distanceToGoalPose);
     Logger.recordOutput(
-        "DriveToPose/DistanceSetpoint", m_translationController.getSetpoint().position);
-    Logger.recordOutput("DriveToPose/ThetaMeasured", currentPose.getRotation().getRadians());
-    Logger.recordOutput("DriveToPose/ThetaSetpoint", m_thetaController.getSetpoint().position);
-    Logger.recordOutput(
-        "DriveToPose/Setpoint",
+        "Drivetrain/DriveToPose/Setpoint",
         new Pose2d(
             m_lastSetpointTranslation, new Rotation2d(m_thetaController.getSetpoint().position)));
-    Logger.recordOutput("DriveToPose/Goal", m_goalPose);
+    Logger.recordOutput("Drivetrain/DriveToPose/Goal", m_goalPose);
   }
 }
diff --git a/src/main/java/frc/robot/drivetrain/Drivetrain.java b/src/main/java/frc/robot/drivetrain/Drivetrain.java
@@ -57,7 +57,7 @@ public class Drivetrain extends SubsystemBase {
       new SwerveDriveKinematics(m_modulePositions);
   public static final double kDriveBaseRadius =
       Math.hypot(kTrackwidthMeters / 2.0, kWheelbaseMeters / 2.0);
-  private static final LoggedTunableNumber speakerRange =
+  private static final LoggedTunableNumber speakerRangeMeters =
       new LoggedTunableNumber("Drivetrain/InRangeRadius", 5.0);
   private final double kMaxLinearSpeedMetersPerSecond = Units.feetToMeters(16);
   private final double kMaxAngularSpeedRadPerSec = 2 * Math.PI;
@@ -145,8 +145,8 @@ public void periodic() {
     }
 
     if (DriverStation.isDisabled()) {
-      Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
-      Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
+      Logger.recordOutput("Drivetrain/SwerveStates/Setpoints", new SwerveModuleState[] {});
+      Logger.recordOutput("Drivetrain/SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
       for (Module module : m_modules) module.stop();
     }
 
@@ -201,8 +201,8 @@ public void runVelocity(ChassisSpeeds speeds) {
           m_modules[moduleIndex].setSetpoint(setpointStates[moduleIndex]);
     }
 
-    Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
-    Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates);
+    Logger.recordOutput("Drivetrain/SwerveStates/Setpoints", setpointStates);
+    Logger.recordOutput("Drivetrain/SwerveStates/SetpointsOptimized", optimizedSetpointStates);
   }
 
   public void runWheelRadiusCharacterization(double characterizationInput) {
@@ -313,12 +313,12 @@ private void configurePathing() {
         targetPose -> Logger.recordOutput("Drivetrain/TrajectorySetpoint", targetPose));
   }
 
-  @AutoLogOutput(key = "SwerveStates/StatesMeasured")
+  @AutoLogOutput(key = "Drivetrain/SwerveStates/StatesMeasured")
   private SwerveModuleState[] getModuleStates() {
     return Arrays.stream(m_modules).map(Module::getState).toArray(SwerveModuleState[]::new);
   }
 
-  @AutoLogOutput(key = "SwerveStates/PositionsMeasured")
+  @AutoLogOutput(key = "Drivetrain/SwerveStates/PositionsMeasured")
   private SwerveModulePosition[] getModulePositions() {
     return Arrays.stream(m_modules).map(Module::getPosition).toArray(SwerveModulePosition[]::new);
   }
@@ -344,6 +344,6 @@ public boolean atHeadingGoal() {
   @AutoLogOutput(key = "Drivetrain/InRangeOfGoal")
   public boolean inRange() {
     return PoseEstimation.getInstance().getAimingParameters().effectiveDistance()
-        <= speakerRange.get();
+        <= speakerRangeMeters.get();
   }
 }
diff --git a/src/main/java/frc/robot/lights/Lights.java b/src/main/java/frc/robot/lights/Lights.java
@@ -1,5 +1,6 @@
 package frc.robot.lights;
 
+import com.ctre.phoenix.ErrorCode;
 import com.ctre.phoenix.led.CANdle;
 import com.ctre.phoenix.led.CANdle.LEDStripType;
 import com.ctre.phoenix.led.CANdleConfiguration;
@@ -41,6 +42,7 @@ public void periodic() {
     Logger.recordOutput("CANdle/5VRailVoltage", m_ledController.get5VRailVoltage());
     Logger.recordOutput("CANdle/Current", m_ledController.getCurrent());
     Logger.recordOutput("CANdle/Temperature", m_ledController.getTemperature());
+    Logger.recordOutput("CANdle/Connected", m_ledController.getLastError() == ErrorCode.OK);
   }
 
   public Command idle() {

diff --git a/src/main/java/frc/robot/vision/Vision.java b/src/main/java/frc/robot/vision/Vision.java
@@ -144,7 +144,7 @@ public void periodic() {
     }
 
     Logger.recordOutput(
-        "Vision/AllEstimatedPoses",
+        "Vision/InvalidEstimatedPoses",
         m_allEstimatedPosesToLog.stream().map(applyPoseOffset).toArray(Pose3d[]::new));
     Logger.recordOutput(
         "Vision/ValidEstimatedPoses",