From a92ea628505ce3666865a9eea2fa0b7bcfc15548 Mon Sep 17 00:00:00 2001 From: Maciej Wiczynski Date: Thu, 28 Mar 2024 07:44:00 -0400 Subject: [PATCH] debug changes --- .../commands/drive/DriveTranslateInX.java | 28 +++++++++++++------ 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/drive/DriveTranslateInX.java b/src/main/java/frc/robot/commands/drive/DriveTranslateInX.java index d8e33c91..031cccbe 100644 --- a/src/main/java/frc/robot/commands/drive/DriveTranslateInX.java +++ b/src/main/java/frc/robot/commands/drive/DriveTranslateInX.java @@ -9,8 +9,12 @@ import frc.robot.Constants; import frc.robot.config.RobotConfig.DriveConstants; import frc.robot.subsystems.drive.Drive; +import frc.robot.util.DevilBotState; + import java.util.function.DoubleSupplier; +import org.littletonrobotics.junction.Logger; + public class DriveTranslateInX extends Command { Drive drive; DoubleSupplier xMeters; @@ -25,15 +29,16 @@ public DriveTranslateInX(Drive drive, DoubleSupplier xMeters) { this.xMeters = xMeters; xTranslatePID.setTolerance(DriveConstants.translatePidErrorInMeters); - addRequirements((Subsystem) drive); + //addRequirements((Subsystem) drive); } @Override public void initialize() { xTargetMeters = drive.getPose().getX() + xMeters.getAsDouble(); - SmartDashboard.putNumber("DriveTranslateInX/xMeters", xMeters.getAsDouble()); - SmartDashboard.putNumber("DriveTranslateInX/xTargetMeters", xTargetMeters); + Logger.recordOutput("DriveTranslateInX/xMeters", xMeters.getAsDouble()); + Logger.recordOutput("DriveTranslateInX/xTargetMeters", xTargetMeters); + Logger.recordOutput("DriveTranslateInX/running", true); xTranslatePID.reset(); xTranslatePID.setSetpoint(xTargetMeters); @@ -51,11 +56,17 @@ public void initialize() { @Override public void execute() { - double translate = xTranslatePID.calculate(drive.getPose().getX()); - ChassisSpeeds speeds = new ChassisSpeeds(translate, 0, 0); - SmartDashboard.putNumber("DriveTranslateInX/speeds", speeds.vxMetersPerSecond); - SmartDashboard.putBoolean("DriveTranslateInX/atSetpoint", xTranslatePID.atSetpoint()); - SmartDashboard.putNumber("DriveTranslateInX/xTargetMeters", xTargetMeters); + double xOutput = xTranslatePID.calculate(drive.getPose().getX()); + ChassisSpeeds speeds = new ChassisSpeeds(xOutput, 0, 0); + + Logger.recordOutput("DriveTranslateInX/xPose", drive.getPose().getX()); + Logger.recordOutput("DriveTranslateInX/xOutput", xOutput); + Logger.recordOutput("DriveTranslateInX/xSpeeds", speeds.vxMetersPerSecond); + Logger.recordOutput("DriveTranslateInX/xTargetMeters", xTargetMeters); + Logger.recordOutput("DriveTranslateInX/atSetpoint", xTranslatePID.atSetpoint()); + + DevilBotState.getVisionRobotXToAmpTarget(); + // drive.runVelocity(speeds); } @@ -77,5 +88,6 @@ public void end(boolean interrupted) { if (Constants.debugCommands) { System.out.println(" END: " + this.getClass().getSimpleName()); } + Logger.recordOutput("DriveTranslateInX/running", false); } }