arjun-MD.2-application

src/main/java/frc/robot/Constants.java

@@ -926,7 +926,7 @@ public static class ElevatorConstants {
     public static final double STATOR_CURRENT_LIMIT = 50.0; // TODO: change for actual match
     public static final double SUPPLY_CURRENT_LIMIT = 30.0; // TODO: change for actual match
 
-    public static double S0C_KP = 1.0; // 1.0 before (okay)
+    public static double S0C_KP = 1.04; // 1.0 before (okay)
     public static double S0C_KI = 0.0;
     public static double S0C_KD = 0.005;
 

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

@@ -73,7 +73,6 @@ public void robotPeriodic() {
     visionRight.addFilteredPose();
     visionLeft.addFilteredPose();
 
-
     DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel());
     DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide());
   }
@@ -173,8 +172,7 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {

src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java

@@ -1,7 +1,6 @@
 // TODO: THIS COMMAND NEED TO BE UPDATED TO WORK PROPERLY
 package frc.robot.commands.DaleCommands;
 
-import dev.doglog.DogLog;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants.ElevatorConstants.ElevatorPositions;
 import frc.robot.subsystems.ArmSubsystem;

src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java

@@ -1,7 +1,6 @@
 package frc.robot.commands.SwerveCommands;
 
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
-import dev.doglog.DogLog;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.SwerveSubsystem;
 

src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java

@@ -102,16 +102,26 @@ public void execute() {
     DogLog.log("Commands/JamesHarden/TargetPose", targetPose);
     DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseX(m)", targetPose.getX());
     DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseY(m)", targetPose.getY());
-    DogLog.log("Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)", targetPose.getRotation().getRadians());
+    DogLog.log(
+        "Commands/JamesHarden/AxisSpecificInformation/TargetPoseHeading(deg)",
+        targetPose.getRotation().getRadians());
 
-    DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond);
-    DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond);
-    DogLog.log("Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)", speeds.omegaRadiansPerSecond);
+    DogLog.log(
+        "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsX(mps)",
+        speeds.vxMetersPerSecond);
+    DogLog.log(
+        "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsY(mps)",
+        speeds.vyMetersPerSecond);
+    DogLog.log(
+        "Commands/JamesHarden/AxisSpecificInformation/DesiredChassisSpeedsTheta(radps)",
+        speeds.omegaRadiansPerSecond);
 
     DogLog.log(
-        "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond);
+        "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)",
+        swerve.getFieldSpeeds().vxMetersPerSecond);
     DogLog.log(
-        "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond);
+        "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)",
+        swerve.getFieldSpeeds().vyMetersPerSecond);
     DogLog.log(
         "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(radps)",
         swerve.getFieldSpeeds().omegaRadiansPerSecond);

src/main/java/frc/robot/subsystems/AnthonyVision.java

@@ -40,7 +40,7 @@ public class AnthonyVision extends SubsystemBase {
   // Data type is "Cameras", an enum defined in Constants.java with only two options (left, right)
   private final Constants.Vision.Cameras cameraId;
 
-  private String camTitle; 
+  private String camTitle;
   // Reef tag IDs for each side of the field
   private static final List<Integer> BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18);
   private static final List<Integer> RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11);
@@ -182,7 +182,7 @@ public void addFilteredPose() {
       DogLog.log("Vision/" + camTitle + "/ValidTags", false);
       return;
     }
-      DogLog.log("Vision/" + camTitle + "/ValidTags", true);
+    DogLog.log("Vision/" + camTitle + "/ValidTags", true);
 
     // Log all tags that haven't been thrown out
     int tagCount = validTags.size();
@@ -306,7 +306,8 @@ private double computeNoiseXY(
     double tagFactor = 1.0 / Math.sqrt(effectiveTags);
 
     // Distance term (keep as d^2)
-    double distanceFactor = baseNoise + distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance);
+    double distanceFactor =
+        baseNoise + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance);
 
     // Speed term (quadratic, saturated)
     double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed;
@@ -315,12 +316,11 @@ private double computeNoiseXY(
     DogLog.log("Vision/tagFactor", tagFactor);
     DogLog.log("Vision/distanceFactor", distanceFactor);
     DogLog.log("Vision/speedFactor", speedFactor);
-    
+
     double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor;
     return computedStdDevs;
   }
 
-
   private double computeNoiseHeading(
       double baseNoise,
       double distanceCoefficient,