From 960b9f5dd4750a4aca5d905ae9133138906e4c74 Mon Sep 17 00:00:00 2001 From: mimigamin Date: Tue, 10 Mar 2026 21:06:43 -0600 Subject: [PATCH] Test offset --- .../java/frc4388/robot/RobotContainer.java | 9 +++++++ .../robot/constants/BuildConstants.java | 12 +++++----- .../robot/subsystems/shooter/Shooter.java | 10 ++++++-- .../robot/subsystems/shooter/ShooterIO.java | 1 + .../robot/subsystems/shooter/ShooterReal.java | 1 + .../robot/subsystems/swerve/SwerveDrive.java | 24 +++++++++++++++++++ 6 files changed, 49 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index dd76e20..697bfb7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -251,6 +251,15 @@ public class RobotContainer { ShooterConstants.AIM_LEAD_TIME.get() ); }, m_robotSwerveDrive) + // () -> { + // m_robotSwerveDrive.driveFacingVelocity( + // getDeadbandedDriverController().getLeft(), + // FieldPositions.HUB_POSITION, + // ShooterConstants.AIM_LEAD_TIME.get(), + // m_robotShooter.getBallVelocity(), + // m_robotShooter.getDistanceToHub() + // ); + // }, m_robotSwerveDrive) ); // D-PAD fine alignment diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 9ff0c32..c801a29 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 147; - public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6"; - public static final String GIT_DATE = "2026-03-07 20:08:15 MST"; - public static final String GIT_BRANCH = "PikesPeak"; - public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT"; - public static final long BUILD_UNIX_TIME = 1773001639109L; + public static final int GIT_REVISION = 149; + public static final String GIT_SHA = "50b8312c48be14a559876064cdddbcd27bcb8ef7"; + public static final String GIT_DATE = "2026-03-10 08:39:05 MDT"; + public static final String GIT_BRANCH = "MiraOrg"; + public static final String BUILD_DATE = "2026-03-10 21:05:03 MDT"; + public static final long BUILD_UNIX_TIME = 1773198303540L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index cb46a97..ed625f9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -31,6 +31,7 @@ public class Shooter extends SubsystemBase { // Supplier m_swervePoseSupplier; public boolean badShooterVelocity; + public double distanceToHub = 5; public Shooter( @@ -76,7 +77,9 @@ public class Shooter extends SubsystemBase { this.mode = ShooterMode.Idle; } - + public double getDistanceToHub(){ + return distanceToHub; + } public void allowShooting() { shooterButtonReady = true; @@ -86,6 +89,9 @@ public class Shooter extends SubsystemBase { shooterButtonReady = false; } + public AngularVelocity getBallVelocity() { + return RotationsPerSecond.of((state.motor1Velocity.in(RotationsPerSecond) + state.indexerVelocity.in(RotationsPerSecond))); + } @AutoLogOutput public ShooterMode getMode() { @@ -126,7 +132,7 @@ public class Shooter extends SubsystemBase { Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation()); // Get the robot's aim distance to hub - double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); + distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm()); //Center of hub to cameras in inches Logger.recordOutput("Hub Dist", distanceToHub); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index c7d3369..9b32782 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -26,6 +26,7 @@ public interface ShooterIO { AngularVelocity motor1Velocity = RotationsPerSecond.of(0); AngularVelocity motor2Velocity = RotationsPerSecond.of(0); + AngularVelocity indexerVelocity = RotationsPerSecond.of(0); double indexerOutput = 0; Current motor1Current = Amps.of(0); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 6ca3ce4..55a75d9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -95,6 +95,7 @@ public class ShooterReal implements ShooterIO { state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().div(ShooterConstants.SHOOTERMOTOR_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); state.indexerOutput = m_indexerMotor.get(); // state.indexerOutput = m_indexerMotor.getVelocity().getValue().div(ShooterConstants.INDEXER_GEAR_RATIO); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java index 6065683..fa4a750 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java @@ -5,6 +5,7 @@ package frc4388.robot.subsystems.swerve; import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; import java.util.function.Supplier; @@ -23,6 +24,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -377,6 +379,28 @@ public class SwerveDrive extends SubsystemBase implements Queryable { driveFieldAngle(leftStick, ang); } + + public void driveFacingVelocity(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime, AngularVelocity ballVelocity, double distanceToHub) { + + Translation2d robotSpeed = new Translation2d( + chassisSpeeds.vxMetersPerSecond, + chassisSpeeds.vyMetersPerSecond + ); + + if (ballVelocity.in(RotationsPerSecond) > 1E-3 && chassisSpeeds.vyMetersPerSecond > 1E-3){ + double aimOffset = chassisSpeeds.vyMetersPerSecond*distanceToHub/(ballVelocity.in(RotationsPerSecond)); + fieldPos = new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset); + Logger.recordOutput("Aim Offset", aimOffset); + } + + Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos); + + Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle(); + + + driveFieldAngle(leftStick, ang); + } + public Pose2d getCurrentPose(){ return state.currentPose; }