From af6cbdadd3f49702310db9e0ad489498e8e668ec Mon Sep 17 00:00:00 2001 From: maxwtan <100314265+MaxwellTTan20@users.noreply.github.com> Date: Sat, 14 Feb 2026 12:43:41 -0800 Subject: [PATCH] a --- src/main/java/frc/robot/RobotContainer.java | 4 ++-- .../java/frc/robot/commands/gpm/AutoShootCommand.java | 10 ++++++---- src/main/java/frc/robot/constants/FieldConstants.java | 2 +- .../frc/robot/controls/PS5ControllerDriverConfig.java | 6 ++++-- .../java/frc/robot/subsystems/shooter/Shooter.java | 4 ++-- src/main/java/frc/robot/subsystems/turret/Turret.java | 5 +++-- 6 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d6400f2..6dbc065 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -86,8 +86,8 @@ public class RobotContainer { case WaffleHouse: // AKA Betabot turret = new Turret(); - shooter = new Shooter(); - hood = new Hood(); + //shooter = new Shooter(); + //hood = new Hood(); case SwerveCompetition: // AKA "Vantage" diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java index db08005..6117761 100644 --- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java @@ -97,7 +97,7 @@ public class AutoShootCommand extends Command { FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), 8.0); // Random initial goalState to prevent it being null - addRequirements(turret, hood, shooter); + addRequirements(turret); } public void updateSetpoints(Pose2d drivepose) { @@ -134,7 +134,7 @@ public class AutoShootCommand extends Command { Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting goalState = ShooterPhysics.getShotParams( target3d.minus(lookahead3d), - 8.0); + 2.0); timeOfFlight = goalState.timeOfFlight(); double offsetX = turretVelocityX * timeOfFlight; @@ -198,12 +198,13 @@ public class AutoShootCommand extends Command { public void execute() { updateDrivePose(); updateSetpoints(drivepose); - //turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); + turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2)); //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity); //shooter.setShooter(goalState.exitVel()); SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint); SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint); + SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel()); System.out.println("COMMAND IS WORKINNGGG"); /** Spindexer Stuff!! */ @@ -216,7 +217,8 @@ public class AutoShootCommand extends Command { public void end(boolean interrupted) { // Set the turret to a safe position when the command ends turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); - hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0); + //shooter.setShooter(0.0); + //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0); if(spindexer != null){ spindexer.stopSpindexer(); } diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index d2ad566..33cdcff 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -31,7 +31,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 + 20), 4.035, Units.inchesToMeters(72)); public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(156.8 + 20), 4.035 + .67, Units.inchesToMeters(72)); diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index c707453..e0631aa 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -14,6 +14,7 @@ import frc.robot.commands.gpm.AutoShootCommand; import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.spindexer.Spindexer; import frc.robot.subsystems.turret.Turret; import frc.robot.subsystems.hood.Hood; @@ -86,7 +87,7 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { } // Auto shoot - if(turret != null && hood != null){ + if(turret != null){ driver.get(PS5Button.SQUARE).onTrue( new InstantCommand(()->{ if (autoShoot != null && autoShoot.isScheduled()){ @@ -96,9 +97,10 @@ public class PS5ControllerDriverConfig extends BaseDriverConfig { CommandScheduler.getInstance().schedule(autoShoot); } }) - ); + ); } + } @Override diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 1af0998..defdefe 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -48,7 +48,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { public Shooter(){ TalonFXConfiguration config = new TalonFXConfiguration(); - config.Slot0.kP = 0.15; + config.Slot0.kP = 2.0; config.Slot0.kI = 0; config.Slot0.kD = 0.03; config.Slot0.kV = 0.20; @@ -114,7 +114,7 @@ public class Shooter extends SubsystemBase implements ShooterIO { public void updateInputs() { inputs.leftShooterVelocity = Units.rotationsToRadians(motorLeft.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; - inputs.rightShooterVelocity =Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; + inputs.rightShooterVelocity = Units.rotationsToRadians(motorRight.getVelocity().getValueAsDouble()) * ShooterConstants.SHOOTER_LAUNCH_DIAMETER/2; inputs.leftShooterCurrent = motorLeft.getStatorCurrent().getValueAsDouble(); inputs.rightShooterCurrent = motorRight.getStatorCurrent().getValueAsDouble(); } diff --git a/src/main/java/frc/robot/subsystems/turret/Turret.java b/src/main/java/frc/robot/subsystems/turret/Turret.java index 4d0d371..51783d7 100644 --- a/src/main/java/frc/robot/subsystems/turret/Turret.java +++ b/src/main/java/frc/robot/subsystems/turret/Turret.java @@ -52,9 +52,10 @@ public class Turret extends SubsystemBase implements TurretIO{ private static final double MIN_ANGLE_RAD = Units.degreesToRadians(-180); // Change this later to the actual values private static final double MAX_ANGLE_RAD = Units.degreesToRadians(180); - private static final double MAX_VEL_RAD_PER_SEC = 25; + private static final double MAX_VEL_RAD_PER_SEC = 15; //private static final double MAX_VEL_RAD_PER_SEC = 3.0; // Starting super duper slow for now - private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0; + // private static final double MAX_ACCEL_RAD_PER_SEC2 = 160.0; + private static final double MAX_ACCEL_RAD_PER_SEC2 = 60.0; private static final double GEAR_RATIO = TurretConstants.TURRET_GEAR_RATIO; -- 2.39.5