From 8828f51e95e62348b706e32eb5a21081d5cf0283 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 20 Mar 2024 19:01:04 -0400 Subject: [PATCH 1/4] Added Logging for MoveToGamepiece --- .../java/frc/robot/commands/MoveToGamepiece.java | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index ca222887..282b3dcd 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 SwerveDrivetrain drivetrain; @@ -36,11 +37,17 @@ public void initialize() { @Override public void execute() { + Logger.logDouble("/Vision/XAngle", vision.getPieceOffestAngleX(), Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/YAngle", vision.getPieceOffestAngleY(), Constants.ENABLE_LOGGING); ychange = vision.getPieceOffestAngleY() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; xchange = vision.getPieceOffestAngleX() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; if (vision.isPieceSeen() && (ychange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD)) { - driveStates = new ChassisSpeeds(movingPIDController.calculate(ychange), 0, turningPIDController.calculate(xchange)); - drivetrain.drive(driveStates); + double forwardSpeed = movingPIDController.calculate(ychange); + double turningSpeed = turningPIDController.calculate(xchange); + Logger.logDouble("/MoveToGamePiece/ForwardSpeed", forwardSpeed, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/turningSpeed", turningSpeed, Constants.ENABLE_LOGGING); + driveStates = new ChassisSpeeds(forwardSpeed, 0, turningSpeed); + drivetrain.drive(driveStates); } } @@ -51,6 +58,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)); } } From e0f9368b4b14e477aa2cc96bb27dc9e2a795a519 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 20 Mar 2024 19:03:28 -0400 Subject: [PATCH 2/4] Renamed them --- src/main/java/frc/robot/commands/MoveToGamepiece.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index 282b3dcd..4955dba2 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -44,8 +44,8 @@ public void execute() { if (vision.isPieceSeen() && (ychange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD)) { double forwardSpeed = movingPIDController.calculate(ychange); double turningSpeed = turningPIDController.calculate(xchange); - Logger.logDouble("/MoveToGamePiece/ForwardSpeed", forwardSpeed, Constants.ENABLE_LOGGING); - Logger.logDouble("/Vision/turningSpeed", turningSpeed, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/" + getName() + "/ForwardSpeed", forwardSpeed, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/" + getName() + "/turningSpeed", turningSpeed, Constants.ENABLE_LOGGING); driveStates = new ChassisSpeeds(forwardSpeed, 0, turningSpeed); drivetrain.drive(driveStates); } From 0f7695fbe0e1223683b496456d602c7139469e16 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 20 Mar 2024 20:04:04 -0400 Subject: [PATCH 3/4] Logging error instead of actual x and y --- src/main/java/frc/robot/commands/MoveToGamepiece.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index 4955dba2..47d6d2b5 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -37,10 +37,10 @@ public void initialize() { @Override public void execute() { - Logger.logDouble("/Vision/XAngle", vision.getPieceOffestAngleX(), Constants.ENABLE_LOGGING); - Logger.logDouble("/Vision/YAngle", vision.getPieceOffestAngleY(), Constants.ENABLE_LOGGING); ychange = vision.getPieceOffestAngleY() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_Y; xchange = vision.getPieceOffestAngleX() - Constants.LIMELIGHT_MOVE_TO_PIECE_DESIRED_X; + Logger.logDouble("/Vision/XError", xchange, Constants.ENABLE_LOGGING); + Logger.logDouble("/Vision/YError", ychange, Constants.ENABLE_LOGGING); if (vision.isPieceSeen() && (ychange > Constants.MOVE_TO_GAMEPIECE_THRESHOLD)) { double forwardSpeed = movingPIDController.calculate(ychange); double turningSpeed = turningPIDController.calculate(xchange); From c02abcfc43ab0116abb4e017a70a5725fd1036e3 Mon Sep 17 00:00:00 2001 From: Aditya Hebbar Date: Wed, 27 Mar 2024 19:09:42 -0400 Subject: [PATCH 4/4] Works --- .../java/frc/robot/commands/MoveToGamepiece.java | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/commands/MoveToGamepiece.java b/src/main/java/frc/robot/commands/MoveToGamepiece.java index 96809c72..22590290 100644 --- a/src/main/java/frc/robot/commands/MoveToGamepiece.java +++ b/src/main/java/frc/robot/commands/MoveToGamepiece.java @@ -46,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