From bcd9ca6504bfd32603f791b8e59209eae97613d8 Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Fri, 13 Feb 2026 17:17:58 -0800 Subject: [PATCH] a --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../robot/commands/gpm/TurretAutoShoot.java | 32 +++++++++++-------- .../frc/robot/constants/FieldConstants.java | 2 +- .../controls/PS5ControllerDriverConfig.java | 11 +++++++ .../frc/robot/subsystems/shooter/Shooter.java | 2 +- 5 files changed, 33 insertions(+), 16 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 575a425..68afc6f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,7 +86,7 @@ public class RobotContainer { case Vertigo: vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS); turret = new Turret(); - shooter = new Shooter(); + //shooter = new Shooter(); drive = new Drivetrain(vision, new GyroIOPigeon2()); driver = new PS5ControllerDriverConfig(drive, shooter, turret); diff --git a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java index 8864ab5..34cf371 100644 --- a/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java +++ b/src/main/java/frc/robot/commands/gpm/TurretAutoShoot.java @@ -1,5 +1,7 @@ package frc.robot.commands.gpm; +import org.littletonrobotics.junction.Logger; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.filter.LinearFilter; import edu.wpi.first.math.geometry.Pose2d; @@ -44,9 +46,10 @@ public class TurretAutoShoot extends Command { public void updateTurretSetpoint(Pose2d drivepose) { Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); - Pose2d turretPosition = drivepose.transformBy(new Transform2d((TurretConstants.DISTANCE_FROM_ROBOT_CENTER), new Rotation2d())); - double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); - + Pose2d turretPosition = drivepose;//.transformBy(new Transform2d((TurretConstants.DISTANCE_FROM_ROBOT_CENTER), new Rotation2d())); + //double turretToTargetDistance = target.getDistance(turretPosition.getTranslation()); + Logger.recordOutput("brrr", turretPosition); + // If the robot is moving, adjust the target position based on velocity ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); @@ -69,7 +72,7 @@ public class TurretAutoShoot extends Command { // Loop (20) until lookahreadTurretToTargetDistance converges //for (int i = 0; i < 20; i++) { - timeOfFlight = 1.0; + timeOfFlight = 0.0; double offsetX = turretVelocityX * timeOfFlight; double offsetY = turretVelocityY * timeOfFlight; lookaheadPose = @@ -78,7 +81,10 @@ public class TurretAutoShoot extends Command { turretPosition.getRotation()); //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation()); //} - turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); + turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();//.minus(new Rotation2d(Math.PI/4)); + Logger.recordOutput("lookahead pose", lookaheadPose); + Logger.recordOutput("target pose", target); + System.out.println(turretAngle); if (lastTurretAngle == null) { lastTurretAngle = turretAngle; } @@ -87,25 +93,25 @@ public class TurretAutoShoot extends Command { turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); lastTurretAngle = turretAngle; - // Add 180 since drivetrain is backwards - double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() + Math.PI); + double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint), -180.0, 180.0); } public void updateDrivePose(){ ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - drivepose = drivetrain.getPose().exp( - new Twist2d( - robotRelVel.vxMetersPerSecond * phaseDelay, - robotRelVel.vyMetersPerSecond * phaseDelay, - robotRelVel.omegaRadiansPerSecond * phaseDelay)); + drivepose = drivetrain.getPose().rotateBy(new Rotation2d(Math.PI)); + // .exp( + // new Twist2d( + // robotRelVel.vxMetersPerSecond * phaseDelay, + // robotRelVel.vyMetersPerSecond * phaseDelay, + // robotRelVel.omegaRadiansPerSecond * phaseDelay)); } @Override public void execute() { updateDrivePose(); updateTurretSetpoint(drivepose); - //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); System.out.println("Turret Setpoint: " + turretSetpoint); //System.out.println("Turret goal velocity" + (turretVelocity - drivetrain.getAngularRate(2))); } diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 5ec836d..7818710 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -27,7 +27,7 @@ public class FieldConstants { /** Location of hub target */ public static final Translation3d HUB_BLUE = - new Translation3d(Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72)); + new Translation3d(Units.inchesToMeters(156.8), 4.035, Units.inchesToMeters(72)); // TODO: Update all of this public static final Translation3d NEUTRAL_LEFT = diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 37cd9d3..373aefc 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -91,6 +91,17 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { }) ); + driver.get(PS5Button.TRIANGLE).onTrue( + new InstantCommand(()->{ + if (turretAutoShoot != null && turretAutoShoot.isScheduled()){ + turretAutoShoot.cancel(); + } else{ + turretAutoShoot = new TurretAutoShoot(turret, getDrivetrain()); + CommandScheduler.getInstance().schedule(turretAutoShoot); + } + }) + ); + driver.get(PS5Button.LB).onTrue(new InstantCommand(() -> shooter.setFeeder(1))).onFalse( new InstantCommand(()->{ shooter.setFeeder(0); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 11f3b96..38b55a5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -102,7 +102,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { } public void periodic() { - runVelocity(Units.rotationsToRadians(shooterTargetSpeed)); + //runVelocity(Units.rotationsToRadians(shooterTargetSpeed)); SmartDashboard.putNumber("Shot Power", shooterPower); shooterPower = SmartDashboard.getNumber("Shot Power", shooterPower); -- 2.39.5