From 130967b5f5a3ee6427d95e36bfe145ff92f2694b Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Thu, 12 Mar 2026 20:16:42 -0700 Subject: [PATCH] Update Superstructure.java --- .../frc/robot/commands/gpm/Superstructure.java | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java index 5c48348..7a35cf6 100644 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ b/src/main/java/frc/robot/commands/gpm/Superstructure.java @@ -93,17 +93,11 @@ public class Superstructure extends Command { // Rotational adjustment is not being used, since turret is in center of robot double turretVelocityX = - fieldRelVel.vxMetersPerSecond - + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians())); + fieldRelVel.vxMetersPerSecond; double turretVelocityY = - fieldRelVel.vyMetersPerSecond - + fieldRelVel.omegaRadiansPerSecond - * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians()) - - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians())); + fieldRelVel.vyMetersPerSecond; - double timeOfFlight; + double timeOfFlight = 0; Pose2d lookaheadPose = turretPosition; /* @@ -146,6 +140,10 @@ public class Superstructure extends Command { lastTurretAngle = turretAngle; Logger.recordOutput("Turret/Target Pose", target); + SmartDashboard.putNumber("Time of flight", timeOfFlight); + SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); + SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); + Logger.recordOutput("Lookahead Pose", lookaheadPose); -- 2.39.5