diff --git a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java b/src/main/java/frc/robot/AutoRoutines/AutoProducer.java index 9f1b4256..136263e2 100644 --- a/src/main/java/frc/robot/AutoRoutines/AutoProducer.java +++ b/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); diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7ef43561..5095db4a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 1743b9ec..0abcf2c2 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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,8 +66,6 @@ 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(); @@ -74,8 +73,6 @@ public void robotPeriodic() { 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() { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 404e2792..89f40b82 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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,7 +470,7 @@ public static void setAlliance() { } public BooleanSupplier getRedSide() { - DogLog.log("get alliance", redside.getAsBoolean()); + DogLog.log("Info/Alliance", redside.getAsBoolean()); return redside; } @@ -479,7 +478,7 @@ 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 = diff --git a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java b/src/main/java/frc/robot/commandGroups/JamesHardenScore.java index 37e05084..1122e68f 100644 --- a/src/main/java/frc/robot/commandGroups/JamesHardenScore.java +++ b/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,8 +85,6 @@ public JamesHardenScore( } else { elevateCommand = new SetElevatorLevel(elevatorSubsystem, height, true); } - // if (DriverStation.isTeleop()) { - DogLog.log("JamesHardenScore/Version", "teleop"); addCommands( movementCommand.alongWith( @@ -94,30 +92,5 @@ public JamesHardenScore( .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); - // } } } diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java index a5d410af..a3401471 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleAndSpinFlywheel.java +++ b/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); } diff --git a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java b/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java index 478deada..801b3c82 100644 --- a/src/main/java/frc/robot/commands/DaleCommands/ArmToAngleCmd.java +++ b/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); } diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java index c19e9668..aaedf985 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorHardStop.java @@ -10,7 +10,7 @@ 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); } @@ -18,7 +18,7 @@ public ZeroElevatorHardStop(ElevatorSubsystem subsystem) { @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 { diff --git a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java index 4b265808..28cfe065 100644 --- a/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java +++ b/src/main/java/frc/robot/commands/ElevatorCommands/ZeroElevatorToFFiltered.java @@ -11,14 +11,14 @@ public class ZeroElevatorToFFiltered extends Command { public ZeroElevatorToFFiltered(ElevatorSubsystem subsystem) { elevatorSubsystem = subsystem; - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", false); addRequirements(elevatorSubsystem); } @Override public void initialize() { ticksAtPosition = 0; - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", true); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", true); elevatorSubsystem.elevateTo(ElevatorPositions.Intake); } @@ -27,15 +27,15 @@ public void execute() {} @Override public void end(boolean interrupted) { - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/running", false); + DogLog.log("Commands/ZeroElevatorToFFiltered/running", false); elevatorSubsystem.resetPositionFiltered(); } @Override public boolean isFinished() { - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); + DogLog.log("Commands/ZeroElevatorToFFiltered/ticksAtPosition", ticksAtPosition); boolean inPosition = elevatorSubsystem.isAtPosition() && elevatorSubsystem.atIntake(); - DogLog.log("subsystems/Elevator/ZeroElevatorToFFiltered/inPosition", inPosition); + DogLog.log("Commands/ZeroElevatorToFFiltered/inPosition", inPosition); if (inPosition) { ticksAtPosition++; diff --git a/src/main/java/frc/robot/commands/EndWhenCloseEnough.java b/src/main/java/frc/robot/commands/EndWhenCloseEnough.java index 9aebf3a2..2b0c097e 100644 --- a/src/main/java/frc/robot/commands/EndWhenCloseEnough.java +++ b/src/main/java/frc/robot/commands/EndWhenCloseEnough.java @@ -34,7 +34,7 @@ public EndWhenCloseEnough( // Called when the command is initially scheduled. @Override public void initialize() { - DogLog.log("END_WHEN_CLOSE_ENOUGH", true); + DogLog.log("Commands/CloseEnough", false); } // Called every time the scheduler runs while the command is scheduled. @@ -44,7 +44,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("END_WHEN_CLOSE_ENOUGH", false); + DogLog.log("Commands/CloseEnough", true); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java b/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java index 0420890c..af88577d 100644 --- a/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java +++ b/src/main/java/frc/robot/commands/FunnelCommands/CoralCheckedIn.java @@ -9,13 +9,13 @@ public class CoralCheckedIn extends Command { public CoralCheckedIn(FunnelSubsystem funnelSubsystem) { this.funnelSubsystem = funnelSubsystem; - DogLog.log("CoralCheckedIn", false); + DogLog.log("CoralPosition/CoralCheckedIn", false); } // Called when the command is initially scheduled. @Override public void initialize() { - DogLog.log("CoralCheckedIn", true); + DogLog.log("CoralPosition/CoralCheckedIn", true); // Store the position of the coral when it was first checked out. } @@ -27,7 +27,7 @@ public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - DogLog.log("CoralCheckedIn", false); + DogLog.log("CoralPosition/CoralCheckedIn", false); } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java b/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java index ca4cc04b..a11cdcf5 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/ApplySwerveVoltage.java +++ b/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; @@ -25,7 +24,6 @@ public void initialize() { @Override public void execute() { - DogLog.log("restarted", counter); swerveDrivetrain.getModule(0).getDriveMotor().setVoltage(0.2425 + applyVoltage); swerveDrivetrain.getModule(1).getDriveMotor().setVoltage(0.2425 + applyVoltage); swerveDrivetrain.getModule(2).getDriveMotor().setVoltage(0.2425 + applyVoltage); diff --git a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java index 334ec461..2da6a0dd 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/JamesHardenMovement.java @@ -99,21 +99,31 @@ public void execute() { ChassisSpeeds speeds = swerve.calculateRequiredEdwardChassisSpeeds(targetPose, initialPathDistance); - DogLog.log("JamesHardenMovement/TargetPose", targetPose); - DogLog.log("JamesHardenMovement/TargetPoseX(m)", targetPose.getX()); - DogLog.log("JamesHardenMovement/TargetPoseY(m)", targetPose.getY()); - DogLog.log("JamesHardenMovement/TargetPoseHeading(deg)", targetPose.getRotation().getRadians()); + 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("JamesHardenMovement/DesiredChassisSpeedsX(mps)", speeds.vxMetersPerSecond); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsY(mps)", speeds.vyMetersPerSecond); - DogLog.log("JamesHardenMovement/DesiredChassisSpeedsX(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( - "JamesHardenMovement/ActualChassisSpeedsX(mps)", swerve.getFieldSpeeds().vxMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(mps)", + swerve.getFieldSpeeds().vxMetersPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsY(mps)", swerve.getFieldSpeeds().vyMetersPerSecond); + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsY(mps)", + swerve.getFieldSpeeds().vyMetersPerSecond); DogLog.log( - "JamesHardenMovement/ActualChassisSpeedsX(radps)", + "Commands/JamesHarden/AxisSpecificInformation/ActualChassisSpeedsX(radps)", swerve.getFieldSpeeds().omegaRadiansPerSecond); swerve.setFieldSpeeds(speeds); } @@ -127,21 +137,21 @@ public boolean isFinished() { currRot = ((2.0 * Math.PI) + (currRot % (2.0 * Math.PI))) % (2.0 * Math.PI); double targetRot = targetPose.getRotation().getRadians(); targetRot = ((2.0 * Math.PI) + (targetRot % (2.0 * Math.PI))) % (2.0 * Math.PI); - DogLog.log("JamesHardenMovement/currRot(rad)", currRot); - DogLog.log("JamesHardenMovement/targetRot(rad)", targetRot); + DogLog.log("Commands/JamesHarden/IsFinished/currRot(rad)", currRot); + DogLog.log("Commands/JamesHarden/IsFinished/targetRot(rad)", targetRot); DogLog.log( - "JamesHardenMovement/xTolMet", + "Commands/JamesHarden/IsFinished/xTolMet", (Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) < Constants.HardenConstants.RegularCommand.xyIndividualTolerance)); DogLog.log( - "JamesHardenMovement/yTolMet", + "Commands/JamesHarden/IsFinished/yTolMet", (Math.abs(swerve.getCurrentState().Pose.getY() - targetPose.getY()) < Constants.HardenConstants.RegularCommand.xyIndividualTolerance)); DogLog.log( - "JamesHardenMovement/rotTolMet", + "Commands/JamesHarden/IsFinished/rotTolMet", (Math.min(Math.abs(targetRot - currRot), (Math.PI * 2) - Math.abs(targetRot - currRot)) < Constants.HardenConstants.RegularCommand.headingTolerance)); - DogLog.log("JamesHardenMovement/End", false); + DogLog.log("Commands/JamesHarden/End", false); // translational is met if ((Math.abs(swerve.getCurrentState().Pose.getX() - targetPose.getX()) @@ -170,7 +180,7 @@ public boolean isFinished() { + (swerve.getFieldSpeeds().vyMetersPerSecond * swerve.getFieldSpeeds().vyMetersPerSecond)) <= 0.2)) { - DogLog.log("JamesHardenMovement/End", true); + DogLog.log("Commands/JamesHarden/End", true); return true; } else return false; } @@ -258,10 +268,6 @@ public static JamesHardenMovement toClosestRightBranch( Supplier targetBranch = () -> { Translation2d currPosition = swerve.getCurrentState().Pose.getTranslation(); - DogLog.log("currPositionX", currPosition.getX()); - DogLog.log("currPositionY", currPosition.getY()); - DogLog.log("currPositionTheta", currPosition.getAngle().getRadians()); - DogLog.log("hardenRed", redSide.getAsBoolean()); if (redSide.getAsBoolean()) { double minDist = currPosition.getDistance(RedLandmarkPose.R0.getPose().getTranslation()); diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index ebe874fe..e9a02bb1 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -193,10 +193,10 @@ public void execute() { final double y = ySpeed; double turn = turningSpeed; - DogLog.log("joystickCommand/xSpeed", xSpeed); - DogLog.log("joystickCommand/ySpeed", ySpeed); - DogLog.log("joystickCommand/turningSpeed", turningSpeed); - DogLog.log("fieldCentric", fieldRelativeFunction.getAsBoolean()); + DogLog.log("Commands/joystickCommand/xSpeed", xSpeed); + DogLog.log("Commands/joystickCommand/ySpeed", ySpeed); + DogLog.log("Commands/joystickCommand/turningSpeed", turningSpeed); + DogLog.log("Information/fieldCentric", fieldRelativeFunction.getAsBoolean()); // 5. Applying the drive request on the swerve drivetrain // Uses SwerveRequestFieldCentric (from java.frc.robot.util to apply module optimization) if (fixedRotation.getAsBoolean()) { diff --git a/src/main/java/frc/robot/subsystems/AnthonyVision.java b/src/main/java/frc/robot/subsystems/AnthonyVision.java new file mode 100644 index 00000000..60b14e60 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/AnthonyVision.java @@ -0,0 +1,356 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import dev.doglog.DogLog; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; +import java.util.List; +import java.util.Optional; +import java.util.function.BooleanSupplier; +import java.util.stream.Collectors; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonCamera; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.PhotonPoseEstimator.PoseStrategy; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** + * VisionSystem fuses multi-tag AprilTag measurements from PhotonVision with swerve odometry by + * dynamically computing measurement noise based on tag count, distance, viewing angle, and robot + * speed. + */ +public class AnthonyVision extends SubsystemBase { + // static member of AnthonyVision that contains array of all existing AnthonyVision systems + private static AnthonyVision[] systemList = + new AnthonyVision[Constants.Vision.Cameras.values().length]; + + // 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; + // Reef tag IDs for each side of the field + private static final List BLUE_SIDE_TAG_IDS = List.of(19, 20, 21, 22, 17, 18); + private static final List RED_SIDE_TAG_IDS = List.of(6, 7, 8, 9, 10, 11); + + // Noise parameters + private double calibrationFactor = 1.0; // constant multiplier to everything + private double baseNoiseX = 0.0008; // meters + private double baseNoiseY = 0.0008; + private double baseNoiseTheta = 0.5; // radians + + // private double distanceCoefficientX = 0.06; + // private double distanceCoefficientY = 0.06; + + private double distanceExponentialCoefficientX = 0.00046074; + private double distanceExponentialBaseX = 2.97294; + private double distanceExponentialCoefficientY = 0.00046074; + private double distanceExponentialBaseY = 2.97294; + + private double distanceCoefficientTheta = 0.9; + + private double angleCoefficientX = 0.5; // noise growth per radian of viewing angle + private double angleCoefficientY = 0.5; + private double angleCoefficientTheta = 0.5; + + private double speedCoefficientX = 0.5; // noise growth per fraction of max speed + private double speedCoefficientY = 0.5; + private double speedCoefficientTheta = 0.5; + + // Maximums for normalization + private double maximumRobotSpeed = 5; // meters per second + private double maximumAllowedDistance = 15.0; // meters, beyond which readings are dropped + + // PhotonVision and odometry references + private final PhotonCamera photonCamera; + private final PhotonPoseEstimator poseEstimator; // MULTI_TAG_PNP_ON_COPROCESSOR + private PhotonPipelineResult latestVisionResult; + private final BooleanSupplier isRedSide; + private SwerveSubsystem swerveDrive; + private final AprilTagFieldLayout fieldLayout; + + public AnthonyVision(Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + this.isRedSide = isRedSide; + this.cameraId = cameraId; + photonCamera = new PhotonCamera(cameraId.toString()); + Transform3d cameraToRobot = Constants.Vision.getCameraTransform(cameraId); + + // Initialize field layout + this.fieldLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); + + // Initialize both pose estimators + poseEstimator = + new PhotonPoseEstimator( + fieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, cameraToRobot); + + camTitle = cameraId.getLoggingName(); + + latestVisionResult = null; + } + + // Get instance of AnthonyVision + public static AnthonyVision getInstance( + Constants.Vision.Cameras cameraId, BooleanSupplier isRedSide) { + int idx = cameraId.ordinal(); + if (systemList[idx] == null) { + systemList[idx] = new AnthonyVision(cameraId, isRedSide); + } + return systemList[idx]; + } + + @Override + public void periodic() { + // Initialize swerve drive if not already done + if (swerveDrive == null) { + swerveDrive = SwerveSubsystem.getInstance(); + } + + // Check camera connection + boolean cameraConnected = photonCamera.isConnected(); + + // If the current camera isn't connected, there's nothing to do here + if (!cameraConnected) { + DogLog.log("Vision/" + camTitle + "/CameraConnected", false); + return; + } + + DogLog.log("Vision/" + camTitle + "/CameraConnected", true); + // Get all unread results + List results = photonCamera.getAllUnreadResults(); + + // Go through all results (if there are any) and update the latest result with the last + for (var result : results) { + latestVisionResult = result; + } + } + + public void addFilteredPose() { + PhotonPoseEstimator selectedEstimator = poseEstimator; + if (latestVisionResult == null || !latestVisionResult.hasTargets()) { + DogLog.log("Vision/" + camTitle + "/HasTargets", false); + return; + } + DogLog.log("Vision/" + camTitle + "/HasTargets", true); + + double averageDistance = + latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .average() + .orElse(Double.NaN); + + double minDistance = + latestVisionResult.getTargets().stream() + .mapToDouble(t -> t.getBestCameraToTarget().getTranslation().getNorm()) + .min() + .orElse(Double.NaN); + + // Filter tags to only those on the active side + List validTags = + latestVisionResult.getTargets().stream() + .filter(t -> isTagOnActiveSide(t.getFiducialId())) + .filter(t -> isNotChopped(t.getYaw())) + .collect(Collectors.toList()); + + DogLog.log("Vision/" + camTitle + "/numValidTags", validTags.size()); + + for (PhotonTrackedTarget tag : validTags) { + DogLog.log("Vision/" + camTitle + "/Area", tag.getArea()); + DogLog.log("Vision/" + camTitle + "/Yaw", tag.getYaw()); + } + + // Log all detected tags for debugging + String allTagIds = + latestVisionResult.getTargets().stream() + .map(t -> Integer.toString(t.getFiducialId())) + .collect(Collectors.joining(",")); + + DogLog.log("Vision/" + camTitle + "/allTagIds", allTagIds); + + if (validTags.isEmpty()) { + DogLog.log("Vision/" + camTitle + "/ValidTags", false); + return; + } + DogLog.log("Vision/" + camTitle + "/ValidTags", true); + + // Log all tags that haven't been thrown out + int tagCount = validTags.size(); + + // Compute effective metrics for solution + // Use camera→target distance from PV (avoids odometry dependence) + + // nothing to do if rejected based on the minDistance or if no min dist has been found + if (Double.isNaN(minDistance) || minDistance > maximumAllowedDistance) { + DogLog.log("Vision/" + camTitle + "/ThrownOutDistance", true); + return; + } + DogLog.log("Vision/" + camTitle + "/ThrownOutDistance", false); + + // find the current speed + double currentSpeed = + Math.hypot( + swerveDrive.getRobotSpeeds().vxMetersPerSecond, + swerveDrive.getRobotSpeeds().vyMetersPerSecond); + DogLog.log("Vision/" + camTitle + "/VisionDrivebaseSpeed", currentSpeed); + + // Get the pose from PhotonVision + Optional maybePose = selectedEstimator.update(latestVisionResult); + if (maybePose.isEmpty()) { + DogLog.log("Vision/" + camTitle + "/AvailablePose", false); + return; + } + DogLog.log("Vision/" + camTitle + "/AvailablePose", true); + + EstimatedRobotPose estimatedPose = maybePose.get(); + Pose2d measuredPose = estimatedPose.estimatedPose.toPose2d(); + + double nX = + computeNoiseXY( + baseNoiseX, + distanceExponentialCoefficientX, + distanceExponentialBaseX, + angleCoefficientX, + speedCoefficientX, + averageDistance, + currentSpeed, + tagCount); + double nY = + computeNoiseXY( + baseNoiseY, + distanceExponentialCoefficientY, + distanceExponentialBaseY, + angleCoefficientY, + speedCoefficientY, + averageDistance, + currentSpeed, + tagCount); + double nTH = + computeNoiseHeading( + baseNoiseTheta, + distanceCoefficientTheta, + angleCoefficientTheta, + speedCoefficientTheta, + averageDistance, + currentSpeed, + tagCount); + + DogLog.log("Vision/" + camTitle + "/speed", currentSpeed); + DogLog.log("Vision/" + camTitle + "/nX", nX); + DogLog.log("Vision/" + camTitle + "/nY", nY); + DogLog.log("Vision/" + camTitle + "/nTH", nTH); + DogLog.log("Vision/" + camTitle + "/Pose", measuredPose); + DogLog.log("Vision/" + camTitle + "/averageDistance", averageDistance); + + Matrix noiseVector = VecBuilder.fill(nX, nY, nTH); + // Process locally (no cross-camera comparison) + processPoseEstimate( + measuredPose, + averageDistance, + currentSpeed, + tagCount, + latestVisionResult.getTimestampSeconds(), + noiseVector); + } + + /** Final processing and addition of pose estimate to odometry. */ + private void processPoseEstimate( + Pose2d measuredPose, + double averageDistance, + double currentSpeed, + int tagCount, + double timestamp, + Matrix noiseVector) { + // Choose timestamp: use vision timestamp unless it differs too much from FPGA + double fpgaTimestamp = Timer.getFPGATimestamp(); + double timestampDifference = Math.abs(timestamp - fpgaTimestamp); + double chosenTimestamp = (timestampDifference > 0.5) ? fpgaTimestamp - 0.03 : timestamp; + + // Build the noise vector and add the vision measurement + + swerveDrive.addVisionMeasurement(measuredPose, chosenTimestamp, noiseVector); + } + + private boolean isTagOnActiveSide(int tagId) { + return isRedSide.getAsBoolean() + ? RED_SIDE_TAG_IDS.contains(tagId) + : BLUE_SIDE_TAG_IDS.contains(tagId); + } + + private boolean isNotChopped(double yaw) { + return (Math.abs(yaw) < 60d); + } + + private double computeNoiseXY( + double baseNoise, + double distanceExponentialCoefficient, + double distanceExponentialBase, + double angleCoefficient, + double speedCoefficient, + double distance, + double robotSpeed, + int tagCount) { + + // Tag count factor (diminishing returns; cap at 4) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1.0 / Math.sqrt(effectiveTags); + + // Distance term (keep as d^2) + // double distanceFactor = baseNoise + + // distanceExponentialCoefficient*Math.pow(distanceExponentialBase, distance); + + double distanceFactor = + (distance < (17.548 + 0.67)) + ? Math.min( + baseNoise + + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance), + 1.167) + : (baseNoise + + distanceExponentialCoefficient * Math.pow(distanceExponentialBase, distance)); + + // Speed term (quadratic, saturated) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); + DogLog.log("Vision/calibrationFactor", calibrationFactor); + 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, + double angleCoefficient, + double speedCoefficient, + double distance, + double robotSpeed, + int tagCount) { + + // Tag count factor (diminishing returns; cap at 4) + int effectiveTags = Math.min(tagCount, 4); + double tagFactor = 1.0 / Math.sqrt(effectiveTags); + + // Distance term (keep as d^2) + double distanceFactor = baseNoise + distanceCoefficient * distance * distance; + + // Speed term (quadratic, saturated) + double vNorm = Math.min(robotSpeed, maximumRobotSpeed) / maximumRobotSpeed; + double speedFactor = 1.0 + speedCoefficient * (vNorm * vNorm); + + double computedStdDevs = calibrationFactor * tagFactor * distanceFactor * speedFactor; + return computedStdDevs; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 31289751..366ec512 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -159,7 +159,7 @@ public boolean checkCurrent() { private void runFlywheelAtRPS(double flywheelSpeed) { double motor_speed = flywheelSpeed / Constants.Flywheel.GEAR_RATIO; motor_speed = MathUtil.clamp(motor_speed, -100, 100); - DogLog.log("subsystems/Dale/Target Motor Speed", motor_speed); + DogLog.log("Subsystems/Dale/Target Motor Speed", motor_speed); flywheelMotor.setControl(controlRequestFlywheel.withVelocity(motor_speed)); } @@ -194,15 +194,15 @@ public void periodic() { encoderDegrees = Constants.Arm.ROTATIONS_TO_DEGEREES(armMotor.getPosition(false).getValueAsDouble()); // This method will be called once per scheduler run - DogLog.log("subsystems/Dale/Arm at target", atTarget(5)); - DogLog.log("subsystems/Dale/Arm Degrees", encoderDegrees); + DogLog.log("Subsystems/Dale/Arm at target", atTarget(5)); + DogLog.log("Subsystems/Dale/Arm Degrees", encoderDegrees); DogLog.log( - "subsystems/Dale/Arm Target Rotations", Constants.Arm.DEGREES_TO_ROTATIONS(targetDegrees)); - DogLog.log("subsystems/Dale/Arm Target Degrees", targetDegrees); + "Subsystems/Dale/Arm Target Rotations", Constants.Arm.DEGREES_TO_ROTATIONS(targetDegrees)); + DogLog.log("Subsystems/Dale/Arm Target Degrees", targetDegrees); // DogLog.log( // "subsystems/Dale/Flywheel Speed", flywheelMotor.getVelocity(false).getValueAsDouble()); DogLog.log( - "subsystems/Dale/command", + "Subsystems/Dale/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java index a00efcb2..a9595664 100644 --- a/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ElevatorSubsystem.java @@ -143,7 +143,7 @@ public void resetPositionFiltered() { master.setPosition( currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", currentHeightToF * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -153,7 +153,7 @@ public void resetPosition() { this.getToFDistance() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", this.getToFDistance() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -164,7 +164,7 @@ public void resetPosition(double posInHeight) { master.setPosition( posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); DogLog.log( - "subsystems/Elevator/resetElevatorPosition", + "Subsystems/Elevator/resetElevatorPosition", posInHeight * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS); } @@ -194,8 +194,8 @@ public void resetElevatorPositionToZero() { public boolean checkCurrent() { double Supplycurrent = Math.abs(master.getSupplyCurrent().getValue().magnitude()); double Statorcurrent = Math.abs(master.getStatorCurrent().getValue().magnitude()); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); - DogLog.log("subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); + DogLog.log("Subsystems/Elevator/ZeroElevatorHardStop/supply", Supplycurrent); + DogLog.log("Subsystems/Elevator/ZeroElevatorHardStop/stator", Statorcurrent); if (Supplycurrent > 1.0 && Statorcurrent > 20) { return true; @@ -232,14 +232,14 @@ public void setPosition(double height) { / ElevatorConstants.CARRAIGE_UPDUCTION) .withSlot(0)); DogLog.log( - "subsystems/Elevator/elevatorSetpoint(rot)", + "Subsystems/Elevator/elevatorSetpoint(rot)", height * ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / ElevatorConstants.CARRAIGE_UPDUCTION); } public void ElevatorTorqueMode() { - DogLog.log("subsystems/Elevator/usingTorqueMode", true); + DogLog.log("Subsystems/Elevator/usingTorqueMode", true); master.setControl(torqueRequest.withOutput(Constants.ElevatorConstants.ELEVATOR_TORQUE)); // .withMaxAbsDutyCycle(Constants.ElevatorConstants.ELEVATOR_DUTY_CYCLE)); } @@ -261,7 +261,7 @@ public boolean canFunnelTransferCoralToScoring() { public double getToFDistance() { // 0.11 is the sensor offset DogLog.log( - "subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); + "Subsystems/Elevator/ToF/DistanceNoOffset", distance.getDistance().getValueAsDouble()); return distance.getDistance().getValueAsDouble() - Constants.ElevatorConstants.SENSOR_OFFSET; } @@ -277,38 +277,38 @@ public void elevatorHasBeenZeroed() { public void periodic() { currentHeightToF = elevatorFilter.calculate(getToFDistance()); // Time of Flight Sensor - DogLog.log("subsystems/Elevator/getError", getError()); - DogLog.log("subsystems/Elevator/ToF/Distance", getToFDistance()); - DogLog.log("subsystems/Elevator/ToF/Connected", distance.isConnected()); - DogLog.log("subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); - - DogLog.log("subsystems/Elevator/isAtPosition", this.isAtPosition()); - DogLog.log("subsystems/Elevator/targetPosition", currentLevel.getPosition()); - DogLog.log("subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); + DogLog.log("Subsystems/Elevator/getError", getError()); + DogLog.log("Subsystems/Elevator/ToF/Distance", getToFDistance()); + DogLog.log("Subsystems/Elevator/ToF/Connected", distance.isConnected()); + DogLog.log("Subsystems/Elevator/ToF/LinearFilterDistance", currentHeightToF); + + DogLog.log("Subsystems/Elevator/isAtPosition", this.isAtPosition()); + DogLog.log("Subsystems/Elevator/targetPosition", currentLevel.getPosition()); + DogLog.log("Subsystems/Elevator/targetHeightDist", currentLevel.getHeight()); DogLog.log( - "subsystems/Elevator/targetHeightRot", + "Subsystems/Elevator/targetHeightRot", currentLevel.getHeight() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_DISTANCE_TO_ROTATIONS / Constants.ElevatorConstants.CARRAIGE_UPDUCTION); DogLog.log( - "subsystems/Elevator/currentHeightDist", + "Subsystems/Elevator/currentHeightDist", master.getPosition().getValueAsDouble() * Constants.ElevatorConstants.CONVERSION_FACTOR_UP_ROTATIONS_TO_DISTANCE * Constants.ElevatorConstants.CARRAIGE_UPDUCTION); - DogLog.log("subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); + DogLog.log("Subsystems/Elevator/currentHeightRot", master.getPosition().getValueAsDouble()); DogLog.log( - "subsystems/Elevator/command", + "Subsystems/Elevator/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); DogLog.log( - "subsystems/Elevator/resetPositionBoolean", + "Subsystems/Elevator/resetPositionBoolean", this.isAtPosition() && this.getLevel().equals(ElevatorPositions.Intake)); DogLog.log( - "subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); - DogLog.log("subsystems/Elevator/targetLevel", this.getLevel().toString()); + "Subsystems/Elevator/targetisIntake", this.getLevel().equals(ElevatorPositions.Intake)); + DogLog.log("Subsystems/Elevator/targetLevel", this.getLevel().toString()); DogLog.log( - "subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); + "Subsystems/Elevator/closedLoopError", master.getClosedLoopError().getValueAsDouble()); DogLog.log( - "subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); + "Subsystems/Elevator/elevatorProfile", master.getClosedLoopReference().getValueAsDouble()); } @Override @@ -321,9 +321,5 @@ public void simulationPeriodic() { double newPosition = currentPosition + simulatedSpeed * 0.02; // Assuming a 20ms loop master.setPosition( newPosition); // Alarming to have this since running this on the robot will lead to - - // Log simulation data for debugging - DogLog.log("Simulated Position", newPosition); - DogLog.log("Simulated Speed", simulatedSpeed); } } diff --git a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java index 543bb5c7..798a2fbe 100644 --- a/src/main/java/frc/robot/subsystems/FunnelSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FunnelSubsystem.java @@ -195,13 +195,13 @@ public boolean drakeTripped() { @Override public void periodic() { // This method will be called once per scheduler run - DogLog.log("subsystems/Funnel/CheckedInStatus", isCoralCheckedIn()); - DogLog.log("subsystems/Funnel/CheckedOutStatus", isCoralCheckedOut()); - DogLog.log("subsystems/Funnel/DrakeStatus", drakeTripped()); + DogLog.log("Subsystems/Funnel/CheckedInStatus", isCoralCheckedIn()); + DogLog.log("Subsystems/Funnel/CheckedOutStatus", isCoralCheckedOut()); + DogLog.log("Subsystems/Funnel/DrakeStatus", drakeTripped()); // DogLog.log("subsystems/Funnel/FunnelVelocity", rightMotor.getVelocity().getValueAsDouble()); - DogLog.log("subsystems/Funnel/AbsPositionalError", getAbsolutePositionalError()); + DogLog.log("Subsystems/Funnel/AbsPositionalError", getAbsolutePositionalError()); DogLog.log( - "subsystems/Funnel/command", + "Subsystems/Funnel/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 72b21422..25f88f6e 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -69,22 +69,21 @@ public SwerveSubsystem( // 1.7, 0.345, 0.0015 qProfiledPIDController = new ProfiledPIDController( - 3.4, // 3.4 not bad // [3.4 good for 0.2-1.2, 0.425 I] - 0.45, // 345 - 0.0005, // 0.0015 + Constants.HardenConstants.QKP, // 3.4 not bad // [3.4 good for 0.2-1.2, 0.425 I] + Constants.HardenConstants.QKI, // 345 + Constants.HardenConstants.QKD, // 0.0015 new TrapezoidProfile.Constraints( - Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND - 0.5, - 8)); // 8.25 // 5 accel and 0.75 p was good + Constants.HardenConstants.QCRUISE, + Constants.HardenConstants.QACCEL)); // 8.25 // 5 accel and 0.75 p was good headingProfiledPIDController = new ProfiledPIDController( - 3.7, // 4 was good - 0.4, // - 0, + Constants.HardenConstants.HKP, // 4 was good + Constants.HardenConstants.HKI, // + Constants.HardenConstants.HKD, new TrapezoidProfile.Constraints( - Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_RATE - 1.5, // -1 was good - Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_ACCELERATION_UNITS_PER_SECOND - - 16)); // -13 was good + Constants.HardenConstants.HCRUISE, // -1 was good + Constants.HardenConstants.HACCEL)); // -13 was good // headingProfiledPIDController = // new ProfiledPIDController( // 1, // 4 was good @@ -95,11 +94,14 @@ public SwerveSubsystem( // Constants.Swerve.TELE_DRIVE_MAX_ANGULAR_ACCELERATION_UNITS_PER_SECOND // - 16)); // -13 was good - qProfiledPIDController.setIZone(0.35); - headingProfiledPIDController.setIZone(0.14); + qProfiledPIDController.setIZone(Constants.HardenConstants.QIZONE); + headingProfiledPIDController.setIZone(Constants.HardenConstants.HIZONE); - qProfiledPIDController.setIntegratorRange(0, 0.2); - headingProfiledPIDController.setIntegratorRange(0.0, Math.PI / 4); // 0.3 before + qProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.QIRANGE_LOWER, Constants.HardenConstants.QIRANGE_UPPER); + headingProfiledPIDController.setIntegratorRange( + Constants.HardenConstants.HIRANGE_LOWER, + Constants.HardenConstants.HIRANGE_UPPER); // 0.3 before headingProfiledPIDController.enableContinuousInput(-Math.PI, Math.PI); // configureAutoBuilder(); @@ -344,39 +346,39 @@ public ChassisSpeeds calculateRequiredEdwardChassisSpeeds( Rotation2d travelAngle = travelAngleTo(targetPose); DogLog.log( - "EdwardCalculation/qProfiledPID/CurrentPositionMeasurement", + "Commands/JamesHarden/Edward/qPositionMeasurement", completePathDistance - distanceToTarget); DogLog.log( - "EdwardCalculation/qProfiledPID/PositionSetpoint", + "Commands/JamesHarden/Edward/qPositionSetpoint", qProfiledPIDController.getSetpoint().position); DogLog.log( - "EdwardCalculation/qProfiledPID/CurrentVelocityMeasurement", + "Commands/JamesHarden/Edward/qVelocityMeasurement", getDirectionalChassisSpeeds(travelAngle)); DogLog.log( - "EdwardCalculation/qProfiledPID/VelocitySetpoint", + "Commands/JamesHarden/Edward/qVelocitySetpoint", qProfiledPIDController.getSetpoint().velocity); DogLog.log( - "EdwardCalculation/qProfiledPID/PositionError", qProfiledPIDController.getPositionError()); + "Commands/JamesHarden/Edward/qPositionError", qProfiledPIDController.getPositionError()); DogLog.log( - "EdwardCalculation/qProfiledPID/VelocityError", qProfiledPIDController.getVelocityError()); + "Commands/JamesHarden/Edward/qVelocityError", qProfiledPIDController.getVelocityError()); DogLog.log( - "EdwardCalculation/headingProfiledPID/CurrentPositionMeasurement", + "Commands/JamesHarden/Rotational/PositionMeasurement", currentState.Pose.getRotation().getRadians()); DogLog.log( - "EdwardCalculation/headingProfiledPID/PositionSetpoint", + "Commands/JamesHarden/Rotational/PositionSetpoint", headingProfiledPIDController.getSetpoint().position); DogLog.log( - "EdwardCalculation/headingProfiledPID/CurrentVelocityMeasurement", + "Commands/JamesHarden/Rotational/VelocityMeasurement", currentState.Speeds.omegaRadiansPerSecond); DogLog.log( - "EdwardCalculation/headingProfiledPID/VelocitySetpoint", + "Commands/JamesHarden/Rotational/VelocitySetpoint", headingProfiledPIDController.getSetpoint().velocity); DogLog.log( - "EdwardCalculation/headingProfiledPID/PositionError", + "Commands/JamesHarden/Rotational/PositionError", headingProfiledPIDController.getPositionError()); DogLog.log( - "EdwardCalculation/headingProfiledPID/VelocityError", + "Commands/JamesHarden/Rotational/VelocityError", headingProfiledPIDController.getVelocityError()); return new ChassisSpeeds( @@ -455,25 +457,6 @@ public void addVisionMeasurement( @Override public void periodic() { - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - DogLog.log( - "subsystems/swerve/module0/drive/speedrps", - this.getModule(1).getDriveMotor().getVelocity().getValueAsDouble()); - currentState = getState(); if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) { @@ -488,16 +471,10 @@ public void periodic() { }); } - DogLog.log("subsystems/swerve/fl_speed", currentState.ModuleStates[0].speedMetersPerSecond); - DogLog.log("subsystems/swerve/fl_angle", currentState.ModuleStates[0].angle.getDegrees()); - DogLog.log("subsystems/swerve/fr_speed", currentState.ModuleStates[1].speedMetersPerSecond); - DogLog.log("subsystems/swerve/fr_angle", currentState.ModuleStates[1].angle.getDegrees()); - DogLog.log("subsystems/swerve/bl_speed", currentState.ModuleStates[2].speedMetersPerSecond); - DogLog.log("subsystems/swerve/bl_angle", currentState.ModuleStates[2].angle.getDegrees()); - DogLog.log("subsystems/swerve/br_speed", currentState.ModuleStates[3].speedMetersPerSecond); - DogLog.log("subsystems/swerve/br_angle", currentState.ModuleStates[3].angle.getDegrees()); + DogLog.log("PoseEstimation/DrivetrainPose", currentState.Pose); + DogLog.log( - "subsystems/swerve/command", + "Subsystems/SwerveSubsystem/CurrentCommand", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } } diff --git a/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java b/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java index fc705c73..ea9e4677 100644 --- a/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TootsieSlideSubsystem.java @@ -77,7 +77,7 @@ public static TootsieSlideSubsystem getInstance() { private void runTootsieAtRPS(double flywheelSpeed) { double motor_speed = flywheelSpeed / Constants.TootsieSlide.GEAR_RATIO; - DogLog.log("subsystems/tootsieslide/Target Motor Speed", motor_speed); + DogLog.log("Subsystems/tootsieslide/Target Motor Speed", motor_speed); master.setControl(m_velocity.withVelocity(motor_speed)); m_flywheelSim.setInputVoltage(master.getSupplyVoltage().getValueAsDouble()); @@ -113,7 +113,7 @@ public void periodic() { // DogLog.log("subsystems/tootsieslide/tootsieVelocity", // master.getVelocity().getValueAsDouble()); DogLog.log( - "subsystems/tootsieslide/command", + "Subsystems/tootsieslide/command", this.getCurrentCommand() == null ? "NOTHING" : this.getCurrentCommand().getName()); } diff --git a/src/main/java/frc/robot/subsystems/VisionSystem.java b/src/main/java/frc/robot/subsystems/VisionSystem.java deleted file mode 100644 index 5004edc8..00000000 --- a/src/main/java/frc/robot/subsystems/VisionSystem.java +++ /dev/null @@ -1,274 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.subsystems; - -import dev.doglog.DogLog; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; -import edu.wpi.first.math.Matrix; -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.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants; -import frc.robot.util.MiscUtils; -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; -import java.util.Optional; -import java.util.function.BooleanSupplier; -import org.photonvision.EstimatedRobotPose; -import org.photonvision.PhotonCamera; -import org.photonvision.PhotonPoseEstimator; -import org.photonvision.PhotonPoseEstimator.PoseStrategy; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; - -/** Creates a new VisionSystem. */ -public class VisionSystem extends SubsystemBase { - - public static int numThrowaways = 0; - public static int numNotThrowaways = 0; - List reefIDs = - new ArrayList(Arrays.asList(19, 20, 21, 22, 17, 18, 6, 7, 8, 9, 10, 11)); - List blueReefID = new ArrayList(Arrays.asList(19, 20, 21, 22, 17, 18)); - List redReefID = new ArrayList(Arrays.asList(6, 7, 8, 9, 10, 11)); - - Pose2d savedResult = new Pose2d(0, 0, new Rotation2d(0.01, 0.01)); - private static VisionSystem[] systemList = - new VisionSystem[Constants.Vision.Cameras.values().length]; - private Transform3d[] camToRobots = { - // right Camera transform - new Transform3d( - new Translation3d( - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_X, - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_Y, - Constants.Vision.RIGHT_CAM_TO_ROBOT_TRANSLATION_Z), - new Rotation3d( - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_ROLL, - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_PITCH, - Constants.Vision.RIGHT_CAM_TO_ROBOT_ROTATION_YAW)), - // left Camera - new Transform3d( - new Translation3d( - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_X, - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_Y, - Constants.Vision.LEFT_CAM_TO_ROBOT_TRANSLATION_Z), - new Rotation3d( - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_ROLL, - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_PITCH, - Constants.Vision.LEFT_CAM_TO_ROBOT_ROTATION_YAW)), - }; - private PhotonCamera camera; - private Constants.Vision.Cameras cameraEnum; - private PhotonPipelineResult pipeline; - AprilTagFieldLayout aprilTagFieldLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - PhotonPoseEstimator photonPoseEstimator; - private SwerveSubsystem driveTrain = SwerveSubsystem.getInstance(); - private BooleanSupplier redSide; - - public VisionSystem(Constants.Vision.Cameras cameraEnum, BooleanSupplier redSide) { - this.cameraEnum = cameraEnum; - String name = cameraEnum.toString(); - int index = cameraEnum.ordinal(); - camera = new PhotonCamera(name); - photonPoseEstimator = - new PhotonPoseEstimator( - aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camToRobots[index]); - this.redSide = redSide; - for (var x : camera.getAllUnreadResults()) { - pipeline = x; - } - } - - public Double getDistance() { - return this.getAprilTagFieldLayout() - .getTagPose(getPipelineResult().getBestTarget().getFiducialId()) - .get() - .getTranslation() - .getDistance( - new Translation3d( - driveTrain.getState().Pose.getX(), driveTrain.getState().Pose.getY(), 0.0)); - } - - public static VisionSystem getInstance(Constants.Vision.Cameras name, BooleanSupplier redSide) { - if (systemList[name.ordinal()] == null) { - systemList[name.ordinal()] = new VisionSystem(name, redSide); - } - - return systemList[name.ordinal()]; - } - - // use this when feeding into drivetrain - public Optional getMultiTagPose3d(Pose2d previousRobotPose) { - photonPoseEstimator.setReferencePose(previousRobotPose); - return photonPoseEstimator.update(pipeline); - } - - public Pose2d getPose2d() { - Optional pose3d = getMultiTagPose3d(savedResult); - if (pose3d.isEmpty()) return null; - savedResult = pose3d.get().estimatedPose.toPose2d(); - return savedResult; - } - - public Pose2d getSaved() { - return savedResult; - } - - public boolean hasTarget(PhotonPipelineResult pipeline) { - if (pipeline == null) { - return false; - } - return pipeline.hasTargets(); - } - - public void setReference(Pose2d newPose) { - if (newPose == null) { - return; - } - savedResult = newPose; - } - - public static Pose2d getAverageForOffBotTesting(Pose2d one, Pose2d two) { - if (one == null) return two; - if (two == null) return one; - - return new Pose2d( - (one.getX() + two.getX()) / 2, (one.getY() + two.getY()) / 2, one.getRotation()); - } - - public AprilTagFieldLayout getAprilTagFieldLayout() { - return this.aprilTagFieldLayout; - } - - public PhotonPipelineResult getPipelineResult() { - return pipeline; - } - - public void addFilteredPose() { - String camName = cameraEnum.getLoggingName(); - PhotonPipelineResult pipelineResult = getPipelineResult(); - - DogLog.log("KalmanDebug/" + camName + "PiplineNull", pipelineResult == null); - DogLog.log("KalmanDebug/" + camName + "PipelineHasTarget", hasTarget(pipelineResult)); - - if (pipelineResult != null && hasTarget(pipelineResult)) { - - List targets = pipelineResult.getTargets(); - - boolean hasReefTag = true; - double poseAmbiguity = pipelineResult.getBestTarget().poseAmbiguity; - double distance = getDistance(); - - DogLog.log("KalmanDebug/" + camName + "PoseAmbiguity", poseAmbiguity); - DogLog.log("KalmanDebug/" + camName + "DistToAprilTag", distance); - - for (PhotonTrackedTarget target : targets) { - DogLog.log("redside in vision system", redSide.getAsBoolean()); - if (redSide.getAsBoolean()) { - if (!redReefID.contains(target.getFiducialId())) { - hasReefTag = false; - } - } else { - if (!blueReefID.contains(target.getFiducialId())) { - hasReefTag = false; - } - } - // if (!reefIDs.contains(target.getFiducialId())) { - // hasReefTag = false; - // } - } - - if (hasReefTag) { - double speedMultiplier = - (Math.sqrt( - Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2))) - / 2 - + 1.0; - double xKalman = MiscUtils.lerp((distance - 0.62) / 3, 0.03, 0.3, 1.0) * speedMultiplier; - double yKalman = MiscUtils.lerp((distance - 0.62) / 3, 0.03, 0.3, 1.0) * speedMultiplier; - // double speedMultiplier = 1; - // if (Math.sqrt( - // Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - // + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2)) - // > 1) { - // speedMultiplier = 2; - // } - // speedMultiplier = - // (Math.sqrt( - // Math.pow(driveTrain.getRobotSpeeds().vxMetersPerSecond, 2) - // + Math.pow(driveTrain.getRobotSpeeds().vyMetersPerSecond, 2))) - // / 2 - // + 1.0; - // double xKalman = MiscUtils.lerp((distance - 0.6) / 2.4, 0.04, 0.5, 1.0) * - // speedMultiplier; - // double yKalman = MiscUtils.lerp((distance - 0.6) / 2.4, 0.04, 0.5, 1.0) * - // speedMultiplier; - double rotationKalman = MiscUtils.lerp((distance - 0.6) / 1.4, 0.4, 5, 30) / 10; - - DogLog.log("KalmanDebug/" + camName + "TranslationStandardDeviation", xKalman); - DogLog.log("KalmanDebug/" + camName + "RotationStandardDeviation", rotationKalman); - - Matrix visionMatrix = VecBuilder.fill(xKalman, yKalman, rotationKalman); - Pose2d bestRobotPose2d = getPose2d(); - - if (bestRobotPose2d == null) { - VisionSystem.numThrowaways++; - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - DogLog.log("KalmanDebug/" + camName + "throwaways", VisionSystem.numThrowaways); - return; - } - Pose2d rotationLess = - new Pose2d(bestRobotPose2d.getTranslation(), driveTrain.getState().Pose.getRotation()); - DogLog.log("KalmanDebug/" + camName + "Rotationless", rotationLess); - DogLog.log("KalmanDebug/" + camName + "RobotPoseIsPresent", bestRobotPose2d != null); - DogLog.log("KalmanDebug/" + camName + "VisionPose", bestRobotPose2d); - - double xDifference = Math.abs(driveTrain.getPose().getX() - bestRobotPose2d.getX()); - double yDifference = Math.abs(driveTrain.getPose().getY() - bestRobotPose2d.getY()); - double rotDifference = - Math.abs( - driveTrain.getPose().getRotation().getDegrees() - - bestRobotPose2d.getRotation().getDegrees()); - Translation2d transDifference = new Translation2d(xDifference, yDifference); - Rotation2d rot2dDifference = new Rotation2d(rotDifference); - - Pose2d visionDiff = new Pose2d(transDifference, rot2dDifference); - double timeDiff = Math.abs(pipelineResult.getTimestampSeconds() - Timer.getTimestamp()); - DogLog.log("KalmanDebug/" + camName + "visionDiff", visionDiff); - DogLog.log("KalmanDebug/" + camName + "diffInTimestamps", timeDiff); - - driveTrain.addVisionMeasurement( - bestRobotPose2d, - timeDiff > 5 ? Timer.getTimestamp() - 0.070 : pipelineResult.getTimestampSeconds(), - visionMatrix); - VisionSystem.numNotThrowaways++; - DogLog.log("KalmanDebug/" + camName + "numNotThrowaway", VisionSystem.numNotThrowaways); - DogLog.log("KalmanDebug/" + camName + "visionUsed", true); - } else { - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - } - } else { - DogLog.log("KalmanDebug/" + camName + "visionUsed", false); - } - } - - @Override - public void periodic() { - for (var x : camera.getAllUnreadResults()) { - pipeline = x; - } - } -} diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json index b410eb1d..2ee13f69 100644 --- a/vendordeps/DogLog.json +++ b/vendordeps/DogLog.json @@ -3,7 +3,7 @@ { "groupId": "com.github.jonahsnider", "artifactId": "doglog", - "version": "2025.3.0" + "version": "2025.8.1" } ], "fileName": "DogLog.json", @@ -15,6 +15,6 @@ "https://jitpack.io" ], "cppDependencies": [], - "version": "2025.3.0", + "version": "2025.8.1", "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" } \ No newline at end of file diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index acc78dba..6f40c840 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.2", + "version": "25.4.0", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.2" + "version": "25.4.0" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,35 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +238,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +254,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +270,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +286,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +302,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +318,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +334,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +350,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +366,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +382,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +398,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +414,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +430,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.2", + "version": "25.4.0", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, @@ -414,6 +442,38 @@ "osxuniversal" ], "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "25.4.0", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "25.4.0", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" } ] } \ No newline at end of file