From 2ada8c9408dc91b0aa63c225f7f91cedb73bc5d6 Mon Sep 17 00:00:00 2001 From: Team578 Driver Station Date: Mon, 13 Apr 2026 20:46:10 -0400 Subject: [PATCH] Fixed turret auto aim --- src/main/java/frc/robot/commands/AutoTurretCommand.java | 2 +- src/main/java/frc/robot/subsystems/TurretSubsystem.java | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/commands/AutoTurretCommand.java b/src/main/java/frc/robot/commands/AutoTurretCommand.java index 3054f32..7ff09b5 100644 --- a/src/main/java/frc/robot/commands/AutoTurretCommand.java +++ b/src/main/java/frc/robot/commands/AutoTurretCommand.java @@ -62,7 +62,7 @@ public class AutoTurretCommand extends Command { Pose3d botPose = driveSubsystem.getBotPose(); Pose3d turretPose = botPose.plus(new Transform3d(new Translation3d(CENTER_TURRET_TO_ROBOT), - new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative())))); + new Rotation3d(Rotation2d.fromRotations(turretSubsystem.getTurretAngleRobotRelative() + 0.5)))); Logger.recordOutput("AutoTurret-TurretPose", turretPose); Translation2d targetTranslation = scoringHubPose.getTranslation().toTranslation2d().minus(turretPose.getTranslation().toTranslation2d()); diff --git a/src/main/java/frc/robot/subsystems/TurretSubsystem.java b/src/main/java/frc/robot/subsystems/TurretSubsystem.java index 3970ecb..c4a64e3 100644 --- a/src/main/java/frc/robot/subsystems/TurretSubsystem.java +++ b/src/main/java/frc/robot/subsystems/TurretSubsystem.java @@ -7,6 +7,7 @@ package frc.robot.subsystems; import org.fairportrobotics.frc.posty.TestableSubsystem; import org.fairportrobotics.frc.posty.test.PostTest; import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.ConsoleSource.RoboRIO; import static org.fairportrobotics.frc.posty.assertions.Assertions.*; @@ -247,9 +248,11 @@ public class TurretSubsystem extends TestableSubsystem { double robotRotations = robotPose.getRotation().toRotation2d().getRotations(); - Angle turretAngle = Angle.ofRelativeUnits(robotRotations, Units.Rotations).plus(Angle.ofRelativeUnits(correctedPos, Units.Rotations)); + Rotation2d turretAngle = Rotation2d.fromRotations(correctedPos).plus(Rotation2d.fromRotations(robotRotations)).minus(Rotation2d.fromRotations(0.5)); - setTurretMotorRotation((turretAngle.in(Units.Rotation) * Constants.ShooterConstants.TURRET_GEAR_RATIO)); +Logger.recordOutput("TurretSubsystem-TurretFieldTarget", turretAngle.getRotations()); + + setTurretRobotRelative(((turretAngle.getRotations()))); } public void setTurretRobotRelative(double pos)