From ee57fb8661bff19fce1ab123d198cd6e9028303a Mon Sep 17 00:00:00 2001 From: mixxlto Date: Thu, 5 Feb 2026 16:49:51 -0800 Subject: [PATCH] fdsa --- .../robot/commands/gpm/AutoShootCommand.java | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index 4e83ba1..dca8e46 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -1,11 +1,14 @@ package frc.robot.commands.gpm; +import java.util.Optional; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.util.Units; @@ -15,15 +18,22 @@ import frc.robot.constants.FieldConstants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.hood.Hood; import frc.robot.subsystems.hood.HoodConstants; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.turret.ShotInterpolation; import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.turret.TurretConstants; +import frc.robot.util.ShooterPhysics; +import frc.robot.util.ShooterPhysics.Constraints; +import frc.robot.util.ShooterPhysics.TurretState; public class AutoShootCommand extends Command { private Turret turret; private Drivetrain drivetrain; private Hood hood; + //TODO: find maximum interpolation + private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); + private double turretSetpoint; private double hoodSetpoint; @@ -40,6 +50,8 @@ public class AutoShootCommand extends Command { private double hoodAngle; private double hoodVelocity; + private TurretState goalState; + private final double phaseDelay = 0.03; public AutoShootCommand(Turret turret, Drivetrain drivetrain, Hood hood) { @@ -79,6 +91,11 @@ public class AutoShootCommand extends Command { // Loop (20) until lookahreadTurretToTargetDistance converges for (int i = 0; i < 20; i++) { + goalState = ShooterPhysics.getShotParams( + new Translation2d(turretVelocityX, turretVelocityY), + new Translation3d(target.minus(lookaheadPose.getTranslation())), + 8.0); + timeOfFlight = ShotInterpolation.timeOfFlightMap.get(lookaheadTurretToTargetDistance); double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; @@ -117,14 +134,6 @@ public class AutoShootCommand extends Command { robotRelVel.omegaRadiansPerSecond * phaseDelay)); } - @Override - public void initialize() { - updateDrivePose(); - updateSetpoints(drivepose); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); - hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); - } - @Override public void execute() { updateDrivePose(); -- 2.39.5