diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index 7fcdf984..22590290 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -7,6 +7,7 @@ import frc.robot.constants.Constants; import frc.robot.subsystems.SwerveDrivetrain; import frc.robot.subsystems.Vision; +import frc.robot.utils.logging.Logger; public class MoveToGamepiece extends Command { private final SwerveDrivetrain drivetrain; @@ -45,10 +46,20 @@ public void execute() { } yChange = gamePieceY - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; double xChange = gamePieceX - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; + Logger.logDouble("/Vision/YError", yChange, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/XError", xChange, Constants.ENABLE_LOGGING); if (pieceNotSeenCounter < Constants.LIMELIGHT_PIECE_NOT_SEEN_COUNT && yChange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD){ - ChassisSpeeds driveStates = new ChassisSpeeds(movingPIDController.calculate(yChange), 0, turningPIDController.calculate(xChange)); + double forwardSpeed = movingPIDController.calculate(yChange); + double turningSpeed = movingPIDController.calculate(xChange); + Logger.logDouble("/Vision/" + getName() + "/ForwardSpeed", forwardSpeed, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/" + getName() + "/TurningSpeed", turningSpeed, Constants.ENABLE_LOGGING); + ChassisSpeeds driveStates = new ChassisSpeeds(forwardSpeed, 0, turningSpeed); drivetrain.drive(driveStates); } + else { + Logger.logDouble("/Vision/" + getName() + "/ForwardSpeed", 0.0, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/" + getName() + "/TurningSpeed", 0.0, Constants.ENABLE_LOGGING); + } } @Override @@ -58,6 +69,8 @@ public boolean isFinished() { @Override public void end(boolean interrupted) { + Logger.logDouble("/MoveToGamePiece/ForwardSpeed", 0, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/turningSpeed", 0, Constants.ENABLE_LOGGING); drivetrain.drive(new ChassisSpeeds(0, 0, 0)); } }