Final code for Chezy

src/main/java/frc/robot/AutoRoutines/AutoProducer.java

@@ -93,7 +93,7 @@ private void settyCycle(
 
     JamesHardenMovement movementCommand, maintainCommand;
     if (!scorePosition.isBranch()) {
-      DogLog.log("JamesHardenScore/Errors", "called without real branch");
+      DogLog.log("Commands/JamesHardenScore/Errors", "Called without a real branch");
       return;
     }
     movementCommand = JamesHardenMovement.toSpecificBranch(driveTrain, () -> scorePosition, false);

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

@@ -14,7 +14,10 @@
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.numbers.N1;
 import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.math.util.Units;
@@ -43,6 +46,24 @@ public static class Kalman {
   }
 
   public static class HardenConstants {
+    public static final double QKP = 3.4;
+    public static final double QKI = 0.45;
+    public static final double QKD = 0.0005;
+    public static final double QCRUISE = 4.368;
+    public static final double QACCEL = 8.0;
+    public static final double QIZONE = 0.35;
+    public static final double QIRANGE_LOWER = 0.0;
+    public static final double QIRANGE_UPPER = 0.2;
+
+    public static final double HKP = 3.7;
+    public static final double HKI = 0.4;
+    public static final double HKD = 0.0;
+    public static final double HCRUISE = 9.417;
+    public static final double HACCEL = 10.971;
+    public static final double HIZONE = 0.14;
+    public static final double HIRANGE_LOWER = 0.0;
+    public static final double HIRANGE_UPPER = Math.PI / 4.0;
+
     public static class EndWhenCloseEnough {
       public static final double translationalToleranceTeleop = 0.8d; // 0.43105229381 worked before
       public static final double translationalToleranceAuto = 1d;
@@ -90,6 +111,33 @@ public String getLoggingName() {
     public static final double LEFT_CAM_TO_ROBOT_ROTATION_ROLL = 0;
     public static final double LEFT_CAM_TO_ROBOT_ROTATION_PITCH = Units.degreesToRadians(-12.5);
     public static final double LEFT_CAM_TO_ROBOT_ROTATION_YAW = Units.degreesToRadians(-40);
+
+    public static Transform3d getCameraTransform(Cameras cam) {
+      switch (cam) {
+        case RIGHT_CAM:
+          return new Transform3d(
+              new Translation3d(
+                  RIGHT_CAM_TO_ROBOT_TRANSLATION_X,
+                  RIGHT_CAM_TO_ROBOT_TRANSLATION_Y,
+                  RIGHT_CAM_TO_ROBOT_TRANSLATION_Z),
+              new Rotation3d(
+                  RIGHT_CAM_TO_ROBOT_ROTATION_ROLL,
+                  RIGHT_CAM_TO_ROBOT_ROTATION_PITCH,
+                  RIGHT_CAM_TO_ROBOT_ROTATION_YAW));
+        case LEFT_CAM:
+          return new Transform3d(
+              new Translation3d(
+                  LEFT_CAM_TO_ROBOT_TRANSLATION_X,
+                  LEFT_CAM_TO_ROBOT_TRANSLATION_Y,
+                  LEFT_CAM_TO_ROBOT_TRANSLATION_Z),
+              new Rotation3d(
+                  LEFT_CAM_TO_ROBOT_ROTATION_ROLL,
+                  LEFT_CAM_TO_ROBOT_ROTATION_PITCH,
+                  LEFT_CAM_TO_ROBOT_ROTATION_YAW));
+        default:
+          throw new IllegalArgumentException("Unknown camera: " + cam);
+      }
+    }
   }
 
   public static class AutoRoutines {
@@ -878,15 +926,15 @@ 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_KI = 0.0;
+    public static double S0C_KP = 1.04; // 1.0 before (okay)
+    public static double S0C_KI = 0.00;
     public static double S0C_KD = 0.005;
 
     public static double S1C_KP = 0.3501;
     public static double S1C_KI = 0.0;
     public static double S1C_KD = 0.0;
 
-    public static double S0C_KS = 0.0;
+    public static double S0C_KS = 0.00;
     public static double S0C_KG = 0.29 + 0.05;
     public static double S0C_KA = 0.0004657452997; // 0.04
     public static double S0C_KV = 0.124; // 10.66

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

@@ -10,10 +10,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.robot.commands.ElevatorCommands.ZeroElevatorHardStop;
+import frc.robot.subsystems.AnthonyVision;
 import frc.robot.subsystems.CoralPosition;
 import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.SwerveSubsystem;
-import frc.robot.subsystems.VisionSystem;
 import frc.robot.util.LoggedTalonFX;
 
 /**
@@ -34,8 +34,8 @@ public class Robot extends TimedRobot {
   // private VisionSystem visionLeft = VisionSystem.getInstance(Constants.Vision.Cameras.LEFT_CAM);
   private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance();
   private final RobotContainer m_robotContainer;
-  private VisionSystem visionRight;
-  private VisionSystem visionLeft;
+  private AnthonyVision visionRight;
+  private AnthonyVision visionLeft;
 
   // standard deviation for x (meters), y (meters) and rotation (radians) camera data
 
@@ -50,9 +50,10 @@ public Robot() {
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
     visionRight =
-        VisionSystem.getInstance(Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide());
+        AnthonyVision.getInstance(
+            Constants.Vision.Cameras.RIGHT_CAM, m_robotContainer.getRedSide());
     visionLeft =
-        VisionSystem.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide());
+        AnthonyVision.getInstance(Constants.Vision.Cameras.LEFT_CAM, m_robotContainer.getRedSide());
     absoluteInit();
   }
 
@@ -65,17 +66,13 @@ public Robot() {
    */
   @Override
   public void robotPeriodic() {
-    DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel());
-    DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide());
     LoggedTalonFX.periodic_static();
     CommandScheduler.getInstance().run();
     m_robotContainer.doTelemetry();
 
     visionRight.addFilteredPose();
     visionLeft.addFilteredPose();
 
-    DogLog.log("KalmanDebug/drivetrainPose", driveTrain.getPose());
-
     DogLog.log("CoralPosition/isCoralInFunnel", CoralPosition.isCoralInFunnel());
     DogLog.log("CoralPosition/isCoralInTootsieSlide", CoralPosition.isCoralInTootsieSlide());
   }
@@ -89,24 +86,44 @@ public void disabledInit() {
   @Override
   public void robotInit() {
     DogLog.setOptions(
-        new DogLogOptions().withNtPublish(false).withCaptureDs(true).withLogExtras(true));
-    DogLog.log("PIDArmKP", Constants.Arm.S0C_KP);
-    DogLog.log("PIDArmKI", Constants.Arm.S0C_KI);
-    DogLog.log("PIDArmKD", Constants.Arm.S0C_KD);
-    DogLog.log("PIDArmKS", Constants.Arm.S0C_KS);
-    DogLog.log("PIDArmKG", Constants.Arm.S0C_KG);
-
-    DogLog.log("PIDElevatorKP", Constants.ElevatorConstants.S0C_KP);
-    DogLog.log("PIDElevatorKI", Constants.ElevatorConstants.S0C_KI);
-    DogLog.log("PIDElevatorKD", Constants.ElevatorConstants.S0C_KD);
-    DogLog.log("PIDElevatorKS", Constants.ElevatorConstants.S0C_KS);
-    DogLog.log("PIDElevatorKG", Constants.ElevatorConstants.S0C_KG);
-
-    DogLog.log("PIDTootsieKP", Constants.TootsieSlide.S0C_KP);
-    DogLog.log("PIDTootsieKI", Constants.TootsieSlide.S0C_KI);
-    DogLog.log("PIDTootsieKD", Constants.TootsieSlide.S0C_KD);
-    DogLog.log("PIDTootsieKS", Constants.TootsieSlide.S0C_KS);
-    DogLog.log("PIDTootsieKG", Constants.TootsieSlide.S0C_KG);
+        new DogLogOptions().withNtPublish(true).withCaptureDs(true).withLogExtras(true));
+    DogLog.log("PIDValues/ArmKP", Constants.Arm.S0C_KP);
+    DogLog.log("PIDValues/ArmKI", Constants.Arm.S0C_KI);
+    DogLog.log("PIDValues/ArmKD", Constants.Arm.S0C_KD);
+    DogLog.log("PIDValues/ArmKS", Constants.Arm.S0C_KS);
+    DogLog.log("PIDValues/ArmKG", Constants.Arm.S0C_KG);
+
+    DogLog.log("PIDValues/ElevatorKP", Constants.ElevatorConstants.S0C_KP);
+    DogLog.log("PIDValues/ElevatorKI", Constants.ElevatorConstants.S0C_KI);
+    DogLog.log("PIDValues/ElevatorKD", Constants.ElevatorConstants.S0C_KD);
+    DogLog.log("PIDValues/ElevatorKS", Constants.ElevatorConstants.S0C_KS);
+    DogLog.log("PIDValues/ElevatorKG", Constants.ElevatorConstants.S0C_KG);
+
+    DogLog.log("PIDValues/TootsieKP", Constants.TootsieSlide.S0C_KP);
+    DogLog.log("PIDValues/TootsieKI", Constants.TootsieSlide.S0C_KI);
+    DogLog.log("PIDValues/TootsieKD", Constants.TootsieSlide.S0C_KD);
+    DogLog.log("PIDValues/TootsieKS", Constants.TootsieSlide.S0C_KS);
+    DogLog.log("PIDValues/TootsieKG", Constants.TootsieSlide.S0C_KG);
+
+    // Q set
+    DogLog.log("PIDValues/QKP", Constants.HardenConstants.QKP);
+    DogLog.log("PIDValues/QKI", Constants.HardenConstants.QKI);
+    DogLog.log("PIDValues/QKD", Constants.HardenConstants.QKD);
+    DogLog.log("PIDValues/QCRUISE", Constants.HardenConstants.QCRUISE);
+    DogLog.log("PIDValues/QACCEL", Constants.HardenConstants.QACCEL);
+    DogLog.log("PIDValues/QIZONE", Constants.HardenConstants.QIZONE);
+    DogLog.log("PIDValues/QIRANGE_LOWER", Constants.HardenConstants.QIRANGE_LOWER);
+    DogLog.log("PIDValues/QIRANGE_UPPER", Constants.HardenConstants.QIRANGE_UPPER);
+
+    // H set
+    DogLog.log("PIDValues/HKP", Constants.HardenConstants.HKP);
+    DogLog.log("PIDValues/HKI", Constants.HardenConstants.HKI);
+    DogLog.log("PIDValues/HKD", Constants.HardenConstants.HKD);
+    DogLog.log("PIDValues/HCRUISE", Constants.HardenConstants.HCRUISE);
+    DogLog.log("PIDValues/HACCEL", Constants.HardenConstants.HACCEL);
+    DogLog.log("PIDValues/HIZONE", Constants.HardenConstants.HIZONE);
+    DogLog.log("PIDValues/HIRANGE_LOWER", Constants.HardenConstants.HIRANGE_LOWER);
+    DogLog.log("PIDValues/HIRANGE_UPPER", Constants.HardenConstants.HIRANGE_UPPER);
     // Commented this code that logs the electric data because it crashed the robot code
     // there is an error related to the usage of this
     // DogLog.setPdh(new PowerDistribution());
@@ -155,10 +172,7 @@ public void teleopInit() {
 
   /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-    // DogLog.log("PID Constant", IncreasePArm.broomIndex());
-    // DogLog.log("Mechanism Type", IncreasePArm.mechIndex());
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {

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

@@ -83,7 +83,6 @@ public void doTelemetry() {
     if (driveTrain.getCurrentCommand() != null) {
       commandName = driveTrain.getCurrentCommand().getName();
     }
-    DogLog.log("Robot/SwerveDriveCommand", commandName);
   }
 
   public RobotContainer() {
@@ -471,15 +470,15 @@ public static void setAlliance() {
   }
 
   public BooleanSupplier getRedSide() {
-    DogLog.log("get alliance", redside.getAsBoolean());
+    DogLog.log("Info/Alliance", redside.getAsBoolean());
     return redside;
   }
 
   public Command getAutonomousCommand() {
     /* Run the path selected from the auto chooser */
     int autoValue = autoChooser.getSelected();
     Command autoCommand;
-    DogLog.log("auto/selected", autoValue);
+    DogLog.log("Info/AutoSelected", autoValue);
     switch (autoValue) {
       case 1:
         autoCommand =

src/main/java/frc/robot/commandGroups/JamesHardenScore.java

@@ -72,7 +72,7 @@ public JamesHardenScore(
 
     JamesHardenMovement movementCommand, maintainCommand;
     if (!branch.isBranch()) {
-      DogLog.log("JamesHardenScore/Errors", "called without real branch");
+      DogLog.log("Commands/JamesHardenScore/Errors", "Called without a real branch");
       return;
     }
 
@@ -85,39 +85,12 @@ public JamesHardenScore(
     } else {
       elevateCommand = new SetElevatorLevel(elevatorSubsystem, height, true);
     }
-    // if (DriverStation.isTeleop()) {
-    DogLog.log("JamesHardenScore/Version", "teleop");
 
     addCommands(
         movementCommand.alongWith(
             (new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d()))
                 .andThen(elevateCommand)),
         new ParallelDeadlineGroup(
             new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand));
-    // addCommands(
-    //     maintainCommand
-    //         .alongWith(
-    //             (new EndWhenCloseEnough(() -> maintainCommand.getTargetPose2d()))
-    //                 .andThen(elevateCommand))
-    //         .alongWith(
-    //             new EndWhenCloseEnough(() -> maintainCommand.getTargetPose2d(),
-    // Constants.HardenConstants.RegularCommand.xyIndividualTolerance * Math.sqrt(2),
-    // Constants.HardenConstants.RegularCommand.headingTolerance)
-    //                 .andThen(new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5))));
-    // )
-    // new ParallelDeadlineGroup(
-    //     new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand));
-    // } else {
-    //   DogLog.log("JamesHardenScore/Version", "auto");
-    //   addCommands(
-    //       movementCommand
-    //           .withTimeout(5.0)
-    //           .alongWith(
-    //               (new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d()))
-    //                   .andThen(elevateCommand)),
-    //       new ParallelDeadlineGroup(
-    //           new ShootTootsieSlide(tootsieSlideSubsystem).withTimeout(0.5), maintainCommand));
-    //       new EndWhenCloseEnough(() -> movementCommand.getTargetPose2d(), 2.8284271247);
-    // }
   }
 }

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;
@@ -27,7 +26,6 @@ public void initialize() {}
   @Override
   public void execute() {
     armPlusFlywheel.spinFlywheel(); // Spin the flywheel at desired speed
-    DogLog.log("Running to target angle: ", angle);
     armPlusFlywheel.setPosition(angle);
   }
 

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

@@ -21,7 +21,7 @@ public void initialize() {}
 
   @Override
   public void execute() {
-    DogLog.log("Running to target angle: ", angle);
+    DogLog.log("Commands/ArmToAngleCmd/TargetAngle", angle);
     arm.setPosition(angle);
   }
 

src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java

@@ -10,15 +10,15 @@ public class ZeroElevatorHardStop extends Command {
 
   public ZeroElevatorHardStop(ElevatorSubsystem subsystem) {
     elevatorSubsystem = subsystem;
-    DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false);
+    DogLog.log("Commands/ZeroElevatorHardStop/Running", false);
     addRequirements(elevatorSubsystem);
   }
 
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
     elevatorSubsystem.reduceCurrentLimits();
-    DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", true);
+    DogLog.log("Commands/ZeroElevatorHardStop/Running", true);
     timesExceededCurrent = 0;
   }
 
@@ -32,7 +32,7 @@ public void execute() {
   // Called once the command ends or is interrupted.
   @Override
   public void end(boolean interrupted) {
-    DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/running", false);
+    DogLog.log("Commands/ZeroElevatorHardStop/Running", false);
     if (!interrupted) {
       elevatorSubsystem.resetElevatorPositionToZero();
     }
@@ -43,9 +43,9 @@ public void end(boolean interrupted) {
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/timesExceededCount", timesExceededCurrent);
+    DogLog.log("Commands/ZeroElevatorHardStop/timesExceededCurrent", timesExceededCurrent);
     boolean checkCurrent = elevatorSubsystem.checkCurrent();
-    DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/checkcurrent", checkCurrent);
+    DogLog.log("Commands/ZeroElevatorHardStop/checkCurrent", checkCurrent);
     if (checkCurrent) {
       timesExceededCurrent++;
     } else {