From 011f77d769b1f478bb90da15807a47cdf8e18b67 Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Sun, 15 Feb 2026 14:05:42 -0800 Subject: [PATCH 01/12] Bangbang uhh change motor ids --- src/main/java/frc/robot/Constants.java | 5 +- src/main/java/frc/robot/FieldConstants.java | 25 -- .../frc/robot/handlers/ShooterHandler.java | 1 + .../frc/robot/handlers/Superstructure.java | 2 +- .../robot/subsystems/ShooterSubsystem.java | 230 +++++------------- 5 files changed, 71 insertions(+), 192 deletions(-) delete mode 100644 src/main/java/frc/robot/FieldConstants.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d4caa5c..b2b7749 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,11 +23,12 @@ public static abstract class ShooterConstants { public static final int ShooterFollowerID = 21; public static final int ShooterFollowerReversedID = 22; public static final int ShooterFollowerReversed2ID = 23; - public static final AngularVelocity FastShot = RPM.of(3000); //HESHEL CHANGE ME FAST - public static final AngularVelocity SlowShot = RPM.of(1500); //HESKEL CHANGE ME SLOW + public static final AngularVelocity FastShot = RPM.of(5000); //HESHEL CHANGE ME FAST + public static final AngularVelocity SlowShot = RPM.of(3500); //HESKEL CHANGE ME SLOW public static final AngularVelocity SetRPMHardStop = RPM.of(3200); public static double ShootingDistance = Feet.of(6).in(Meter); //6ft starting public static int RPMTolarance = 50; + } public static abstract class IntakeConstants { diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java deleted file mode 100644 index 99ef85e..0000000 --- a/src/main/java/frc/robot/FieldConstants.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot; - -import com.fasterxml.jackson.databind.ObjectMapper; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.Filesystem; -import java.io.IOException; -import java.nio.file.Path; - -public class FieldConstants { -public static class Tower { - public static final double width = Units.inchesToMeters(49.25); - public static final double depth = Units.inchesToMeters(45.0); - public static final double height = Units.inchesToMeters(78.25); - public static final double innerOpeningWidth = Units.inchesToMeters(32.250); - public static final double frontFaceX = Units.inchesToMeters(43.51); - - - } - - -} diff --git a/src/main/java/frc/robot/handlers/ShooterHandler.java b/src/main/java/frc/robot/handlers/ShooterHandler.java index 6e22125..46f826b 100644 --- a/src/main/java/frc/robot/handlers/ShooterHandler.java +++ b/src/main/java/frc/robot/handlers/ShooterHandler.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Joystick; diff --git a/src/main/java/frc/robot/handlers/Superstructure.java b/src/main/java/frc/robot/handlers/Superstructure.java index d910936..080fb88 100644 --- a/src/main/java/frc/robot/handlers/Superstructure.java +++ b/src/main/java/frc/robot/handlers/Superstructure.java @@ -240,7 +240,7 @@ public void periodic() { }*/ SmartDashboard.putBoolean("shooterAtVelo?", shooter.isAtTargetVelo()); - SmartDashboard.putBoolean("Drivetrain aimed?",drivetrain.isAimedAtTarget()); + SmartDashboard.putBoolean("Drivetrain aimed?", drivetrain.isAimedAtTarget()); } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 308b9fd..6813ace 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -20,18 +20,7 @@ import frc.robot.Constants; import frc.robot.handlers.IntakeHandler; import frc.robot.handlers.Superstructure.SuperstructureState; -import yams.gearing.GearBox; -import yams.gearing.MechanismGearing; -import yams.mechanisms.SmartMechanism; -import yams.motorcontrollers.SmartMotorControllerConfig; import edu.wpi.first.units.measure.AngularVelocity; -import yams.motorcontrollers.SmartMotorController; -import yams.mechanisms.config.FlyWheelConfig; -import yams.mechanisms.velocity.FlyWheel; -import yams.motorcontrollers.SmartMotorControllerConfig.ControlMode; -import yams.motorcontrollers.SmartMotorControllerConfig.MotorMode; -import yams.motorcontrollers.SmartMotorControllerConfig.TelemetryVerbosity; -import yams.motorcontrollers.remote.TalonFXWrapper; import com.ctre.phoenix6.sim.TalonFXSimState; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.controls.Follower; @@ -44,119 +33,26 @@ public class ShooterSubsystem extends SubsystemBase { - - private SmartMotorControllerConfig smcConfig = new SmartMotorControllerConfig(this) - .withControlMode(ControlMode.CLOSED_LOOP) - // Feedback Constants (PID Constants) - .withClosedLoopController(0.48, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(45)) - .withSimClosedLoopController(0.48, 0, 0, DegreesPerSecond.of(180), DegreesPerSecondPerSecond.of(45)) - // Feedforward Constants - .withFeedforward(new SimpleMotorFeedforward(0, 0.19, 1.13)) - .withSimFeedforward(new SimpleMotorFeedforward(0, 0.19, 1.13)) - // Telemetry name and verbosity level - .withTelemetry("ShooterMotor", TelemetryVerbosity.HIGH) - // Gearing from the motor rotor to final shaft. - // In this example GearBox.fromReductionStages(3,4) is the same as GearBox.fromStages("3:1","4:1") which corresponds to the gearbox attached to your motor. - // You could also use .withGearing(12) which does the same thing. - .withGearing(new MechanismGearing(GearBox.fromReductionStages(1))) - // Motor properties to prevent over currenting. - .withMotorInverted(false) - .withIdleMode(MotorMode.COAST) - .withStatorCurrentLimit(Amps.of(40)); - - // Vendor motor controller object + // motors private TalonFX shooterLeader = new TalonFX(Constants.ShooterConstants.ShooterLeaderID); - private TalonFX shooterFollower = new TalonFX(Constants.ShooterConstants.ShooterFollowerID); - private TalonFX shooterFollowerReversed = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversedID); - private TalonFX shooterFollowerReversed2 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversed2ID); + private TalonFX shooterFollower1 = new TalonFX(Constants.ShooterConstants.ShooterFollowerID); + private TalonFX shooterFollower2 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversedID); + private TalonFX shooterFollower3 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversed2ID); private TalonFXSimState shooterMotorSim = shooterLeader.getSimState(); - // Create our SmartMotorController from our Spark and config with the NEO. - private SmartMotorController SmartMotorController = new TalonFXWrapper(shooterLeader, DCMotor.getKrakenX60(4), smcConfig); - - - private final FlyWheelConfig shooterConfig = new FlyWheelConfig(SmartMotorController) - // Diameter of the flywheel. - .withDiameter(Inches.of(4)) - // Mass of the flywheel. - .withMass(Pounds.of(5.2)) //CHangle later maybe - // Maximum speed of the shooter. - .withUpperSoftLimit(RPM.of(3200)) - // Telemetry name and verbosity for the arm. - .withTelemetry("ShooterMech", TelemetryVerbosity.HIGH); - - // Shooter Mechanism - private FlyWheel shooter = new FlyWheel(shooterConfig); - + private double f = 0.4; AngularVelocity setVelo = RPM.of(0); - /** - * Gets the current velocity of the shooter. - * - * @return Shooter velocity. - */ - public AngularVelocity getVelocity() { - return shooter.getSpeed();} - - public Command stop() { - setVelo = RPM.of(0); - return shooter.set(0);} - - /** - * Set the shooter velocity WITHOUT creating a command. - * Use this for continuous updates (like distance-based shooting). - * - * @param speed Speed to set. - */ - public void setVelocityDirect(AngularVelocity speed) { - if (speed.gte(Constants.ShooterConstants.SetRPMHardStop)) {speed = Constants.ShooterConstants.SetRPMHardStop;} - System.out.println("new speed" + speed); - setVelo = speed; - } - - /** - * Set the shooter velocity. - * - * @param speed Speed to set. - * @return {@link edu.wpi.first.wpilibj2.command.RunCommand} - */ - public Command setVelocity(AngularVelocity speed) { - if (speed.gte(Constants.ShooterConstants.SetRPMHardStop)){ speed = Constants.ShooterConstants.SetRPMHardStop;} - setVelo = speed; - return shooter.setSpeed(speed);} - - - /** - * Set the dutycycle of the shooter. - * - * @param dutyCycle DutyCycle to set. - * @return {@link edu.wpi.first.wpilibj2.command.RunCommand} - */ - public Command set(double dutyCycle) { return shooter.set(dutyCycle);} private CommandSwerveDrivetrain drivetrain; - public ShooterSubsystem() { - - //-------------COMMENT ALL THIS OUT IF TESTING WITH PHYNEX TUNER----------------- - shooterFollower.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - - // 2. Reversed Followers (Opposite direction of leader) - shooterFollowerReversed.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - shooterFollowerReversed2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); + public ShooterSubsystem(){ + shooterFollower1.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); + shooterFollower2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); + shooterFollower3.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); } - /** Creates a new ShooterSubsystem. with drivetrain perameters */ - public ShooterSubsystem(CommandSwerveDrivetrain drivetrin) { - //-------------COMMENT ALL THIS OUT IF TESTING WITH PHYNEX TUNER----------------- - shooterFollower.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - - // 2. Reversed Followers (Opposite direction of leader) - // Use 'true' for the opposeLeader parameter - shooterFollowerReversed.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - shooterFollowerReversed2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Opposed)); - } private static ShooterSubsystem instance; public static ShooterSubsystem getInstance(){ if (instance == null){ @@ -165,9 +61,42 @@ public static ShooterSubsystem getInstance(){ return instance; } - public void initialize(CommandSwerveDrivetrain drivetrain) { + public void initialize(CommandSwerveDrivetrain drivetrain) { this.drivetrain = drivetrain; + } + + // Get current velocity from the motor + public AngularVelocity getVelocity() { + // TalonFX getVelocity() returns rotations per second, convert to RPM + double rps = shooterLeader.getVelocity().getValueAsDouble(); + return RPM.of(rps * 60.0); + } + + public Command setVelocity(AngularVelocity speed) { + return Commands.runOnce(() -> { + AngularVelocity finalSpeed = speed; + if (speed.gte(Constants.ShooterConstants.SetRPMHardStop)) { + finalSpeed = Constants.ShooterConstants.SetRPMHardStop; + } + setVelo = finalSpeed; + }, this); +} + + // Stop the shooter + public Command stop() { + return Commands.runOnce(() -> { + setVelo = RPM.of(0); + shooterLeader.set(0); + }, this); +} + + public double BangBangChicken(double cur, double vel) { + if (cur < vel) { + return 1.0; + } else { + return f * vel; } + } //Checking if shooter RPM is at threashold (waiting for it to spinup) public boolean isAtTargetVelo(){ @@ -178,45 +107,26 @@ public boolean isAtTargetVelo(){ return currentRPM >= targetRPM; } - + public AngularVelocity getCalcedRPM(double DistMeters){ //double distanceMeters = drivetrain.getDistance(); double distanceInches = DistMeters * 39.3701; - - AngularVelocity velo = RPM.of(-0.0312466 * Math.pow(distanceInches, 2) + 29.07009 * distanceInches + 828.29202); - - if (velo.gte(Constants.ShooterConstants.SetRPMHardStop)){ velo = Constants.ShooterConstants.SetRPMHardStop;} - setVelo = velo; - return velo; -} - - public Command setIdleMode(Pose2d RobotPose){ - Pose2d currentPose = RobotPose; - double robotX = currentPose.getX(); - - // Define field zones (adjust these values based on your field layout) - // Assuming field is ~16.5 meters long (54 feet) - double ourZoneMax = 5.5; // meters - our third of the field - double middleZoneMax = 11.0; // meters - middle third - // Anything beyond middleZoneMax is "their" zone + AngularVelocity velo = RPM.of(-0.0312466 * Math.pow(distanceInches, 2) + 29.07009 * distanceInches + 828.29202); - // Determine which zone we're in and set appropriate state - if (robotX < ourZoneMax) { - return setVelocity(Constants.ShooterConstants.SlowShot); - } else if (robotX < middleZoneMax) { - return stop(); - } else { - return stop(); + if (velo.gte(Constants.ShooterConstants.SetRPMHardStop)) { + velo = Constants.ShooterConstants.SetRPMHardStop; } + + return velo; } -public boolean isReadyToShoot() { + public boolean isReadyToShoot() { // Check if at target velocity and drivetrain is aimed return isAtTargetVelo() && drivetrain != null && isAimedAtTarget(); -} + } -public boolean isAimedAtTarget() { + public boolean isAimedAtTarget() { if (drivetrain == null) return false; Pose2d robotPose = drivetrain.getPose(); @@ -232,39 +142,31 @@ public boolean isAimedAtTarget() { SmartDashboard.putNumber("Shooter/Aim Error (deg)", errorDegrees); return errorDegrees < 2.0; // Within 2 degrees -} - - + } @Override public void periodic() { - + double output = BangBangChicken(getVelocity().in(RPM), setVelo.in(RPM)); + shooterLeader.set(output); // Send percentage to motor (0.0 to 1.0) + SmartDashboard.putNumber("Request RPM", setVelo.in(RPM)); SmartDashboard.putNumber("Real RPM", getVelocity().in(RPM)); - - // This method will be called once per scheduler run - shooter.updateTelemetry(); + SmartDashboard.putNumber("Bang-Bang Output", output); } -@Override -public void simulationPeriodic() { + @Override + public void simulationPeriodic() { // This method will be called once per scheduler run during simulation - shooter.simIterate(); - //SmartDashboard.putNumber("Request RPM", setVelo.in(RPM)); - //SmartDashboard.putNumber("Real RPM", getVelocity().in(RPM)); - - - //System.out.println("Sim periodic running - Leader voltage: " + shooterLeader.getSimState().getMotorVoltage()); // Log voltages for all motors double leaderVoltage = shooterLeader.getSimState().getMotorVoltage(); - shooterFollower.getSimState().setSupplyVoltage(leaderVoltage); - shooterFollowerReversed.getSimState().setSupplyVoltage(leaderVoltage); - shooterFollowerReversed2.getSimState().setSupplyVoltage(leaderVoltage); + shooterFollower1.getSimState().setSupplyVoltage(leaderVoltage); + shooterFollower2.getSimState().setSupplyVoltage(leaderVoltage); + shooterFollower3.getSimState().setSupplyVoltage(leaderVoltage); SmartDashboard.putNumber("Shooter/Leader Voltage", shooterLeader.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/Follower Voltage", shooterFollower.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/FollowerReversed Voltage", shooterFollowerReversed.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/FollowerReversed2 Voltage", shooterFollowerReversed2.getSimState().getMotorVoltage()); + SmartDashboard.putNumber("Shooter/Follower Voltage", shooterFollower1.getSimState().getMotorVoltage()); + SmartDashboard.putNumber("Shooter/FollowerReversed Voltage", shooterFollower2.getSimState().getMotorVoltage()); + SmartDashboard.putNumber("Shooter/FollowerReversed2 Voltage", shooterFollower3.getSimState().getMotorVoltage()); } -} +} \ No newline at end of file From 981af14d4204b179b2466f65e20077e3683d1cf5 Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Tue, 17 Feb 2026 17:27:44 -0800 Subject: [PATCH 02/12] targets no worky but i had to go --- simgui-ds.json | 3 +- src/main/java/frc/robot/Constants.java | 27 +++++++ src/main/java/frc/robot/RobotContainer.java | 5 +- .../frc/robot/subsystems/ClimbSubsystem.java | 2 - .../subsystems/CommandSwerveDrivetrain.java | 72 +++++++++++++------ .../robot/subsystems/ShooterSubsystem.java | 21 +++--- 6 files changed, 91 insertions(+), 39 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..2676c30 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "030000007e0500000920000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index b2b7749..521e308 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -9,7 +9,9 @@ import static edu.wpi.first.units.Units.Meter; import static edu.wpi.first.units.Units.RPM; +import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; @@ -85,6 +87,31 @@ public static abstract class FieldConstants{ Units.Inches.of(72.0), new Rotation3d() ); + + public static final Pose2d blueTargetHighPose = new Pose2d( + Units.Inches.of(150), + Units.Inches.of(175), + new Rotation2d() + ); + + public static final Pose2d blueTargetLowPose = new Pose2d( + Units.Inches.of(150), + Units.Inches.of(100), + new Rotation2d() + ); + + public static final Pose2d redTargetHighPose = new Pose2d( + Units.Inches.of(500), + Units.Inches.of(175), + new Rotation2d() + ); + + public static final Pose2d redTargetLowPose = new Pose2d( + Units.Inches.of(500), + Units.Inches.of(100), + new Rotation2d() + ); + } public static abstract class DriveConstants{ public static final double xyP = 10; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 1165b83..e6ce10c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -144,7 +144,7 @@ private void configureBindings() { joystick.rightBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); joystick.rightBumper().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); - //joystick.leftBumper().onTrue(new InstantCommand(() -:drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no"))); //shoot while stationary + joystick.leftBumper().onTrue(new InstantCommand(() -> drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no")))); //shoot while stationary joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); @@ -159,7 +159,8 @@ private void configureBindings() { joystick.rightStick().onTrue(new InstantCommand(() -> drivetrain.seedFieldCentric())); - joystick.leftBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.CLIMBPREP))); + //joystick.leftBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.CLIMBPREP))); + //FOR HESHEL /* joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); diff --git a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java index fab88e9..9000790 100644 --- a/src/main/java/frc/robot/subsystems/ClimbSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimbSubsystem.java @@ -47,8 +47,6 @@ public class ClimbSubsystem extends SubsystemBase { // Position Limits private final double softLowerLimitMeters = 0.0; private final double softUpperLimitMeters = 2.0; - private final double hardLowerLimitMeters = 0.0; - private final double hardUpperLimitMeters = 3.0; // Control private final ElevatorFeedforward feedforward = new ElevatorFeedforward(0, .5, 0, 0); // Tune kG for gravity compensation diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index b876547..ba94f50 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -75,6 +75,8 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); + + /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -354,6 +356,19 @@ public static Pose3d getHubPose() { .orElse(Constants.FieldConstants.blueHubPose); // Default to blue if alliance unknown } + public static Pose2d getTargetPose(double robotX, double robotY) { + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance == Alliance.Red) { + return robotX > 250 + ? (robotY > 158.32 ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) + : Constants.FieldConstants.redHubPose.toPose2d(); + } else { + return robotX < 250 + ? (robotY > 158.32 ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) + : Constants.FieldConstants.blueHubPose.toPose2d(); + } +} + /** * Calculates the closest point on the predefined circle to the current robot pose. * @@ -400,51 +415,62 @@ public double getDistance(){ return currentDistance; } + + + + + + Pose2d ShootingLocation; - public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { +public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { return applyRequest(() -> { - - // Get current pose and target hub position + Pose2d drivePose = getState().Pose; - Pose2d targetPose = getHubPose().toPose2d(); + Pose2d targetPose = getTargetPose(drivePose.getX(), drivePose.getY()); ShootingLocation = targetPose; - - // Calculate angle from hub to robot + Translation2d toRobot = drivePose.getTranslation().minus(targetPose.getTranslation()); Rotation2d angleToRobot = toRobot.getAngle(); - // Calculate desired rotation (face the hub) - Rotation2d desiredAngle = angleToRobot.rotateBy(Rotation2d.k180deg); // Face toward hub | Take RotateBy out for back to face Hub + Rotation2d desiredAngle = angleToRobot.rotateBy(Rotation2d.k180deg); Rotation2d currentAngle = drivePose.getRotation(); - if(Math.abs(desiredAngle.getDegrees() - currentAngle.getDegrees()) <= 1) {desiredAngle = currentAngle;} + if (Math.abs(desiredAngle.getDegrees() - currentAngle.getDegrees()) <= 1) { + desiredAngle = currentAngle; + } - // Calculate rotational rate to face hub double rotationalRate = rotationController.calculate( currentAngle.getRadians(), desiredAngle.getRadians() ); + double veloX = -controller.getLeftY(); - if (Math.abs(veloX) < 0.1 ){ - veloX = 0; - } + if (Math.abs(veloX) < 0.1) veloX = 0; double veloY = -controller.getLeftX(); - if (Math.abs(veloY) < 0.1 ){ - veloY = 0; - } + if (Math.abs(veloY) < 0.1) veloY = 0; - if (tuning.equals("PovLeft")) { veloX = 0; veloY = 0.3;} - if (tuning.equals("PovRight")) { veloX = 0; veloY = -0.3;} - // Apply the request: radial (distance maintenance) + tangential (circling) - return alignRequest - .withVelocityX(veloX * maxSpeed) - .withVelocityY(veloY * maxSpeed) - .withRotationalRate(rotationalRate * maxAngularRate); + if (tuning.equals("PovLeft")) { veloX = 0; veloY = 0.3; } + if (tuning.equals("PovRight")) { veloX = 0; veloY = -0.3; } + + SmartDashboard.putNumber("Target/X", targetPose.getX()); + SmartDashboard.putNumber("Target/Y", targetPose.getY()); + SmartDashboard.putNumber("Robot/X", drivePose.getX()); + SmartDashboard.putNumber("Robot/Y", drivePose.getY()); + + return alignRequest + .withVelocityX(veloX * maxSpeed) + .withVelocityY(veloY * maxSpeed) + .withRotationalRate(rotationalRate * maxAngularRate); }); } + + + + + public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ return applyRequest(() ->{ return alignRequest.withVelocityX(-joystick.getLeftY() * MaxSpeed).withDeadband(Constants.DriveConstants.TranslationDeadband) // Drive forward with negative Y (forward) DriveStraight = robot centric diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6813ace..492228d 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,13 +1,15 @@ // Copyright (c) FIRST and other WPILib contributors. // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. - package frc.robot.subsystems; import static edu.wpi.first.units.Units.*; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.InvertedValue; +import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.wpilibj.Encoder; + import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -47,6 +49,10 @@ public class ShooterSubsystem extends SubsystemBase { private CommandSwerveDrivetrain drivetrain; + + // Creates a BangBangController + BangBangController controller = new BangBangController(); + public ShooterSubsystem(){ shooterFollower1.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); shooterFollower2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); @@ -90,13 +96,6 @@ public Command stop() { }, this); } - public double BangBangChicken(double cur, double vel) { - if (cur < vel) { - return 1.0; - } else { - return f * vel; - } - } //Checking if shooter RPM is at threashold (waiting for it to spinup) public boolean isAtTargetVelo(){ @@ -145,9 +144,9 @@ public boolean isAimedAtTarget() { } @Override - public void periodic() { - double output = BangBangChicken(getVelocity().in(RPM), setVelo.in(RPM)); - shooterLeader.set(output); // Send percentage to motor (0.0 to 1.0) + public void periodic(){ + double output = controller.calculate(getVelocity().in(RPM), setVelo.in(RPM)); + shooterLeader.set(output); SmartDashboard.putNumber("Request RPM", setVelo.in(RPM)); SmartDashboard.putNumber("Real RPM", getVelocity().in(RPM)); From ac9782d2eab64fe2e27b532b481c9f46641b80af Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Wed, 18 Feb 2026 18:46:47 -0800 Subject: [PATCH 03/12] zone detection p much self explanatory, also passes instead of aiming for hub if outside zone --- build.gradle | 2 +- simgui-ds.json | 3 +-- src/main/java/frc/robot/Constants.java | 16 ++++++------- src/main/java/frc/robot/RobotContainer.java | 3 +-- .../subsystems/CommandSwerveDrivetrain.java | 23 +++++++++++++++---- 5 files changed, 29 insertions(+), 18 deletions(-) diff --git a/build.gradle b/build.gradle index 6ca0c83..543ad68 100644 --- a/build.gradle +++ b/build.gradle @@ -104,4 +104,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} +} \ No newline at end of file diff --git a/simgui-ds.json b/simgui-ds.json index 2676c30..69b1a3c 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,8 +91,7 @@ ], "robotJoysticks": [ { - "guid": "030000007e0500000920000000000000", - "useGamepad": true + "guid": "Keyboard0" } ] } diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 521e308..bdc9810 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -89,26 +89,26 @@ public static abstract class FieldConstants{ ); public static final Pose2d blueTargetHighPose = new Pose2d( - Units.Inches.of(150), - Units.Inches.of(175), + Units.Inches.of(50), + Units.Inches.of(265), new Rotation2d() ); public static final Pose2d blueTargetLowPose = new Pose2d( - Units.Inches.of(150), - Units.Inches.of(100), + Units.Inches.of(50), + Units.Inches.of(50), new Rotation2d() ); public static final Pose2d redTargetHighPose = new Pose2d( - Units.Inches.of(500), - Units.Inches.of(175), + Units.Inches.of(600), + Units.Inches.of(265), new Rotation2d() ); public static final Pose2d redTargetLowPose = new Pose2d( - Units.Inches.of(500), - Units.Inches.of(100), + Units.Inches.of(600), + Units.Inches.of(50), new Rotation2d() ); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e6ce10c..07bdc43 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -139,12 +139,11 @@ private void configureBindings() { joystick.start().whileTrue(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, MaxSpeed, MaxAngularRate)); - //joystick.rightBumper().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "no")); //Shoot while moving joystick.rightBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); joystick.rightBumper().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); - joystick.leftBumper().onTrue(new InstantCommand(() -> drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no")))); //shoot while stationary + joystick.leftBumper().whileTrue(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no")); joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index ba94f50..3facce1 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -357,14 +357,27 @@ public static Pose3d getHubPose() { } public static Pose2d getTargetPose(double robotX, double robotY) { + SmartDashboard.putNumber("Robot/X", robotX); + SmartDashboard.putNumber("Robot/Y", robotY); + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + + + final double thresholdY = Units.Inches.of(158.32).in(Units.Meters); //horizontal middle field line + + SmartDashboard.putNumber("Threshold/Y", thresholdY); + if (alliance == Alliance.Red) { - return robotX > 250 - ? (robotY > 158.32 ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) - : Constants.FieldConstants.redHubPose.toPose2d(); + double thresholdX = Units.Inches.of(469.11).in(Units.Meters); // + SmartDashboard.putNumber("Threshold/X", thresholdX); + return robotX < thresholdX + ? (robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) + : Constants.FieldConstants.redHubPose.toPose2d(); } else { - return robotX < 250 - ? (robotY > 158.32 ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) + double thresholdX = Units.Inches.of(182.11).in(Units.Meters); + SmartDashboard.putNumber("Threshold/X", thresholdX); + return robotX > thresholdX + ? (robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) : Constants.FieldConstants.blueHubPose.toPose2d(); } } From ef5d2bd80a0a93cc1d20050f0c2ab5777104d88d Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Sun, 1 Mar 2026 13:41:17 -0800 Subject: [PATCH 04/12] balldetect idk if works switches pipelines when needed 0 apriltag/megatag 1 is fuel. drives or is supposed to go to fuel when detected via noahaude code --- src/main/java/frc/robot/RobotContainer.java | 40 ++++++++++----- .../java/frc/robot/handlers/DriveHandler.java | 21 ++++++-- .../frc/robot/handlers/VisionHandler.java | 51 +++++++++++++++++++ .../subsystems/CommandSwerveDrivetrain.java | 39 +++++++++++++- .../java/frc/robot/subsystems/Vision.java | 21 +++++++- 5 files changed, 154 insertions(+), 18 deletions(-) create mode 100644 src/main/java/frc/robot/handlers/VisionHandler.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 07bdc43..68a7e7b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -40,6 +40,7 @@ import frc.robot.handlers.ClimbHandler; import frc.robot.handlers.Superstructure; import frc.robot.handlers.Superstructure.SuperstructureState; +import frc.robot.handlers.VisionHandler; import frc.robot.subsystems.ClimbSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.HopperSubsystem; @@ -87,20 +88,19 @@ public class RobotContainer { /* Path follower */ private final SendableChooser autoChooser; - public RobotContainer() { - DriveHandler.getInstance().initialize(drivetrain, joystick, drive, MaxSpeed, MaxAngularRate); - ShooterHandler.getInstance().initialize(drivetrain, shooter); - Superstructure.getInstance().initialize(shooter, drivetrain, climb); - ShooterSubsystem.getInstance().initialize(drivetrain); + public RobotContainer() { + DriveHandler.getInstance().initialize(drivetrain, joystick, drive, MaxSpeed, MaxAngularRate, vision); + ShooterHandler.getInstance().initialize(drivetrain, shooter); + Superstructure.getInstance().initialize(shooter, drivetrain, climb); + VisionHandler.getInstance().initialize(vision); + ShooterSubsystem.getInstance().initialize(drivetrain); - autoChooser = AutoBuilder.buildAutoChooser("Tests"); - SmartDashboard.putData("Auto Mode", autoChooser); + autoChooser = AutoBuilder.buildAutoChooser("Tests"); + SmartDashboard.putData("Auto Mode", autoChooser); - configureBindings(); - - // Warmup PathPlanner to avoid Java pauses - FollowPathCommand.warmupCommand().schedule(); - } + configureBindings(); + FollowPathCommand.warmupCommand().schedule(); +} private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. @@ -145,8 +145,22 @@ private void configureBindings() { joystick.leftBumper().whileTrue(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no")); + + + + + joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); - + + + + + + + + + joystick.y().onTrue(new InstantCommand(() -> VisionHandler.getInstance().setDesiredState(VisionHandler.VisionState.CHASING))); + joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); diff --git a/src/main/java/frc/robot/handlers/DriveHandler.java b/src/main/java/frc/robot/handlers/DriveHandler.java index 7990a1b..1856a02 100644 --- a/src/main/java/frc/robot/handlers/DriveHandler.java +++ b/src/main/java/frc/robot/handlers/DriveHandler.java @@ -12,11 +12,13 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.LimelightHelpers; import frc.robot.handlers.ShooterHandler.ShooterState; import frc.robot.handlers.StateSubsystem.State; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.subsystems.Vision; public class DriveHandler extends SubsystemBase { @@ -26,7 +28,8 @@ public enum DriveState implements State { SHOOTONTHEMOVE, PASSING, SNAKE, - XDRIVE + XDRIVE, + CHASE } @@ -36,7 +39,7 @@ public enum DriveState implements State { private double maxSpeed; private double maxAngularRate; private static DriveHandler instance; - + private Vision vision; private DriveState desiredState = DriveState.AUTOALLIGN; @@ -53,12 +56,13 @@ public static DriveHandler getInstance(){ return instance; } - public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, SwerveRequest.FieldCentric drive, double maxSpeed, double maxAngularRate) { + public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, SwerveRequest.FieldCentric drive, double maxSpeed, double maxAngularRate, Vision vision) { this.drivetrain = drivetrain; this.joystick = joystick; this.drive = drive; this.maxSpeed = maxSpeed; this.maxAngularRate = maxAngularRate; + this.vision = vision; update(); } @@ -94,26 +98,37 @@ public void update() { } switch (desiredState) { case TELEOPDRIVE: + LimelightHelpers.setPipelineIndex("limelight-front", 0); System.out.print("TELEOP"); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; case AUTOALLIGN: + LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, maxSpeed, maxAngularRate, "no")); break; case SHOOTONTHEMOVE: + LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.shootOnTheMoveIterative(joystick, maxSpeed, maxAngularRate, "no")); break; case PASSING: + LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); //make code for break; case SNAKE: + LimelightHelpers.setPipelineIndex("limelight-front", 0); System.out.print("SNAKE"); drivetrain.setDefaultCommand(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, maxSpeed, maxAngularRate)); break; case XDRIVE: + LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> brake)); break; + case CHASE: + LimelightHelpers.setPipelineIndex("limelight-front", 1); + drivetrain.setDefaultCommand(drivetrain.detectChase(vision, maxSpeed, maxAngularRate)); + break; default: + LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; } diff --git a/src/main/java/frc/robot/handlers/VisionHandler.java b/src/main/java/frc/robot/handlers/VisionHandler.java new file mode 100644 index 0000000..319a4de --- /dev/null +++ b/src/main/java/frc/robot/handlers/VisionHandler.java @@ -0,0 +1,51 @@ +package frc.robot.handlers; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.handlers.StateSubsystem.State; +import frc.robot.subsystems.Vision; + +public class VisionHandler extends SubsystemBase { + + public enum VisionState implements State { + IDLE, + CHASING + } + + private Vision vision; + private static VisionHandler instance; + + private VisionState desiredState = VisionState.IDLE; + private VisionState currentState = VisionState.IDLE; + + private VisionHandler() {} + + public static VisionHandler getInstance() { + if (instance == null) instance = new VisionHandler(); + return instance; + } + + public void initialize(Vision vision) { + this.vision = vision; + } + + public void setDesiredState(VisionState state) { + if (desiredState != state) { + desiredState = state; + } + } + + public boolean isChasing() { + return currentState == VisionState.CHASING; + } + + public Vision getVision() { + return vision; + } + + @Override + public void periodic() { + currentState = desiredState; + SmartDashboard.putString("VisionState", currentState.toString()); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 3facce1..c7af44f 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -39,6 +39,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import edu.wpi.first.units.*; import frc.robot.Constants; +import frc.robot.Constants.DriveConstants; +import frc.robot.LimelightHelpers.RawDetection; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -435,7 +437,7 @@ public double getDistance(){ Pose2d ShootingLocation; -public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { + public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { return applyRequest(() -> { Pose2d drivePose = getState().Pose; @@ -479,10 +481,41 @@ public Command headingLocktoHub(CommandXboxController controller, double maxSpee }); } + private final SwerveRequest.RobotCentric chaseRequest = new SwerveRequest.RobotCentric(); +public Command detectChase(Vision vision, double maxSpeed, double maxAngularRate) { + return applyRequest(() -> { + double[] data = vision.getFuelData(); + + // data[0] = cluster count, [1] = best angle X, [2] = angle Y, + // [3] = distance inches, [4] = score, [5] = fuel count + if (data == null || data.length < 6 || data[0] == 0) { + return chaseRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); + } + + double tx = data[1]; // horizontal angle to best cluster, negative = left + + double targetAngle = getState().Pose.getRotation().getRadians() + Math.toRadians(tx); + double rotationalRate = rotationController.calculate( + getState().Pose.getRotation().getRadians(), + targetAngle + ); + // only drive forward once aimed at target + double forwardSpeed = Math.abs(tx) < DriveConstants.RotationalToleranceDegrees ? maxSpeed : 0; + SmartDashboard.putNumber("Chase/TX", tx); + SmartDashboard.putNumber("Chase/Distance", data[3]); + SmartDashboard.putNumber("Chase/FuelCount", data[5]); + SmartDashboard.putNumber("Chase/Score", data[4]); + SmartDashboard.putNumber("Chase/RotationalRate", rotationalRate); + return chaseRequest + .withVelocityX(forwardSpeed) + .withVelocityY(0) + .withRotationalRate(rotationalRate); + }); +} public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ return applyRequest(() ->{ @@ -679,6 +712,10 @@ public Command getSnakeDriveCommand(SwerveRequest.FieldCentric drive, CommandSwe .withRotationalRate(rotationRate); }); } +/*Open limelight-front.local:5801 in a browser +Create a neural detector pipeline +Check if there's a built-in model for your game piece — if yes, use it, if no, you'll need to train one +Verify you can see detections in the camera stream */ @Override diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 31ce073..2797015 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,6 +3,8 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; +import frc.robot.LimelightHelpers.RawDetection; +import frc.robot.LimelightHelpers.RawFiducial; public class Vision extends SubsystemBase { @@ -95,6 +97,23 @@ public void updateVisionMeasurements() { @Override public void periodic() { - updateVisionMeasurements(); + updateVisionMeasurements();} + + public boolean hasDetection() { + return LimelightHelpers.getRawDetections(PosLimelight).length > 0; +} + +public double getDetectionTX() { + LimelightHelpers.RawDetection[] detections = LimelightHelpers.getRawDetections(PosLimelight); + if (detections.length == 0) return 0.0; + return detections[0].txnc; + } + +public double[] getFuelData() { + return LimelightHelpers.getPythonScriptData(PosLimelight); +} + +public RawDetection[] getAllFuel() { + return LimelightHelpers.getRawDetections(PosLimelight); } } \ No newline at end of file From 4a0fe08f1cef7ef01160e592e502da7048a6655b Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Tue, 3 Mar 2026 17:39:49 -0800 Subject: [PATCH 05/12] Shift timer taken largely from mechanical advantage rebuilt chielf delphi post, js tune min max dist --- build.gradle | 3 + src/main/java/frc/robot/Constants.java | 5 + src/main/java/frc/robot/Robot.java | 2 + .../java/frc/robot/handlers/DriveHandler.java | 5 - .../frc/robot/handlers/ShooterHandler.java | 36 ++-- .../robot/subsystems/ShooterSubsystem.java | 2 +- .../java/frc/robot/utils/HubShiftUtil.java | 180 ++++++++++++++++++ 7 files changed, 209 insertions(+), 24 deletions(-) create mode 100644 src/main/java/frc/robot/utils/HubShiftUtil.java diff --git a/build.gradle b/build.gradle index 543ad68..4ea5ef9 100644 --- a/build.gradle +++ b/build.gradle @@ -53,6 +53,9 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { + compileOnly 'org.projectlombok:lombok:1.18.30' + annotationProcessor 'org.projectlombok:lombok:1.18.30' + annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index bdc9810..eb059b9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,6 +20,11 @@ /** Add your docs here. */ public class Constants { + public static abstract class ShiftConstants { + public static final double MinShootingDistanceInches = 40.0; + public static final double MaxShootingDistanceInches = 200.0; //random number not too pivotal rn +} + public static abstract class ShooterConstants { public static final int ShooterLeaderID = 20; //all IDs should be changed (I think theyre 40-43 and 0?) public static final int ShooterFollowerID = 21; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 190160c..332fc3d 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -13,6 +13,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc.robot.utils.HubShiftUtil; import edu.wpi.first.wpilibj.simulation.BatterySim; import edu.wpi.first.wpilibj.simulation.RoboRioSim; @@ -88,6 +89,7 @@ public void autonomousExit() {} @Override public void teleopInit() { + HubShiftUtil.initialize(); CameraServer.startAutomaticCapture(); if (m_autonomousCommand != null) { CommandScheduler.getInstance().cancel(m_autonomousCommand); diff --git a/src/main/java/frc/robot/handlers/DriveHandler.java b/src/main/java/frc/robot/handlers/DriveHandler.java index 1856a02..3072d06 100644 --- a/src/main/java/frc/robot/handlers/DriveHandler.java +++ b/src/main/java/frc/robot/handlers/DriveHandler.java @@ -111,24 +111,19 @@ public void update() { drivetrain.setDefaultCommand(drivetrain.shootOnTheMoveIterative(joystick, maxSpeed, maxAngularRate, "no")); break; case PASSING: - LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); //make code for break; case SNAKE: - LimelightHelpers.setPipelineIndex("limelight-front", 0); System.out.print("SNAKE"); drivetrain.setDefaultCommand(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, maxSpeed, maxAngularRate)); break; case XDRIVE: - LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> brake)); break; case CHASE: - LimelightHelpers.setPipelineIndex("limelight-front", 1); drivetrain.setDefaultCommand(drivetrain.detectChase(vision, maxSpeed, maxAngularRate)); break; default: - LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; } diff --git a/src/main/java/frc/robot/handlers/ShooterHandler.java b/src/main/java/frc/robot/handlers/ShooterHandler.java index 46f826b..a1b50a8 100644 --- a/src/main/java/frc/robot/handlers/ShooterHandler.java +++ b/src/main/java/frc/robot/handlers/ShooterHandler.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.RPM; +import java.time.chrono.IsoChronology; + import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; @@ -14,6 +16,7 @@ import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.RobotContainer; +import frc.robot.utils.HubShiftUtil; public class ShooterHandler extends SubsystemBase implements StateSubsystem { @@ -30,13 +33,12 @@ public enum ShooterState implements State { } private static ShooterHandler instance; - // AFTER: - private CommandSwerveDrivetrain drivetrain; // ← ADDED THIS - private ShooterSubsystem shooter; // ← REMOVED "= new ShooterSubsystem()" + private CommandSwerveDrivetrain drivetrain; + private ShooterSubsystem shooter; private ShooterState desiredState = ShooterState.OFF; private ShooterState currentState = ShooterState.OFF; - private Command shooterCommand = null; // ← ADD THIS + private Command shooterCommand = null; private ShooterHandler() {} @@ -54,8 +56,8 @@ public void initialize(CommandSwerveDrivetrain drivetrain, ShooterSubsystem shoo } AngularVelocity TuneablefastShot = Constants.ShooterConstants.FastShot; - public void adjustFastShot(double valu){ - TuneablefastShot = TuneablefastShot.plus(RPM.of(valu)); + public void adjustFastShot(double value){ + TuneablefastShot = TuneablefastShot.plus(RPM.of(value)); if(currentState == ShooterState.TUNING){ shooter.setVelocity(TuneablefastShot); @@ -79,7 +81,6 @@ public void setDesiredState(State state) { @Override public void handleStateTransition() { - // Optional: delegate to update update(); } @@ -91,7 +92,7 @@ public void update() { } if (currentState != desiredState) { System.out.println(" State changing from " + currentState + " to " + desiredState); - handleStateChange(); // switch states + handleStateChange(); } updateContinuousStates(); } @@ -99,8 +100,6 @@ public void update() { private void handleStateChange(){ switch (desiredState) { case SHOOTING: - //double DistMeters = drivetrain.GetFutureDistMeters(); - //shooter.setVelocityWithCalc(DistMeters).schedule(); break; case SLOW: CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.SlowShot)); @@ -109,10 +108,6 @@ private void handleStateChange(){ CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.FastShot)); break; case TUNING: - //shooter.setVelocity(TuneablefastShot).schedule(); - break; - case OFF: - CommandScheduler.getInstance().schedule(shooter.stop()); break; default: CommandScheduler.getInstance().schedule(shooter.stop()); @@ -124,11 +119,15 @@ private void handleStateChange(){ private void updateContinuousStates() { switch (currentState) { case SHOOTING: + if(!HubShiftUtil.getOfficialShiftInfo().active()){ + CommandScheduler.getInstance().schedule(shooter.stop()); + break; + } double DistMeters = drivetrain.GetFutureDistMeters(); AngularVelocity targetSpeed = shooter.getCalcedRPM(DistMeters); CommandScheduler.getInstance().schedule(shooter.setVelocity(targetSpeed)); break; - + case TUNING: CommandScheduler.getInstance().schedule(shooter.setVelocity(TuneablefastShot)); break; @@ -137,12 +136,13 @@ private void updateContinuousStates() { break; } } + public ShooterState getCurrentState() { return currentState; } @Override -public void periodic() { - update(); // Handle state transitions -} + public void periodic() { + update(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 492228d..6a90bcd 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -113,7 +113,7 @@ public AngularVelocity getCalcedRPM(double DistMeters){ double distanceInches = DistMeters * 39.3701; AngularVelocity velo = RPM.of(-0.0312466 * Math.pow(distanceInches, 2) + 29.07009 * distanceInches + 828.29202); - if (velo.gte(Constants.ShooterConstants.SetRPMHardStop)) { + if (velo.gte(Constants.ShooterConstants.SetRPMHardStop)) { velo = Constants.ShooterConstants.SetRPMHardStop; } diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java new file mode 100644 index 0000000..a3c8c68 --- /dev/null +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -0,0 +1,180 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; +import java.util.Optional; +import lombok.Setter; +import java.util.function.Supplier; +import frc.robot.Constants; + +public class HubShiftUtil { + public enum ShiftEnum { + TRANSITION, + + SHIFT1, + SHIFT2, + SHIFT3, + SHIFT4, + ENDGAME, + AUTO, + DISABLED; + } + + public record ShiftInfo( + ShiftEnum currentShift, double elapsedTime, double remainingTime, boolean active) {} + + private static Timer shiftTimer = new Timer(); + private static final ShiftEnum[] shiftsEnums = ShiftEnum.values(); + + private static final double[] shiftStartTimes = {0.0, 10.0, 35.0, 60.0, 85.0, 110.0}; + private static final double[] shiftEndTimes = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0}; + + private static final double minFuelCountDelay = 1.0; + private static final double maxFuelCountDelay = 2.0; + private static final double shiftEndFuelCountExtension = 3.0; + private static final double minTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MinShootingDistanceInches + 0.330879; +private static final double maxTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MaxShootingDistanceInches + 0.330879; + private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); + private static final double endingActiveFudge = + shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + + public static final double autoEndTime = 20.0; + public static final double teleopDuration = 140.0; + private static final boolean[] activeSchedule = {true, true, false, true, false, true}; + private static final boolean[] inactiveSchedule = {true, false, true, false, true, true}; + + @Setter private static Supplier> allianceWinOverride = () -> Optional.empty(); + + public static Optional getAllianceWinOverride() { + return allianceWinOverride.get(); + } + + public static Alliance getFirstActiveAlliance() { + var alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + + var winOverride = getAllianceWinOverride(); + if (!winOverride.isEmpty()) { + return winOverride.get() + ? (alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue) + : (alliance == Alliance.Blue ? Alliance.Blue : Alliance.Red); + } + + String message = DriverStation.getGameSpecificMessage(); + if (message.length() > 0) { + char character = message.charAt(0); + if (character == 'R') { + return Alliance.Blue; + } else if (character == 'B') { + return Alliance.Red; + } + } + + return alliance == Alliance.Blue ? Alliance.Red : Alliance.Blue; + } + + public static void initialize() { + shiftTimer.restart(); + } + + private static boolean[] getSchedule() { + boolean[] currentSchedule; + Alliance startAlliance = getFirstActiveAlliance(); + currentSchedule = + startAlliance == DriverStation.getAlliance().orElse(Alliance.Blue) + ? activeSchedule + : inactiveSchedule; + return currentSchedule; + } + + private static ShiftInfo getShiftInfo( + boolean[] currentSchedule, double[] shiftStartTimes, double[] shiftEndTimes) { + double currentTime = shiftTimer.get(); + double stateTimeElapsed = shiftTimer.get(); + double stateTimeRemaining = 0.0; + boolean active = false; + ShiftEnum currentShift = ShiftEnum.DISABLED; + + if (DriverStation.isAutonomousEnabled()) { + stateTimeElapsed = currentTime; + stateTimeRemaining = autoEndTime - currentTime; + active = true; + currentShift = ShiftEnum.AUTO; + } else if (DriverStation.isEnabled()) { + int currentShiftIndex = -1; + for (int i = 0; i < shiftStartTimes.length; i++) { + if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { + currentShiftIndex = i; + break; + } + } + if (currentShiftIndex < 0) { + currentShiftIndex = shiftStartTimes.length - 1; + } + + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex]; + stateTimeRemaining = shiftEndTimes[currentShiftIndex] - currentTime; + + if (currentShiftIndex > 0) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex - 1]) { + stateTimeElapsed = currentTime - shiftStartTimes[currentShiftIndex - 1]; + } + } + + if (currentShiftIndex < shiftEndTimes.length - 1) { + if (currentSchedule[currentShiftIndex] == currentSchedule[currentShiftIndex + 1]) { + stateTimeRemaining = shiftEndTimes[currentShiftIndex + 1] - currentTime; + } + } + + active = currentSchedule[currentShiftIndex]; + currentShift = shiftsEnums[currentShiftIndex]; + } + ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active); + return shiftInfo; + } + + public static ShiftInfo getOfficialShiftInfo() { + return getShiftInfo(getSchedule(), shiftStartTimes, shiftEndTimes); + } + + public static ShiftInfo getShiftedShiftInfo() { + boolean[] shiftSchedule = getSchedule(); + if (shiftSchedule[1] == true) { + double[] shiftedShiftStartTimes = { + 0.0, + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge + }; + double[] shiftedShiftEndTimes = { + 10.0, + 35.0 + endingActiveFudge, + 60.0 + approachingActiveFudge, + 85.0 + endingActiveFudge, + 110.0 + approachingActiveFudge, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } + double[] shiftedShiftStartTimes = { + 0.0, + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0 + }; + double[] shiftedShiftEndTimes = { + 10.0 + endingActiveFudge, + 35.0 + approachingActiveFudge, + 60.0 + endingActiveFudge, + 85.0 + approachingActiveFudge, + 110.0, + 140.0 + }; + return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); + } +} \ No newline at end of file From 149f3b897da2ff56929b222cd2295ed7b0afa219 Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Tue, 3 Mar 2026 18:34:15 -0800 Subject: [PATCH 06/12] Shifts Should be working still needa tune min/max distance. Passes now if not shift or should at least. --- .OutlineViewer/outlineviewer.json | 1 + .../frc/robot/handlers/Superstructure.java | 2 +- .../subsystems/CommandSwerveDrivetrain.java | 31 ++++++++++++------- .../java/frc/robot/utils/HubShiftUtil.java | 8 +++-- 4 files changed, 27 insertions(+), 15 deletions(-) create mode 100644 .OutlineViewer/outlineviewer.json diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/frc/robot/handlers/Superstructure.java b/src/main/java/frc/robot/handlers/Superstructure.java index 080fb88..b47c0d0 100644 --- a/src/main/java/frc/robot/handlers/Superstructure.java +++ b/src/main/java/frc/robot/handlers/Superstructure.java @@ -227,7 +227,7 @@ public void periodic() { return; } - if (currentState == SuperstructureState.SPINUP && (shooter.isAtTargetVelo() || drivetrain.isAimedAtTarget())){ //checks if its at target velo and angle + if (currentState == SuperstructureState.SPINUP && (shooter.isAtTargetVelo() && drivetrain.isAimedAtTarget())){ //checks if its at target velo and angle setDesiredState(SuperstructureState.STATIONARYSHOT); } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index c7af44f..c084cd3 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -42,6 +42,7 @@ import frc.robot.Constants.DriveConstants; import frc.robot.LimelightHelpers.RawDetection; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; +import frc.robot.utils.HubShiftUtil; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** @@ -364,26 +365,32 @@ public static Pose2d getTargetPose(double robotX, double robotY) { Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); - final double thresholdY = Units.Inches.of(158.32).in(Units.Meters); //horizontal middle field line - SmartDashboard.putNumber("Threshold/Y", thresholdY); - if (alliance == Alliance.Red) { - double thresholdX = Units.Inches.of(469.11).in(Units.Meters); // + double thresholdX = Units.Inches.of(469.11).in(Units.Meters); SmartDashboard.putNumber("Threshold/X", thresholdX); - return robotX < thresholdX - ? (robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) - : Constants.FieldConstants.redHubPose.toPose2d(); - } else { + + if (!HubShiftUtil.getOfficialShiftInfo().active()) { + return robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose; + } + + return robotX < thresholdX + ? (robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) + : Constants.FieldConstants.redHubPose.toPose2d(); +} else { double thresholdX = Units.Inches.of(182.11).in(Units.Meters); SmartDashboard.putNumber("Threshold/X", thresholdX); - return robotX > thresholdX - ? (robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) - : Constants.FieldConstants.blueHubPose.toPose2d(); + + if (!HubShiftUtil.getOfficialShiftInfo().active()) { + return robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose; } -} + return robotX > thresholdX + ? (robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) + : Constants.FieldConstants.blueHubPose.toPose2d(); + } +} /** * Calculates the closest point on the predefined circle to the current robot pose. * diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java index a3c8c68..9492c1a 100644 --- a/src/main/java/frc/robot/utils/HubShiftUtil.java +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -4,7 +4,6 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; import java.util.Optional; -import lombok.Setter; import java.util.function.Supplier; import frc.robot.Constants; @@ -44,7 +43,12 @@ public record ShiftInfo( private static final boolean[] activeSchedule = {true, true, false, true, false, true}; private static final boolean[] inactiveSchedule = {true, false, true, false, true, true}; - @Setter private static Supplier> allianceWinOverride = () -> Optional.empty(); + + private static Supplier> allianceWinOverride = () -> Optional.empty(); + + public static void setAllianceWinOverride(Supplier> override) { + allianceWinOverride = override; + } public static Optional getAllianceWinOverride() { return allianceWinOverride.get(); From 750b3ee002211de2d1b11fa1c11ce5b5861252df Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Wed, 4 Mar 2026 16:31:22 -0800 Subject: [PATCH 07/12] Shiftstuff no longer forces shot at passing pos when not in shift. Js stops shooter or should in shooterhandler --- build.gradle | 2 -- .../subsystems/CommandSwerveDrivetrain.java | 16 ++++++++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/build.gradle b/build.gradle index 4ea5ef9..d90ddf5 100644 --- a/build.gradle +++ b/build.gradle @@ -53,8 +53,6 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { - compileOnly 'org.projectlombok:lombok:1.18.30' - annotationProcessor 'org.projectlombok:lombok:1.18.30' annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index c084cd3..d0c52c8 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -350,6 +350,15 @@ public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } + public static boolean isInAllianceZone(double robotX){ + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance == Alliance.Red) { + return robotX > Units.Inches.of(469.11).in(Units.Meters); + } else { + return robotX < Units.Inches.of(182.11).in(Units.Meters); + } +} + // Hub poses for each alliance // Get hub pose based on alliance @@ -371,9 +380,11 @@ public static Pose2d getTargetPose(double robotX, double robotY) { double thresholdX = Units.Inches.of(469.11).in(Units.Meters); SmartDashboard.putNumber("Threshold/X", thresholdX); - if (!HubShiftUtil.getOfficialShiftInfo().active()) { + /*if (!HubShiftUtil.getOfficialShiftInfo().active() && isInAllianceZone(robotX)) { return robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose; } + commented out bcz not usable atm, basically returns passing position + regardless of pose when not your shift*/ return robotX < thresholdX ? (robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) @@ -440,9 +451,6 @@ public double getDistance(){ - - - Pose2d ShootingLocation; public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { return applyRequest(() -> { From 19dcdfd9971aeabd0469eb1f089e7e814fd3aefb Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Wed, 4 Mar 2026 16:50:11 -0800 Subject: [PATCH 08/12] isinalliancezone used but lowk useless too --- .../frc/robot/subsystems/CommandSwerveDrivetrain.java | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index d0c52c8..81b2763 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -350,7 +350,8 @@ public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } - public static boolean isInAllianceZone(double robotX){ + public static boolean isInAllianceZone(Pose2d robotPose){ + double robotX = robotPose.getX(); Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); if (alliance == Alliance.Red) { return robotX > Units.Inches.of(469.11).in(Units.Meters); @@ -358,8 +359,6 @@ public static boolean isInAllianceZone(double robotX){ return robotX < Units.Inches.of(182.11).in(Units.Meters); } } - - // Hub poses for each alliance // Get hub pose based on alliance public static Pose3d getHubPose() { @@ -380,13 +379,15 @@ public static Pose2d getTargetPose(double robotX, double robotY) { double thresholdX = Units.Inches.of(469.11).in(Units.Meters); SmartDashboard.putNumber("Threshold/X", thresholdX); + + /*if (!HubShiftUtil.getOfficialShiftInfo().active() && isInAllianceZone(robotX)) { return robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose; } commented out bcz not usable atm, basically returns passing position regardless of pose when not your shift*/ - return robotX < thresholdX + return !isInAllianceZone(new Pose2d(robotX, robotY, new Rotation2d())) ? (robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) : Constants.FieldConstants.redHubPose.toPose2d(); } else { @@ -397,7 +398,7 @@ public static Pose2d getTargetPose(double robotX, double robotY) { return robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose; } - return robotX > thresholdX + return isInAllianceZone(new Pose2d(robotX, robotY, new Rotation2d())) ? (robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) : Constants.FieldConstants.blueHubPose.toPose2d(); } From 2def0af647a19db969ef454b9d80bdfd6d0c8850 Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Sun, 8 Mar 2026 15:14:56 -0700 Subject: [PATCH 09/12] earlyshot and more so u can hold the shoot button now idk if rumble works fully, and shift now gets tracked and you shoot a second early if holding spinup --- simgui-ds.json | 3 +- src/main/java/frc/robot/RobotContainer.java | 60 ++++++++++++------- .../frc/robot/handlers/ShooterHandler.java | 33 +++++++--- .../java/frc/robot/utils/HubShiftUtil.java | 19 +++++- 4 files changed, 85 insertions(+), 30 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3c..c4b7efd 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,7 +91,8 @@ ], "robotJoysticks": [ { - "guid": "Keyboard0" + "guid": "78696e70757401000000000000000000", + "useGamepad": true } ] } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 68a7e7b..8b5727f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -49,6 +49,7 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Vision; +import frc.robot.utils.HubShiftUtil; import pabeles.concurrency.IntOperatorTask.Max; public class RobotContainer { @@ -92,7 +93,7 @@ public RobotContainer() { DriveHandler.getInstance().initialize(drivetrain, joystick, drive, MaxSpeed, MaxAngularRate, vision); ShooterHandler.getInstance().initialize(drivetrain, shooter); Superstructure.getInstance().initialize(shooter, drivetrain, climb); - VisionHandler.getInstance().initialize(vision); + VisionHandler.getInstance().initialize(vision); ShooterSubsystem.getInstance().initialize(drivetrain); autoChooser = AutoBuilder.buildAutoChooser("Tests"); @@ -155,10 +156,6 @@ private void configureBindings() { - - - - joystick.y().onTrue(new InstantCommand(() -> VisionHandler.getInstance().setDesiredState(VisionHandler.VisionState.CHASING))); joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); @@ -203,19 +200,42 @@ private void configureBindings() { - - joystick.povDown().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(-250))); //in RPM - joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(250))); - - joystick.povLeft().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovLeft")); //Shoot while moving - - joystick.povRight().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovRight")); //Shoot while moving - - } - - public Command getAutonomousCommand() { - /* Run the path selected from the auto chooser */ - return autoChooser.getSelected(); - } -} + joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(250))); + + joystick.povLeft().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovLeft")); //Shoot while moving + + joystick.povRight().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovRight")); //Shoot while moving + + new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { + double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); + return remaining <= 5.0 && remaining > 4.75; + }).onTrue(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 1.0)) + .andThen(new WaitCommand(0.25)) + .andThen(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0)))); + + new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { + double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); + return remaining <= 3.0 && remaining > 2.75; + }).onTrue(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 1.0)) + .andThen(new WaitCommand(0.25)) + .andThen(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0)))); + + new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { + double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); + return remaining <= 1.0 && remaining > 0.0; + }).whileTrue(Commands.run(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 1.0))) + .onFalse(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0))); + + new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { + double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); + return remaining <= 1.0 && HubShiftUtil.isNextShiftActive(); + }).onTrue(Commands.runOnce(() -> { + System.out.println("Early shoot window opened, remaining: " + HubShiftUtil.getOfficialShiftInfo().remainingTime()); + })); + } + + public Command getAutonomousCommand() { + return autoChooser.getSelected(); + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/ShooterHandler.java b/src/main/java/frc/robot/handlers/ShooterHandler.java index a1b50a8..c799c27 100644 --- a/src/main/java/frc/robot/handlers/ShooterHandler.java +++ b/src/main/java/frc/robot/handlers/ShooterHandler.java @@ -8,6 +8,7 @@ import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.Joystick; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; @@ -17,6 +18,7 @@ import frc.robot.subsystems.ShooterSubsystem; import frc.robot.RobotContainer; import frc.robot.utils.HubShiftUtil; +import frc.robot.utils.HubShiftUtil.ShiftInfo; public class ShooterHandler extends SubsystemBase implements StateSubsystem { @@ -97,10 +99,26 @@ public void update() { updateContinuousStates(); } + Double DistMeters = 0.0; private void handleStateChange(){ switch (desiredState) { case SHOOTING: - break; + + + ShiftInfo shiftInfo = HubShiftUtil.getOfficialShiftInfo(); + boolean earlyShoot = !shiftInfo.active() && shiftInfo.remainingTime() <= 1.0 && HubShiftUtil.isNextShiftActive(); + System.out.println("active: " + shiftInfo.active() + " | remainingTime: " + shiftInfo.remainingTime() + " | earlyShoot: " + earlyShoot); + + if (!shiftInfo.active() && !earlyShoot) { + CommandScheduler.getInstance().schedule(shooter.stop()); + break; + } + break; + + + + + case SLOW: CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.SlowShot)); break; @@ -119,10 +137,7 @@ private void handleStateChange(){ private void updateContinuousStates() { switch (currentState) { case SHOOTING: - if(!HubShiftUtil.getOfficialShiftInfo().active()){ - CommandScheduler.getInstance().schedule(shooter.stop()); - break; - } + double DistMeters = drivetrain.GetFutureDistMeters(); AngularVelocity targetSpeed = shooter.getCalcedRPM(DistMeters); CommandScheduler.getInstance().schedule(shooter.setVelocity(targetSpeed)); @@ -142,7 +157,9 @@ public ShooterState getCurrentState() { } @Override - public void periodic() { - update(); - } +public void periodic() { + DistMeters = drivetrain.getDistance(); + update(); // Handle state transitions + SmartDashboard.putString("ShooterState", currentState.toString()); +} } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java index 9492c1a..83ecf8b 100644 --- a/src/main/java/frc/robot/utils/HubShiftUtil.java +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -1,8 +1,11 @@ package frc.robot.utils; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import java.util.Optional; import java.util.function.Supplier; import frc.robot.Constants; @@ -104,7 +107,7 @@ private static ShiftInfo getShiftInfo( stateTimeRemaining = autoEndTime - currentTime; active = true; currentShift = ShiftEnum.AUTO; - } else if (DriverStation.isEnabled()) { + } else if (DriverStation.isEnabled() || RobotBase.isSimulation()) { int currentShiftIndex = -1; for (int i = 0; i < shiftStartTimes.length; i++) { if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { @@ -134,6 +137,7 @@ private static ShiftInfo getShiftInfo( active = currentSchedule[currentShiftIndex]; currentShift = shiftsEnums[currentShiftIndex]; } + SmartDashboard.putBoolean("Shift Active", active); ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active); return shiftInfo; } @@ -181,4 +185,17 @@ public static ShiftInfo getShiftedShiftInfo() { }; return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); } + + public static boolean isNextShiftActive() { + boolean[] schedule = getSchedule(); + double currentTime = shiftTimer.get(); + + for (int i = 0; i < shiftStartTimes.length - 1; i++) { + if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { + return schedule[i + 1]; + } + } + return false; + } + } \ No newline at end of file From f464a9d00de2feece6fe3369ba342b07e58404ef Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Thu, 12 Mar 2026 18:00:21 -0700 Subject: [PATCH 10/12] HubShiftUtil updated - uh should work w updated stuff and also i made it so that it stops the low indexer cuz obvi it shouldnt stop robot from spinup idk what i was tweakin before also made a few methods in vision.java --- .wpilib/wpilib_preferences.json | 4 +- build.gradle | 3 +- src/main/deploy/pathplanner/autos/Depot.auto | 37 +++ src/main/deploy/pathplanner/paths/D1-D2.path | 68 ++++ src/main/deploy/pathplanner/paths/D2-Hub.path | 66 ++++ src/main/deploy/pathplanner/paths/D2-O.path | 54 ++++ src/main/deploy/pathplanner/paths/S1-D1.path | 66 ++++ src/main/deploy/pathplanner/settings.json | 32 +- src/main/java/frc/robot/Constants.java | 93 +++--- src/main/java/frc/robot/FieldConstants.java | 25 ++ src/main/java/frc/robot/RobotContainer.java | 222 +++++++------ .../frc/robot/generated/TunerConstants.java | 54 ++-- .../java/frc/robot/handlers/DriveHandler.java | 52 +-- .../frc/robot/handlers/IndexerLowHandler.java | 20 +- .../frc/robot/handlers/IntakeHandler.java | 25 +- .../robot/handlers/IntakeSlideHandler.java | 121 +++++++ .../frc/robot/handlers/ShooterHandler.java | 83 ++--- .../frc/robot/handlers/Superstructure.java | 121 ++++--- .../subsystems/BangBangShooterSubsystem.java | 179 +++++++++++ .../subsystems/CommandSwerveDrivetrain.java | 289 ++++++++--------- .../java/frc/robot/subsystems/Elevator.java | 299 ++++++++++++++++++ .../frc/robot/subsystems/HopperSubsystem.java | 2 +- .../subsystems/IndexerHighSubsystem.java | 3 +- .../robot/subsystems/IndexerLowSubsystem.java | 3 +- .../frc/robot/subsystems/IntakeSubsystem.java | 13 +- .../java/frc/robot/subsystems/Vision.java | 71 +++-- .../java/frc/robot/utils/HubShiftUtil.java | 2 +- 27 files changed, 1477 insertions(+), 530 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Depot.auto create mode 100644 src/main/deploy/pathplanner/paths/D1-D2.path create mode 100644 src/main/deploy/pathplanner/paths/D2-Hub.path create mode 100644 src/main/deploy/pathplanner/paths/D2-O.path create mode 100644 src/main/deploy/pathplanner/paths/S1-D1.path create mode 100644 src/main/java/frc/robot/FieldConstants.java create mode 100644 src/main/java/frc/robot/handlers/IntakeSlideHandler.java create mode 100644 src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java create mode 100644 src/main/java/frc/robot/subsystems/Elevator.java diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index eadec71..8ef6df4 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -2,5 +2,5 @@ "enableCppIntellisense": false, "currentLanguage": "java", "projectYear": "2026", - "teamNumber": 7762 -} + "teamNumber": 7415 +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index d90ddf5..6ca0c83 100644 --- a/build.gradle +++ b/build.gradle @@ -53,7 +53,6 @@ def includeDesktopSupport = true // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries. // Also defines JUnit 5. dependencies { - annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() @@ -105,4 +104,4 @@ wpi.java.configureTestTasks(test) // Configure string concat to always inline compile tasks.withType(JavaCompile) { options.compilerArgs.add '-XDstringConcat=inline' -} \ No newline at end of file +} diff --git a/src/main/deploy/pathplanner/autos/Depot.auto b/src/main/deploy/pathplanner/autos/Depot.auto new file mode 100644 index 0000000..33b1edf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Depot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S1-D1" + } + }, + { + "type": "path", + "data": { + "pathName": "D1-D2" + } + }, + { + "type": "path", + "data": { + "pathName": "D2-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D1-D2.path b/src/main/deploy/pathplanner/paths/D1-D2.path new file mode 100644 index 0000000..bcf3584 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D1-D2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.6368556537195638, + "y": 6.982748610619782 + }, + "prevControl": null, + "nextControl": { + "x": 0.6368556537195639, + "y": 5.982748610619782 + }, + "isLocked": false, + "linkedName": "D1" + }, + { + "anchor": { + "x": 0.6368556537195638, + "y": 5.029978324456286 + }, + "prevControl": { + "x": 0.6368556537195638, + "y": 5.84299565391915 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D2" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D2-Hub.path b/src/main/deploy/pathplanner/paths/D2-Hub.path new file mode 100644 index 0000000..880bc2f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D2-Hub.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.6368556537195638, + "y": 5.029978324456286 + }, + "prevControl": null, + "nextControl": { + "x": 1.3119677039106872, + "y": 5.247272024545016 + }, + "isLocked": false, + "linkedName": "D2" + }, + { + "anchor": { + "x": 2.628411712511091, + "y": 4.819671694764862 + }, + "prevControl": { + "x": 1.8880035492457847, + "y": 5.342786157941436 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOff", + "waypointRelativePos": 0.21226415094339626, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D2-O.path b/src/main/deploy/pathplanner/paths/D2-O.path new file mode 100644 index 0000000..35ec47b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/D2-O.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.6368556537195638, + "y": 5.029978324456286 + }, + "prevControl": null, + "nextControl": { + "x": 3.4849047047979993, + "y": 4.997786665183881 + }, + "isLocked": false, + "linkedName": "D2" + }, + { + "anchor": { + "x": 0.4071872227151724, + "y": 0.6267080745341604 + }, + "prevControl": { + "x": 3.367907081247016, + "y": 0.9808163265306109 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "O" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 45.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S1-D1.path b/src/main/deploy/pathplanner/paths/S1-D1.path new file mode 100644 index 0000000..9079154 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S1-D1.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.586113575865128, + "y": 7.5479148181011535 + }, + "prevControl": null, + "nextControl": { + "x": 1.952386867787004, + "y": 7.145519077193867 + }, + "isLocked": false, + "linkedName": "S1" + }, + { + "anchor": { + "x": 0.6368556537195638, + "y": 6.982748610619782 + }, + "prevControl": { + "x": 0.7405657518219149, + "y": 7.629981208715258 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "ok", + "waypointRelativePos": 0.3596698113207547, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3, + "maxAcceleration": 3, + "maxAngularVelocity": 540, + "maxAngularAcceleration": 720, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 4a0d130..dac61a8 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -1,6 +1,6 @@ { - "robotWidth": 0.762, - "robotLength": 0.762, + "robotWidth": 1.002, + "robotLength": 0.799, "holonomicMode": true, "pathFolders": [], "autoFolders": [], @@ -9,23 +9,23 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 75.0, - "robotMOI": 6.883, + "robotMass": 68.0, + "robotMOI": 6.35, "robotTrackwidth": 0.5588, - "driveWheelRadius": 0.05504, - "driveGearing": 7.363636, - "maxDriveSpeed": 4.7, + "driveWheelRadius": 0.0508, + "driveGearing": 6.03, + "maxDriveSpeed": 5.1206, "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 120.0, - "wheelCOF": 1.7, - "flModuleX": 0.267, - "flModuleY": 0.2667, - "frModuleX": 0.267, - "frModuleY": -0.267, - "blModuleX": -0.267, - "blModuleY": 0.267, - "brModuleX": -0.267, - "brModuleY": -0.267, + "wheelCOF": 2.255, + "flModuleX": 0.232, + "flModuleY": 0.327, + "frModuleX": 0.232, + "frModuleY": -0.327, + "blModuleX": -0.232, + "blModuleY": 0.327, + "brModuleX": -0.232, + "brModuleY": -0.327, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, "robotFeatures": [] diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index eb059b9..fa5fb71 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -4,78 +4,77 @@ package frc.robot; -import static edu.wpi.first.units.Units.Feet; -import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.Meter; -import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.*; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.DistanceUnit; import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; /** Add your docs here. */ public class Constants { - - public static abstract class ShiftConstants { +//TODO change + public static abstract class ShiftConstants { public static final double MinShootingDistanceInches = 40.0; public static final double MaxShootingDistanceInches = 200.0; //random number not too pivotal rn } public static abstract class ShooterConstants { - public static final int ShooterLeaderID = 20; //all IDs should be changed (I think theyre 40-43 and 0?) + public static final int ShooterLeaderID = 20; public static final int ShooterFollowerID = 21; - public static final int ShooterFollowerReversedID = 22; - public static final int ShooterFollowerReversed2ID = 23; - public static final AngularVelocity FastShot = RPM.of(5000); //HESHEL CHANGE ME FAST - public static final AngularVelocity SlowShot = RPM.of(3500); //HESKEL CHANGE ME SLOW - public static final AngularVelocity SetRPMHardStop = RPM.of(3200); - public static double ShootingDistance = Feet.of(6).in(Meter); //6ft starting - public static int RPMTolarance = 50; - + public static final int ShooterFollowerReversedID = 22; // + public static final int ShooterFollowerReversed2ID = 23; // + public static final double FastShot = 25; // IN RPS + public static final double SlowShot = 20; //HESKEL CHANGE ME SLOW + public static final double RPSHardStop = 100.0; + public static final double RPSTolarance = 1; + public static final double kS = 0.3; //volt to overcome static friction feedforawd TUNED ALR + public static final double kV = 0.13; //volts per rps 0.125 } public static abstract class IntakeConstants { - public static final int IntakeMotorID = 31; - public static final int IntakeFollowerReversedMotorID = 30; - public static final double FastIntake = 1; + public static final int IntakeMotorID = 30; + public static final int IntakeFollowerReversedMotorID = 31; + public static final double FastIntake = 0.87; //used to be 1 but 0.87 is more effechient or sum public static final double SlowIntake = 0.5; public static final double SlowReverse = -0.3; - public static final double FastReverse = -1; + public static final double FastReverse = -0.87; + + public static final int IntakeSlideMotorID = 35; + public static final double IntakeSlideOutSetPoint = 25; //In ROTATIONS + public static final double IntakeSlideMiddleSetPoint = 10; + public static final double IntakeSlideInSetPoint = 0; + public static final Angle SafeOutPosition = Degree.of(9.3*360); //in rotations + + public static final double IntakeSlideOutHardStop = 25.5; //rotations; Used for re-zero out + + public static final double PositionTolerance = 0.3; //in degreee + } public static abstract class HopperConstants { public static final int HopperMotorID = 32; // - public static final double FastRoll = -0.3; - public static final double SlowRoll = -0.2; - public static final double FastOutRoll = 0.8; - public static final double SlowOutRoll = 0.4; + public static final double FastRoll = 0.8; + public static final double SlowRoll = 0.3; //0.2 + public static final double FastOutRoll = -0.4; + public static final double SlowOutRoll = -0.2; } public static abstract class IndexerConstants { - public static final int IndexerLowMotorID = 33; // - public static final int IndexerHighMotorID = 34; //NOT RIGHTFAKE FIX NOW - public static final double FastRoll = 1; - public static final double SlowRoll = 0.6; - public static final double FastOutRoll = -0.5; - public static final double SlowOutRoll = -0.3; - - } - - public static abstract class ClimberConstants { - public static final int ClimberMotorID = 32; //change me ts NOT a real motor - public static final double ClimbSpeed = 0.7; - public static final double DescendSpeed = -0.5; - - public static final Distance HighSetPoint = Inches.of(63.0); - public static final Distance MiddleSetPoint = Inches.of(45.0); - public static final Distance LowSetPoint = Inches.of(27.0); + public static final int HighIndexerMotorID = 34; // + public static final int LowIndexerMotorID = 33; // + public static final double FastRoll = -0.87; + public static final double SlowRoll = -0.6; + public static final double FastOutRoll = 0.87; + public static final double SlowOutRoll = 0.3; - public static final Distance LowSetPointDown = Inches.of(23.0); } public static abstract class FieldConstants{ @@ -92,7 +91,6 @@ public static abstract class FieldConstants{ Units.Inches.of(72.0), new Rotation3d() ); - public static final Pose2d blueTargetHighPose = new Pose2d( Units.Inches.of(50), Units.Inches.of(265), @@ -115,21 +113,20 @@ public static abstract class FieldConstants{ Units.Inches.of(600), Units.Inches.of(50), new Rotation2d() - ); - + ); } public static abstract class DriveConstants{ - public static final double xyP = 10; + public static final double xyP = 1; public static final double xyI = 0; public static final double xyD = 0; - public static final double rotP = 3; + public static final double rotP = 0.85; //0.05 public static final double rotI = 0; - public static final double rotD = 0.175; + public static final double rotD = 0.05; //0.05 public static final double TranslationDeadband = 0.1; public static final double RotationDeadband = 0.1; - public static final double RotationalToleranceDegrees = 5.0; + public static final double RotationalToleranceDegrees = 3.0; } } \ No newline at end of file diff --git a/src/main/java/frc/robot/FieldConstants.java b/src/main/java/frc/robot/FieldConstants.java new file mode 100644 index 0000000..99ef85e --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,25 @@ +package frc.robot; + +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.apriltag.AprilTagFieldLayout; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.Filesystem; +import java.io.IOException; +import java.nio.file.Path; + +public class FieldConstants { +public static class Tower { + public static final double width = Units.inchesToMeters(49.25); + public static final double depth = Units.inchesToMeters(45.0); + public static final double height = Units.inchesToMeters(78.25); + public static final double innerOpeningWidth = Units.inchesToMeters(32.250); + public static final double frontFaceX = Units.inchesToMeters(43.51); + + + } + + +} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b5727f..0988d3b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,51 +6,48 @@ import static edu.wpi.first.units.Units.*; -import java.util.Set; - -import org.w3c.dom.traversal.TreeWalker; +import org.ejml.sparse.csc.linsol.qr.LinearSolverQrLeftLooking_DSCC; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; -import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.swerve.SwerveRequest; - import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; +import com.pathplanner.lib.events.OneShotTriggerEvent; import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; - import frc.robot.generated.TunerConstants; import frc.robot.handlers.DriveHandler; import frc.robot.handlers.IntakeHandler; import frc.robot.handlers.ShooterHandler; -import frc.robot.handlers.ClimbHandler; import frc.robot.handlers.Superstructure; +import frc.robot.handlers.IntakeHandler.IntakeState; +import frc.robot.handlers.IntakeSlideHandler; +import frc.robot.handlers.IntakeSlideHandler.IntakeSlideState; import frc.robot.handlers.Superstructure.SuperstructureState; -import frc.robot.handlers.VisionHandler; -import frc.robot.subsystems.ClimbSubsystem; +import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; import frc.robot.subsystems.IndexerLowSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.Vision; import frc.robot.utils.HubShiftUtil; -import pabeles.concurrency.IntOperatorTask.Max; public class RobotContainer { @@ -74,34 +71,53 @@ public class RobotContainer { public final Vision vision = new Vision(drivetrain); - public final ShooterSubsystem shooter = new ShooterSubsystem(); + public final BangBangShooterSubsystem shooter = new BangBangShooterSubsystem(); public final IntakeSubsystem intake = new IntakeSubsystem(); public final HopperSubsystem hopper = new HopperSubsystem(); public final IndexerHighSubsystem HighIndexer = new IndexerHighSubsystem(); public final IndexerLowSubsystem LowIndexer = new IndexerLowSubsystem(); - public final ClimbSubsystem climb = new ClimbSubsystem(); - + public final Elevator IntakeSlide = new Elevator(); public final Superstructure superstructure = Superstructure.getInstance(); - //private TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID); - /* Path follower */ private final SendableChooser autoChooser; - public RobotContainer() { - DriveHandler.getInstance().initialize(drivetrain, joystick, drive, MaxSpeed, MaxAngularRate, vision); - ShooterHandler.getInstance().initialize(drivetrain, shooter); - Superstructure.getInstance().initialize(shooter, drivetrain, climb); - VisionHandler.getInstance().initialize(vision); - ShooterSubsystem.getInstance().initialize(drivetrain); + public RobotContainer() { + DriveHandler.getInstance().initialize(drivetrain, joystick, drive, MaxSpeed, MaxAngularRate); + ShooterHandler.getInstance().initialize(drivetrain, shooter); + Superstructure.getInstance().initialize(shooter, drivetrain); + + configurePathPlanner(); - autoChooser = AutoBuilder.buildAutoChooser("Tests"); - SmartDashboard.putData("Auto Mode", autoChooser); + autoChooser = AutoBuilder.buildAutoChooser("Tests"); + SmartDashboard.putData("Auto Mode", autoChooser); + + configureBindings(); + + // Warmup PathPlanner to avoid Java pauses + FollowPathCommand.warmupCommand().schedule(); + + } + + private void configurePathPlanner() { + NamedCommands.registerCommand("Intake", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); + + NamedCommands.registerCommand("IntakeOff", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + + NamedCommands.registerCommand("Shoot", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); + + NamedCommands.registerCommand("ShootSafe", + new SequentialCommandGroup( + new InstantCommand( () -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP)), + Commands.waitSeconds(2), + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT)) + )); + } - configureBindings(); - FollowPathCommand.warmupCommand().schedule(); -} private void configureBindings() { // Note that X is defined as forward according to WPILib convention, // and Y is defined as to the left according to WPILib convention. @@ -135,55 +151,70 @@ private void configureBindings() { joystick.rightStick().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric)); drivetrain.registerTelemetry(logger::telemeterize); - PathConstraints constraints = new PathConstraints(3, 4, - Degrees.of(540).in(Radians), Degrees.of(720).in(Radians)); - - joystick.start().whileTrue(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, MaxSpeed, MaxAngularRate)); - - //joystick.rightBumper().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "no")); //Shoot while moving - joystick.rightBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); - joystick.rightBumper().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); - - joystick.leftBumper().whileTrue(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no")); - - - - joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); - + // Create the constraints to use while pathfinding + PathConstraints constraints = new PathConstraints( + 3.0, 4.0, + Units.degreesToRadians(540), Units.degreesToRadians(720)); + //ROTATE 90 degreese + joystick.leftBumper().onTrue(Commands.runOnce(() -> { + Rotation2d targetRotation = drivetrain.getPose().getRotation().plus(Rotation2d.fromDegrees(90)); + + // Use your existing rotation controller + drivetrain.applyRequest(() -> { + double rotationalRate = CommandSwerveDrivetrain.rotationController.calculate( + drivetrain.getPose().getRotation().getRadians(), + targetRotation.getRadians() + ); + return new SwerveRequest.FieldCentric() + .withVelocityX(0) + .withVelocityY(0) + .withRotationalRate(rotationalRate * 6); // Max angular rate + }).withTimeout(2.0).schedule(); + })); + + + joystick.rightTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); + + joystick.rightTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); - joystick.y().onTrue(new InstantCommand(() -> VisionHandler.getInstance().setDesiredState(VisionHandler.VisionState.CHASING))); + //joystick.leftBumper().onTrue(new InstantCommand(() -:drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no"))); //shoot while stationary + + joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST))); + joystick.y().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT))); + joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.TUNING))); + joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); - joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); + joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); joystick.leftStick().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.REVERSE))); + + + //joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState((Superstructure.SuperstructureState.TUNING)))); + //joystick.a().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState((Superstructure.SuperstructureState.TUNING)))); + + joystick.rightStick().onTrue(new InstantCommand(() -> drivetrain.seedFieldCentric())); - //joystick.leftBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.CLIMBPREP))); + + //FOR HESHEL /* joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - joystick.a().onTrue( - new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPSLOW)) - .andThen(new WaitCommand(1.0)) - .andThen(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SLOWSHOT)))); - joystick.y().onTrue( - new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST)) - .andThen(new WaitCommand(1.0)) - .andThen(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT)))); + joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST))); + joystick.y().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT))); joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); */ @@ -200,42 +231,53 @@ private void configureBindings() { - joystick.povDown().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(-250))); //in RPM - joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(250))); - - joystick.povLeft().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovLeft")); //Shoot while moving - - joystick.povRight().whileTrue(drivetrain.shootOnTheMoveIterative(joystick, MaxSpeed, MaxAngularRate, "PovRight")); //Shoot while moving - - new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { - double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); - return remaining <= 5.0 && remaining > 4.75; - }).onTrue(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 1.0)) - .andThen(new WaitCommand(0.25)) - .andThen(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0)))); - - new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { - double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); - return remaining <= 3.0 && remaining > 2.75; - }).onTrue(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 1.0)) - .andThen(new WaitCommand(0.25)) - .andThen(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0)))); - - new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { - double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); - return remaining <= 1.0 && remaining > 0.0; - }).whileTrue(Commands.run(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 1.0))) - .onFalse(Commands.runOnce(() -> joystick.getHID().setRumble(edu.wpi.first.wpilibj.GenericHID.RumbleType.kBothRumble, 0.0))); - new edu.wpi.first.wpilibj2.command.button.Trigger(() -> { + + //joystick.povDown().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROIN))); + //joystick.povUp().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROOUT))); //in RPM + joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(1))); + joystick.povDown().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(-1))); + + //joystick.povLeft().onTrue(IntakeSlide.goToSetpoint(()-> Elevator.Setpoint.OUT)); + //joystick.povRight().onTrue(IntakeSlide.goToSetpoint(()-> Elevator.Setpoint.IN)); + + joystick.povRight().whileTrue(IntakeSlide.manualDrive(() -> 0.67)); // out + joystick.povLeft().whileTrue(IntakeSlide.manualDrive(() -> -0.67)); //in + + Runnable rumbleOn = () -> joystick.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0); + Runnable rumbleOff = () -> joystick.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0); + + new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + if (info == null) return false; + double r = info.remainingTime(); + return r <= 5.0 && r > 4.5; + }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); + + new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + if (info == null) return false; + double r = info.remainingTime(); + return r <= 3.0 && r > 2.5; + }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); + + new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + if (info == null) return false; + double r = info.remainingTime(); + return r <= 1.0 && r > 0.0; + }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); + + new Trigger(() -> { double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); return remaining <= 1.0 && HubShiftUtil.isNextShiftActive(); }).onTrue(Commands.runOnce(() -> { System.out.println("Early shoot window opened, remaining: " + HubShiftUtil.getOfficialShiftInfo().remainingTime()); })); - } + } - public Command getAutonomousCommand() { - return autoChooser.getSelected(); - } - } \ No newline at end of file + public Command getAutonomousCommand() { + /* Run the path selected from the auto chooser */ + return autoChooser.getSelected(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 59c543b..9600855 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -84,10 +84,10 @@ public class TunerConstants { private static final double kSteerGearRatio = 26.09090909090909; private static final Distance kWheelRadius = Inches.of(2); - private static final boolean kInvertLeftSide = true; - private static final boolean kInvertRightSide = false; + private static final boolean kInvertLeftSide = false; + private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 21; + private static final int kPigeonId = 50; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); @@ -126,48 +126,48 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 22; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.373291015625); + private static final int kFrontLeftDriveMotorId = 4; + private static final int kFrontLeftSteerMotorId = 3; + private static final int kFrontLeftEncoderId = 19; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.247802734375); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = false; - private static final Distance kFrontLeftXPos = Inches.of(9.25); - private static final Distance kFrontLeftYPos = Inches.of(13); + private static final Distance kFrontLeftXPos = Inches.of(9.125); + private static final Distance kFrontLeftYPos = Inches.of(13.875); // Front Right - private static final int kFrontRightDriveMotorId = 5; - private static final int kFrontRightSteerMotorId = 6; - private static final int kFrontRightEncoderId = 21; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.0302734375); + private static final int kFrontRightDriveMotorId = 7; + private static final int kFrontRightSteerMotorId = 8; + private static final int kFrontRightEncoderId = 20; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.035888671875); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = false; - private static final Distance kFrontRightXPos = Inches.of(9.25); - private static final Distance kFrontRightYPos = Inches.of(-13); + private static final Distance kFrontRightXPos = Inches.of(9.125); + private static final Distance kFrontRightYPos = Inches.of(-13.875); // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 8; - private static final int kBackLeftEncoderId = 20; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.03564453125); + private static final int kBackLeftDriveMotorId = 5; + private static final int kBackLeftSteerMotorId = 6; + private static final int kBackLeftEncoderId = 21; + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.0361328125); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = false; - private static final Distance kBackLeftXPos = Inches.of(-9.25); - private static final Distance kBackLeftYPos = Inches.of(13); + private static final Distance kBackLeftXPos = Inches.of(-9.125); + private static final Distance kBackLeftYPos = Inches.of(13.875); // Back Right - private static final int kBackRightDriveMotorId = 4; - private static final int kBackRightSteerMotorId = 3; - private static final int kBackRightEncoderId = 19; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.248291015625); + private static final int kBackRightDriveMotorId = 1; + private static final int kBackRightSteerMotorId = 2; + private static final int kBackRightEncoderId = 22; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.373046875); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = false; - private static final Distance kBackRightXPos = Inches.of(-9.25); - private static final Distance kBackRightYPos = Inches.of(-13); + private static final Distance kBackRightXPos = Inches.of(-9.125); + private static final Distance kBackRightYPos = Inches.of(-13.875); public static final SwerveModuleConstants FrontLeft = diff --git a/src/main/java/frc/robot/handlers/DriveHandler.java b/src/main/java/frc/robot/handlers/DriveHandler.java index 3072d06..988f26e 100644 --- a/src/main/java/frc/robot/handlers/DriveHandler.java +++ b/src/main/java/frc/robot/handlers/DriveHandler.java @@ -6,19 +6,21 @@ import com.ctre.phoenix6.swerve.SwerveRequest; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rectangle2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import frc.robot.LimelightHelpers; import frc.robot.handlers.ShooterHandler.ShooterState; import frc.robot.handlers.StateSubsystem.State; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.subsystems.Vision; public class DriveHandler extends SubsystemBase { @@ -26,10 +28,9 @@ public enum DriveState implements State { TELEOPDRIVE, AUTOALLIGN, SHOOTONTHEMOVE, - PASSING, SNAKE, XDRIVE, - CHASE + BUMP_LOCK } @@ -39,10 +40,10 @@ public enum DriveState implements State { private double maxSpeed; private double maxAngularRate; private static DriveHandler instance; - private Vision vision; - private DriveState desiredState = DriveState.AUTOALLIGN; + + private DriveState desiredState = DriveState.TELEOPDRIVE; private DriveState currentState = DriveState.TELEOPDRIVE; private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake(); @@ -56,16 +57,29 @@ public static DriveHandler getInstance(){ return instance; } - public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, SwerveRequest.FieldCentric drive, double maxSpeed, double maxAngularRate, Vision vision) { + Rectangle2d zone1 = new Rectangle2d(new Pose2d(1.0, 1.0, new Rotation2d()), 2.0, 2.0); + Rectangle2d zone2 = new Rectangle2d(new Pose2d(5.0, 3.0, new Rotation2d()), 1.5, 2.0); + Rectangle2d zone3 = new Rectangle2d(new Pose2d(10.0, 2.0, new Rotation2d()), 3.0, 1.0); + Rectangle2d zone4 = new Rectangle2d(new Pose2d(14.0, 5.0, new Rotation2d()), 2.0, 2.0); + + Trigger inAnyZone = new Trigger(() -> { + Translation2d robotPos = drivetrain.getState().Pose.getTranslation(); + return zone1.contains(robotPos) || + zone2.contains(robotPos) || + zone3.contains(robotPos) || + zone4.contains(robotPos); + }); + + + public void initialize(CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, SwerveRequest.FieldCentric drive, double maxSpeed, double maxAngularRate) { this.drivetrain = drivetrain; this.joystick = joystick; this.drive = drive; this.maxSpeed = maxSpeed; this.maxAngularRate = maxAngularRate; - this.vision = vision; update(); - } + } public void setDesiredState(DriveState state){ @@ -82,10 +96,6 @@ public void updateStates(){ } } - - - - public void handleStateTransition() { update(); } @@ -98,30 +108,22 @@ public void update() { } switch (desiredState) { case TELEOPDRIVE: - LimelightHelpers.setPipelineIndex("limelight-front", 0); - System.out.print("TELEOP"); drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; case AUTOALLIGN: - LimelightHelpers.setPipelineIndex("limelight-front", 0); - drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, maxSpeed, maxAngularRate, "no")); + drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, maxSpeed, maxAngularRate)); break; case SHOOTONTHEMOVE: - LimelightHelpers.setPipelineIndex("limelight-front", 0); drivetrain.setDefaultCommand(drivetrain.shootOnTheMoveIterative(joystick, maxSpeed, maxAngularRate, "no")); break; - case PASSING: - drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); //make code for - break; case SNAKE: - System.out.print("SNAKE"); drivetrain.setDefaultCommand(drivetrain.getSnakeDriveCommand(drive, drivetrain, joystick, maxSpeed, maxAngularRate)); break; case XDRIVE: drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> brake)); break; - case CHASE: - drivetrain.setDefaultCommand(drivetrain.detectChase(vision, maxSpeed, maxAngularRate)); + case BUMP_LOCK: + drivetrain.setDefaultCommand(drivetrain.bumpLockCommand(drive, drivetrain, joystick, maxSpeed, maxAngularRate)); break; default: drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); diff --git a/src/main/java/frc/robot/handlers/IndexerLowHandler.java b/src/main/java/frc/robot/handlers/IndexerLowHandler.java index e891308..1b9e631 100644 --- a/src/main/java/frc/robot/handlers/IndexerLowHandler.java +++ b/src/main/java/frc/robot/handlers/IndexerLowHandler.java @@ -11,7 +11,10 @@ import frc.robot.handlers.StateSubsystem.State; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; +import frc.robot.subsystems.IndexerLowSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.HubShiftUtil; +import frc.robot.utils.HubShiftUtil.ShiftInfo; public class IndexerLowHandler extends SubsystemBase implements StateSubsystem { @@ -24,7 +27,7 @@ public enum IndexerLowState implements State { } private static IndexerLowHandler instance; - private final IndexerHighSubsystem index = new IndexerHighSubsystem(); + private final IndexerLowSubsystem index = new IndexerLowSubsystem(); private IndexerLowState desiredState = IndexerLowState.OFF; private IndexerLowState currentState = IndexerLowState.OFF; @@ -55,10 +58,23 @@ public void update() { if((currentState != desiredState)){ switch (desiredState) { case FAST: - index.set(Constants.IndexerConstants.FastRoll); + ShiftInfo shiftInfo = HubShiftUtil.getOfficialShiftInfo(); + boolean earlyShoot = !shiftInfo.active() && shiftInfo.remainingTime() <= 1.0 && HubShiftUtil.isNextShiftActive(); + System.out.println("active: " + shiftInfo.active() + " | remainingTime: " + shiftInfo.remainingTime() + " | earlyShoot: " + earlyShoot); + + if (!shiftInfo.active() && !earlyShoot) { + index.set(Constants.IndexerConstants.FastRoll); + } + break; case SLOWINTAKE: + ShiftInfo shiftInfo2 = HubShiftUtil.getOfficialShiftInfo(); + boolean earlyShoot2 = !shiftInfo2.active() && shiftInfo2.remainingTime() <= 1.0 && HubShiftUtil.isNextShiftActive(); + System.out.println("active: " + shiftInfo2.active() + " | remainingTime: " + shiftInfo2.remainingTime() + " | earlyShoot: " + earlyShoot2); + + if (!shiftInfo2.active() && !earlyShoot2) { index.set(Constants.IndexerConstants.SlowRoll); + } break; case FASTREVERSE: index.set(Constants.IndexerConstants.FastOutRoll); diff --git a/src/main/java/frc/robot/handlers/IntakeHandler.java b/src/main/java/frc/robot/handlers/IntakeHandler.java index 05d745f..4028bef 100644 --- a/src/main/java/frc/robot/handlers/IntakeHandler.java +++ b/src/main/java/frc/robot/handlers/IntakeHandler.java @@ -4,14 +4,19 @@ package frc.robot.handlers; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants; -import frc.robot.handlers.ShooterHandler.ShooterState; -import frc.robot.handlers.StateSubsystem.State; +import frc.robot.subsystems.Elevator; import frc.robot.subsystems.IntakeSubsystem; public class IntakeHandler extends SubsystemBase implements StateSubsystem { + public enum IntakeState implements State { FASTINTAKE, SLOWINTAKE, @@ -21,11 +26,16 @@ public enum IntakeState implements State { } private static IntakeHandler instance; - private IntakeSubsystem intake = new IntakeSubsystem(); + private Elevator lintake; + private IntakeSubsystem intake = new IntakeSubsystem(); private IntakeState desiredState = IntakeState.OFF; private IntakeState currentState = IntakeState.OFF; - /** Creates a new IntakeHandler. */ + public void initialize(Elevator lintake) { + this.lintake = lintake; + } + + private IntakeHandler() {} public static IntakeHandler getInstance(){ @@ -48,11 +58,11 @@ public void handleStateTransition() { update(); } - @Override + @Override public void update() { switch (desiredState) { case FASTINTAKE: - intake.set(Constants.IntakeConstants.FastIntake); + intake.set(Constants.IntakeConstants.FastIntake); break; case SLOWINTAKE: intake.set(Constants.IntakeConstants.SlowIntake); @@ -79,7 +89,8 @@ public IntakeState getCurrentState() { @Override public void periodic() { + SmartDashboard.putString("IntakeHandlerState", currentState.toString()); update(); // This method will be called once per scheduler run } -} +} diff --git a/src/main/java/frc/robot/handlers/IntakeSlideHandler.java b/src/main/java/frc/robot/handlers/IntakeSlideHandler.java new file mode 100644 index 0000000..59a7366 --- /dev/null +++ b/src/main/java/frc/robot/handlers/IntakeSlideHandler.java @@ -0,0 +1,121 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.handlers; + +import static edu.wpi.first.units.Units.Rotations; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.subsystems.Elevator; + +public class IntakeSlideHandler extends SubsystemBase implements StateSubsystem { + + public enum IntakeSlideState implements State { + OUT, + MIDDLE, + IN, + SLOWIN, + BRAKE, + REZEROIN, + REZEROOUT, + OSCILLATE + } + + private static IntakeSlideHandler instance; + private final Elevator intakeSlide = new Elevator(); //name it correct + + + Trigger intakeAboveFive = new Trigger(() -> intakeSlide.getPosition().in(Rotations) > 5.0); + + + private IntakeSlideState desiredState = IntakeSlideState.IN; + private IntakeSlideState currentState = IntakeSlideState.IN; + /** Creates a new IntakeHandler. */ + private IntakeSlideHandler() {} + + public static IntakeSlideHandler getInstance(){ + if (instance == null){ + instance = new IntakeSlideHandler(); + } + return instance; + } + + @Override + public void setDesiredState(State state){ + if (state instanceof IntakeSlideState IntakeSlideState && desiredState != IntakeSlideState) { + desiredState = IntakeSlideState; + } + } + + @Override + public void handleStateTransition() { + update(); + } + + boolean isAtLowSetpoint = false; + @Override + public void update() { + if((currentState != desiredState)){ + switch (desiredState) { + case OUT: + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.OUT).schedule(); + break; + case MIDDLE: + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.Middle).schedule(); + break; + case IN: //In also re-zeros it once it reaches "in" + new SequentialCommandGroup( + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.IN) + .until(() -> isAtLowSetpoint), + intakeSlide.calibrateZeroIn() + ).schedule(); + break; + case SLOWIN: + new SequentialCommandGroup( + Commands.waitSeconds(1), + intakeSlide.manualDrive(() -> -0.125).until(intakeSlide.isHardStop).withTimeout( 5) + ).schedule(); + //intakeSlide.goToSetpoint(() -> Elevator.Setpoint.Middle).schedule(); + break; + case OSCILLATE: + new SequentialCommandGroup( + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.OUT).withTimeout(0.5), + Commands.waitSeconds(0.5), + intakeSlide.goToSetpoint(() -> Elevator.Setpoint.IN).withTimeout(0.5) + ).repeatedly().schedule(); + break; + case BRAKE: + intakeSlide.holdPosition(); + break; + case REZEROIN: + intakeSlide.calibrateZeroIn().schedule(); + break; + case REZEROOUT: + intakeSlide.calibrateZeroOut().schedule(); + break; + default: + intakeSlide.holdPosition(); + break; + } + currentState = desiredState; + } + } + + public IntakeSlideState getCurrentState() { + return currentState; + } + + @Override + public void periodic() { + update(); + isAtLowSetpoint = intakeSlide.isAtSetpoint(Elevator.Setpoint.IN); + SmartDashboard.putBoolean("isAtLowSetpoint", isAtLowSetpoint); + SmartDashboard.putString("IntakeSlide State", currentState.toString()); + // This method will be called once per scheduler run + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/ShooterHandler.java b/src/main/java/frc/robot/handlers/ShooterHandler.java index c799c27..a3f5e82 100644 --- a/src/main/java/frc/robot/handlers/ShooterHandler.java +++ b/src/main/java/frc/robot/handlers/ShooterHandler.java @@ -2,8 +2,6 @@ import static edu.wpi.first.units.Units.RPM; -import java.time.chrono.IsoChronology; - import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.units.AngularVelocityUnit; import edu.wpi.first.units.measure.AngularVelocity; @@ -14,16 +12,14 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.ShooterSubsystem; -import frc.robot.RobotContainer; import frc.robot.utils.HubShiftUtil; import frc.robot.utils.HubShiftUtil.ShiftInfo; +import frc.robot.RobotContainer; public class ShooterHandler extends SubsystemBase implements StateSubsystem { - public Joystick leftJoystick = new Joystick(2); - public enum ShooterState implements State { SHOOTING, @@ -35,13 +31,12 @@ public enum ShooterState implements State { } private static ShooterHandler instance; - private CommandSwerveDrivetrain drivetrain; - private ShooterSubsystem shooter; + // AFTER: + private CommandSwerveDrivetrain drivetrain; + private BangBangShooterSubsystem shooter; private ShooterState desiredState = ShooterState.OFF; private ShooterState currentState = ShooterState.OFF; - private Command shooterCommand = null; - private ShooterHandler() {} @@ -52,17 +47,17 @@ public static ShooterHandler getInstance() { return instance; } - public void initialize(CommandSwerveDrivetrain drivetrain, ShooterSubsystem shooter) { + public void initialize(CommandSwerveDrivetrain drivetrain, BangBangShooterSubsystem shooter) { this.drivetrain = drivetrain; this.shooter = shooter; } - AngularVelocity TuneablefastShot = Constants.ShooterConstants.FastShot; - public void adjustFastShot(double value){ - TuneablefastShot = TuneablefastShot.plus(RPM.of(value)); + double TuneablefastShot = 40; + public void adjustFastShot(double valu){ + TuneablefastShot = TuneablefastShot + (valu); if(currentState == ShooterState.TUNING){ - shooter.setVelocity(TuneablefastShot); + shooter.setTargetVelocity(TuneablefastShot); } } @@ -83,6 +78,7 @@ public void setDesiredState(State state) { @Override public void handleStateTransition() { + // Optional: delegate to update update(); } @@ -92,64 +88,35 @@ public void update() { System.out.println("ERROR: ShooterHandler not initialized! Call initialize() first."); return; } - if (currentState != desiredState) { - System.out.println(" State changing from " + currentState + " to " + desiredState); - handleStateChange(); + if (currentState != desiredState || currentState == ShooterState.SHOOTING) { + handleStateChange(); } - updateContinuousStates(); } - Double DistMeters = 0.0; + double DistMeters = 0; + private void handleStateChange(){ switch (desiredState) { case SHOOTING: - - - ShiftInfo shiftInfo = HubShiftUtil.getOfficialShiftInfo(); - boolean earlyShoot = !shiftInfo.active() && shiftInfo.remainingTime() <= 1.0 && HubShiftUtil.isNextShiftActive(); - System.out.println("active: " + shiftInfo.active() + " | remainingTime: " + shiftInfo.remainingTime() + " | earlyShoot: " + earlyShoot); - - if (!shiftInfo.active() && !earlyShoot) { - CommandScheduler.getInstance().schedule(shooter.stop()); - break; - } - break; - - - - - + shooter.setTargetVeloDistance(DistMeters); + break; case SLOW: - CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.SlowShot)); + shooter.setTargetVelocity(Constants.ShooterConstants.SlowShot); break; case FAST: - CommandScheduler.getInstance().schedule(shooter.setVelocity(Constants.ShooterConstants.FastShot)); + shooter.setTargetVelocity(Constants.ShooterConstants.FastShot); break; case TUNING: + shooter.setTargetVelocity(TuneablefastShot); break; - default: - CommandScheduler.getInstance().schedule(shooter.stop()); + case OFF: + shooter.coast(); break; - } - currentState = desiredState; - } - - private void updateContinuousStates() { - switch (currentState) { - case SHOOTING: - - double DistMeters = drivetrain.GetFutureDistMeters(); - AngularVelocity targetSpeed = shooter.getCalcedRPM(DistMeters); - CommandScheduler.getInstance().schedule(shooter.setVelocity(targetSpeed)); - break; - - case TUNING: - CommandScheduler.getInstance().schedule(shooter.setVelocity(TuneablefastShot)); - break; - default: + //CommandScheduler.getInstance().schedule(shooter.stop()); break; } + currentState = desiredState; } public ShooterState getCurrentState() { @@ -161,5 +128,5 @@ public void periodic() { DistMeters = drivetrain.getDistance(); update(); // Handle state transitions SmartDashboard.putString("ShooterState", currentState.toString()); -} + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/Superstructure.java b/src/main/java/frc/robot/handlers/Superstructure.java index b47c0d0..3d0dd0f 100644 --- a/src/main/java/frc/robot/handlers/Superstructure.java +++ b/src/main/java/frc/robot/handlers/Superstructure.java @@ -6,15 +6,17 @@ import frc.robot.Constants.ShooterConstants; import frc.robot.handlers.IndexerHighHandler.IndexerHighState; import frc.robot.handlers.IndexerLowHandler.IndexerLowState; -import frc.robot.subsystems.ClimbSubsystem; +import frc.robot.handlers.IntakeSlideHandler.IntakeSlideState; +import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; import frc.robot.subsystems.IndexerLowSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.subsystems.ShooterSubsystem; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import static edu.wpi.first.units.Units.Degrees; @@ -35,15 +37,12 @@ public enum SuperstructureState { SPINUPSLOW, SPINUPFAST, FASTSHOT, - CLIMBPREP, - CLIMBED + AIM } private static Superstructure instance; - private ShooterSubsystem shooter; + private BangBangShooterSubsystem shooter; private CommandSwerveDrivetrain drivetrain; - private ClimbSubsystem climber; - //private final ShooterSubsystem Shooter = new ShooterSubsystem(); private final ShooterHandler shooterHandler = ShooterHandler.getInstance(); @@ -51,6 +50,8 @@ public enum SuperstructureState { //private final IntakeSubsystem Intake = new IntakeSubsystem(); private final IntakeHandler intakeHandler = IntakeHandler.getInstance(); + private final IntakeSlideHandler intakeSlideHandler = IntakeSlideHandler.getInstance(); + //private final HopperSubsystem Hopper = new HopperSubsystem(); private final HopperHandler hopperHandler = HopperHandler.getInstance(); @@ -60,11 +61,13 @@ public enum SuperstructureState { private final DriveHandler driveHandler = DriveHandler.getInstance(); - private final ClimbHandler climbHandler = ClimbHandler.getInstance(); - private SuperstructureState desiredState = SuperstructureState.IDLE; private SuperstructureState currentState = SuperstructureState.IDLE; private Angle targetAngle = Degrees.of(0); + + boolean DTaimed = false; + boolean ShooterAtVelo = false; + boolean WaitTimed = false; public static Superstructure getInstance(){ @@ -77,10 +80,9 @@ public static Superstructure getInstance(){ /** * Initialize superstructure with required subsystems */ - public void initialize(ShooterSubsystem shooter, CommandSwerveDrivetrain drivetrain, ClimbSubsystem climber) { + public void initialize(BangBangShooterSubsystem shooter, CommandSwerveDrivetrain drivetrain) { this.shooter = shooter; this.drivetrain = drivetrain; - this.climber = climber; } private Superstructure() {} @@ -104,7 +106,7 @@ private void handleStateTransition() { indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.BRAKE); break; case INTAKE: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -112,90 +114,73 @@ private void handleStateTransition() { hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - driveHandler.setDesiredState(DriveHandler.DriveState.SNAKE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); break; case SPINUP: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); + shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); //change to shooting intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + ShooterAtVelo = false; break; case STATIONARYSHOT: shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTINTAKE); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); break; - case SPINUPSLOW: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.SLOW); + case TUNING: //dont use + shooterHandler.setDesiredState(ShooterHandler.ShooterState.TUNING); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); - hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); - indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); - indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); + driveHandler.setDesiredState(DriveHandler.DriveState.BUMP_LOCK); break; - case SLOWSHOT: + case SLOWSHOT: //dont use shooterHandler.setDesiredState(ShooterHandler.ShooterState.SLOW); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.SLOW); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); - driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + //driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); break; - case SPINUPFAST: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); + case SPINUPFAST: //dont use + shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); //SWITCHEWD TO TUNING SWITCH BACK TO FAST SHOT intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerHighHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); break; - case FASTSHOT: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); + case FASTSHOT: //dont use + shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); //SWITCHED TO TUNABLE FAST SHOT SWITCH BACK TO FAST intakeHandler.setDesiredState(IntakeHandler.IntakeState.SLOWINTAKE); hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); - driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + //driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); + //intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); break; - case CLIMBPREP: + case REVERSE: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTREVERSE); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.LOW); break; - case CLIMBED: - climbHandler.setDesiredState(ClimbHandler.ClimbState.LOWPULL); - break; - case TUNING: - shooterHandler.setDesiredState(ShooterHandler.ShooterState.TUNING); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); - hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); - indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); - indexerHighHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); - driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); - break; - case REVERSE: + case AIM: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); - intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTREVERSE); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); break; case OFF: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -204,7 +189,7 @@ private void handleStateTransition() { indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.IN); break; default: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -213,7 +198,7 @@ private void handleStateTransition() { indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); - climbHandler.setDesiredState(ClimbHandler.ClimbState.OFF); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.IN); break; } currentState = desiredState; @@ -227,23 +212,23 @@ public void periodic() { return; } - if (currentState == SuperstructureState.SPINUP && (shooter.isAtTargetVelo() && drivetrain.isAimedAtTarget())){ //checks if its at target velo and angle - setDesiredState(SuperstructureState.STATIONARYSHOT); - } + DTaimed = drivetrain.isAimedAtTarget(); + ShooterAtVelo = shooter.atTargetVelo(); - if (currentState == SuperstructureState.CLIMBPREP && climbHandler.extendedClimb){ - setDesiredState(SuperstructureState.CLIMBED); + + if (currentState == SuperstructureState.SPINUP && ShooterAtVelo && DTaimed){ //checks if its at target velo and angle + setDesiredState(SuperstructureState.STATIONARYSHOT); + WaitTimed = false; } + + + SmartDashboard.putString("SuperState", currentState.toString()); - /*if (currentState == SuperstructureState.CLIMBPREP && climber.atTarget()){ - setDesiredState(SuperS); - }*/ - - SmartDashboard.putBoolean("shooterAtVelo?", shooter.isAtTargetVelo()); - SmartDashboard.putBoolean("Drivetrain aimed?", drivetrain.isAimedAtTarget()); + SmartDashboard.putBoolean("shooterAtVelo?", ShooterAtVelo); + SmartDashboard.putBoolean("Drivetrain aimed?",DTaimed); + SmartDashboard.putBoolean("Timer", WaitTimed); } - public SuperstructureState getCurrentState() { return currentState; } diff --git a/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java b/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java new file mode 100644 index 0000000..5ef14b0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java @@ -0,0 +1,179 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; + +import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + + +public class BangBangShooterSubsystem extends SubsystemBase { + /** Creates a new BangBangShooterSubsystem. */ + private final TalonFX ShooterMotor = new TalonFX(Constants.ShooterConstants.ShooterLeaderID, "Upper"); + private final TalonFX ShooterMotor2 = new TalonFX(Constants.ShooterConstants.ShooterFollowerID, "Upper"); + private final TalonFX ShooterMotorRev3 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversed2ID, "Upper"); + private final TalonFX ShooterMotorRev4 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversedID, "Upper"); + + private final BangBangController controllerBangBang = new BangBangController(); + + private static final InterpolatingDoubleTreeMap Shooter1Map = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap Shooter2Map = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap Shooter3Map = new InterpolatingDoubleTreeMap(); + private static final InterpolatingDoubleTreeMap Shooter4Map = new InterpolatingDoubleTreeMap(); + + //inches to center hub from Robot orign , RPS + static { + Shooter1Map.put(138.4 ,100.0); + Shooter1Map.put(121.4 ,77.0); + Shooter1Map.put(104.6 ,70.0); + Shooter1Map.put(85.8, 64.0); + Shooter1Map.put(69.0, 60.0); + + Shooter2Map.put(138.4 ,100.0); + Shooter2Map.put(121.4 ,77.0); + Shooter2Map.put(104.6 ,70.0); + + Shooter3Map.put(138.4 ,100.0); + Shooter3Map.put(121.4 ,77.0); + Shooter3Map.put(104.6 ,70.0); + + Shooter4Map.put(138.4 ,100.0); + Shooter4Map.put(121.4 ,77.0); + Shooter4Map.put(104.6 ,70.0); + + + + } + + + private final SimpleMotorFeedforward feedFoward = new SimpleMotorFeedforward(Constants.ShooterConstants.kS, Constants.ShooterConstants.kV); + private final VoltageOut voltageRequest = new VoltageOut(0); + + private double targetVeloRPS1 = 0; + private double targetVeloRPS2 = 0; + private double targetVeloRPS3 = 0; + private double targetVeloRPS4 = 0; + + private boolean shooterEnabled = false; + + public BangBangShooterSubsystem() { + + ShooterMotor.setNeutralMode(NeutralModeValue.Coast); + ShooterMotor2.setNeutralMode(NeutralModeValue.Coast); + ShooterMotorRev3.setNeutralMode(NeutralModeValue.Coast); + ShooterMotorRev4.setNeutralMode(NeutralModeValue.Coast); + + controllerBangBang.setTolerance(Constants.ShooterConstants.RPSTolarance); + } + + + private static BangBangShooterSubsystem instance; + public static BangBangShooterSubsystem getInstance(){ + if (instance == null){ + instance = new BangBangShooterSubsystem(); + } + return instance; + } + + public void setTargetVelocity(double VelocityRPS) { + if (VelocityRPS > Constants.ShooterConstants.RPSHardStop) { + targetVeloRPS1 = Constants.ShooterConstants.RPSHardStop; + targetVeloRPS2 = Constants.ShooterConstants.RPSHardStop; + targetVeloRPS3 = Constants.ShooterConstants.RPSHardStop; + targetVeloRPS4 = Constants.ShooterConstants.RPSHardStop; + } + + else { + targetVeloRPS1 = VelocityRPS; + targetVeloRPS2 = VelocityRPS; + targetVeloRPS3 = VelocityRPS; + targetVeloRPS4 = VelocityRPS; + } + shooterEnabled = true; + } + + public void setTargetVeloDistance(double distance) { + double inches = distance * 39.3701; + targetVeloRPS1 = Shooter1Map.get(inches); + targetVeloRPS2 = Shooter2Map.get(inches); + targetVeloRPS3 = Shooter3Map.get(inches); + targetVeloRPS4 = Shooter4Map.get(inches); + + if (targetVeloRPS1 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS1 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS2 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS2 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS3 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS3 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS4 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS4 = Constants.ShooterConstants.RPSHardStop;} + + shooterEnabled = true; + } + + public void coast(){ + shooterEnabled = false; + targetVeloRPS1 = 0; + targetVeloRPS2 = 0; + targetVeloRPS3 = 0; + targetVeloRPS4 = 0; + } + + public boolean atTargetVelo() { + boolean atTargBelo = Math.abs(ShooterMotor.getVelocity().getValueAsDouble()) >= targetVeloRPS1-2.0; + return atTargBelo; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + if (!shooterEnabled){ + ShooterMotor.setControl(voltageRequest.withOutput(0)); + ShooterMotor2.setControl(voltageRequest.withOutput(0)); + ShooterMotorRev3.setControl(voltageRequest.withOutput(0)); + ShooterMotorRev4.setControl(voltageRequest.withOutput(0)); + return; + } + + double currentVelocity1RPS = Math.abs(ShooterMotor.getVelocity().getValueAsDouble()); + double currentVelocity2RPS = Math.abs(ShooterMotor2.getVelocity().getValueAsDouble()); + double currentVelocity3RPS = Math.abs(ShooterMotorRev3.getVelocity().getValueAsDouble()); + double currentVelocity4RPS = Math.abs(ShooterMotorRev4.getVelocity().getValueAsDouble()); + + double bangBangVolts1 = controllerBangBang.calculate(currentVelocity1RPS, targetVeloRPS1)*12; + double bangBangVolts2 = controllerBangBang.calculate(currentVelocity2RPS, targetVeloRPS2)*12; + double bangBangVolts3 = controllerBangBang.calculate(currentVelocity3RPS, targetVeloRPS3)*12; + double bangBangVolts4 = controllerBangBang.calculate(currentVelocity4RPS, targetVeloRPS4)*12; + + double feedfowardVolts1 = feedFoward.calculate(targetVeloRPS1)*0.9; + double feedfowardVolts2 = feedFoward.calculate(targetVeloRPS2)*0.9; + double feedfowardVolts3 = feedFoward.calculate(targetVeloRPS3)*0.9; + double feedfowardVolts4 = feedFoward.calculate(targetVeloRPS4)*0.9; + + ShooterMotor.setControl(voltageRequest.withOutput(-(bangBangVolts1 + feedfowardVolts1))); //idk bru bang bang volts 1 + ShooterMotor2.setControl(voltageRequest.withOutput(-(bangBangVolts2 + feedfowardVolts2))); //backwards in phy tuner + ShooterMotorRev3.setControl(voltageRequest.withOutput(-(bangBangVolts3 + feedfowardVolts3))); //- + ShooterMotorRev4.setControl(voltageRequest.withOutput(-(bangBangVolts4 + feedfowardVolts4))); //- + + + SmartDashboard.putNumber("Shooter1Volts", bangBangVolts1 + feedfowardVolts1); + SmartDashboard.putNumber("Shooter2Volts", bangBangVolts2 + feedfowardVolts2); + SmartDashboard.putNumber("Shooter3Volts", bangBangVolts3 + feedfowardVolts3); + SmartDashboard.putNumber("Shooter4Volts", bangBangVolts4 + feedfowardVolts4); + + SmartDashboard.putNumber("Shooter1RPS", currentVelocity1RPS); + SmartDashboard.putNumber("Shooter2RPS", currentVelocity2RPS); + SmartDashboard.putNumber("Shooter3RPS", currentVelocity3RPS); + SmartDashboard.putNumber("Shooter4RPS", currentVelocity4RPS); + + SmartDashboard.putNumber("TargetRPS1", targetVeloRPS1); + SmartDashboard.putNumber("TargetRPS2", targetVeloRPS2); + SmartDashboard.putNumber("TargetRPS3", targetVeloRPS3); + SmartDashboard.putNumber("TargetRPS4", targetVeloRPS4); + } +} diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 81b2763..2ea0a02 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -1,50 +1,49 @@ package frc.robot.subsystems; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Second; +import static edu.wpi.first.units.Units.Volts; import java.util.Optional; import java.util.function.Supplier; import com.ctre.phoenix6.SignalLogger; import com.ctre.phoenix6.Utils; -import com.ctre.phoenix6.mechanisms.swerve.LegacySwerveRequest.FieldCentric; import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants; import com.ctre.phoenix6.swerve.SwerveModuleConstants; import com.ctre.phoenix6.swerve.SwerveRequest; - import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.config.PIDConstants; import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.controllers.PPHolonomicDriveController; + import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj.Notifier; import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.Subsystem; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import edu.wpi.first.units.*; import frc.robot.Constants; import frc.robot.Constants.DriveConstants; -import frc.robot.LimelightHelpers.RawDetection; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; -import frc.robot.utils.HubShiftUtil; -import edu.wpi.first.wpilibj.smartdashboard.Field2d; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements * Subsystem so it can easily be used in command-based projects. @@ -78,8 +77,6 @@ public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Su private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains(); private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation(); - - /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */ private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine( new SysIdRoutine.Config( @@ -350,15 +347,8 @@ public Optional samplePoseAt(double timestampSeconds) { return super.samplePoseAt(Utils.fpgaToCurrentTime(timestampSeconds)); } - public static boolean isInAllianceZone(Pose2d robotPose){ - double robotX = robotPose.getX(); - Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); - if (alliance == Alliance.Red) { - return robotX > Units.Inches.of(469.11).in(Units.Meters); - } else { - return robotX < Units.Inches.of(182.11).in(Units.Meters); - } -} + // --------------------- END GENERATED CODE ------------------- + // Get hub pose based on alliance public static Pose3d getHubPose() { @@ -367,65 +357,32 @@ public static Pose3d getHubPose() { .orElse(Constants.FieldConstants.blueHubPose); // Default to blue if alliance unknown } - public static Pose2d getTargetPose(double robotX, double robotY) { - SmartDashboard.putNumber("Robot/X", robotX); - SmartDashboard.putNumber("Robot/Y", robotY); - - Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); - final double thresholdY = Units.Inches.of(158.32).in(Units.Meters); //horizontal middle field line - - if (alliance == Alliance.Red) { - double thresholdX = Units.Inches.of(469.11).in(Units.Meters); - SmartDashboard.putNumber("Threshold/X", thresholdX); - - - - /*if (!HubShiftUtil.getOfficialShiftInfo().active() && isInAllianceZone(robotX)) { - return robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose; - } - commented out bcz not usable atm, basically returns passing position - regardless of pose when not your shift*/ - - return !isInAllianceZone(new Pose2d(robotX, robotY, new Rotation2d())) - ? (robotY > thresholdY ? Constants.FieldConstants.redTargetHighPose : Constants.FieldConstants.redTargetLowPose) - : Constants.FieldConstants.redHubPose.toPose2d(); -} else { - double thresholdX = Units.Inches.of(182.11).in(Units.Meters); - SmartDashboard.putNumber("Threshold/X", thresholdX); - - if (!HubShiftUtil.getOfficialShiftInfo().active()) { - return robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose; + public static Distance getCloseBumpY(Pose2d currentPose){ + if (currentPose.getMeasureY().gt(Inches.of(158.845))){ //if closer to blue left + return Inches.of(218.84); + } + else{ + return Inches.of(98.84); //closer to blue right Bump + } } - return isInAllianceZone(new Pose2d(robotX, robotY, new Rotation2d())) - ? (robotY > thresholdY ? Constants.FieldConstants.blueTargetHighPose : Constants.FieldConstants.blueTargetLowPose) - : Constants.FieldConstants.blueHubPose.toPose2d(); + public static boolean isInAllianceZone(Pose2d robotPose){ + double robotX = robotPose.getX(); + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance == Alliance.Red) { + return robotX > Inches.of(469.11).in(Meters); + } else { + return robotX < Inches.of(182.11).in(Meters); + } } -} + + /** * Calculates the closest point on the predefined circle to the current robot pose. * * @return The closest point on the circle to the current robot position */ - public Pose2d getClosestPointOnHub() { - - Pose2d hub = getHubPose().toPose2d(); - Pose2d currentPose = getPose(); - - // Calculate vector from center to current position - double deltaX = currentPose.getX() - hub.getX(); - double deltaY = currentPose.getY() - hub.getY(); - - // Calculate angle from center to current position - double angle = Math.atan2(deltaY, deltaX); - - // Calculate the closest point on the circle - double closestX = hub.getX() + Constants.ShooterConstants.ShootingDistance * Math.cos(angle); - double closestY = hub.getY() + Constants.ShooterConstants.ShootingDistance * Math.sin(angle); - - return new Pose2d(closestX, closestY, new Rotation2d(angle)); - } public static final PIDController rotationController = getRotationController(); @@ -449,55 +406,90 @@ public double getDistance(){ return currentDistance; } - - + public Pose2d getTargetPose(Pose2d currentPose2d){ + final double thresholdY = Inches.of(158.32).in(Meters); //horizontal middle field line + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + if (alliance == Alliance.Red){ + if(isInAllianceZone(currentPose2d)){ + return getHubPose().toPose2d(); + } + else if (currentPose2d.getY() >= thresholdY){ + return Constants.FieldConstants.redTargetHighPose; + } + else { + return Constants.FieldConstants.redTargetLowPose; + } + } + if (alliance == Alliance.Blue){ + if(isInAllianceZone(currentPose2d)){ + return getHubPose().toPose2d(); + } + else if (currentPose2d.getY() >= thresholdY){ + return Constants.FieldConstants.blueTargetHighPose; + } + else { + return Constants.FieldConstants.blueTargetLowPose; + } + } + return null; + } Pose2d ShootingLocation; - public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { + public Command headingLocktoHub(CommandXboxController controller, double maxSpeed, double maxAngularRate) { return applyRequest(() -> { - + + // Get current pose and target hub position Pose2d drivePose = getState().Pose; - Pose2d targetPose = getTargetPose(drivePose.getX(), drivePose.getY()); + Pose2d targetPose = getTargetPose(drivePose); ShootingLocation = targetPose; - + + // Calculate angle from hub to robot Translation2d toRobot = drivePose.getTranslation().minus(targetPose.getTranslation()); + + /* + if (toRobot.getNorm() < 0.1) { // Within 10cm of hub + double veloX = -controller.getLeftY(); + if (Math.abs(veloX) < 0.1) veloX = 0; + + double veloY = -controller.getLeftX(); + if (Math.abs(veloY) < 0.1) veloY = 0; + + return alignRequest + .withVelocityX(veloX * maxSpeed) + .withVelocityY(veloY * maxSpeed) + .withRotationalRate(0); + } */ + Rotation2d angleToRobot = toRobot.getAngle(); - Rotation2d desiredAngle = angleToRobot.rotateBy(Rotation2d.k180deg); + // Calculate desired rotation (face the hub) + Rotation2d desiredAngle = angleToRobot.rotateBy(Rotation2d.k180deg); // Face toward hub | Take RotateBy out for back to face Hub Rotation2d currentAngle = drivePose.getRotation(); - - if (Math.abs(desiredAngle.getDegrees() - currentAngle.getDegrees()) <= 1) { - desiredAngle = currentAngle; - } + // Calculate rotational rate to face hub double rotationalRate = rotationController.calculate( currentAngle.getRadians(), desiredAngle.getRadians() ); - double veloX = -controller.getLeftY(); - if (Math.abs(veloX) < 0.1) veloX = 0; + if (Math.abs(veloX) < 0.1 ){ + veloX = 0; + } double veloY = -controller.getLeftX(); - if (Math.abs(veloY) < 0.1) veloY = 0; - - if (tuning.equals("PovLeft")) { veloX = 0; veloY = 0.3; } - if (tuning.equals("PovRight")) { veloX = 0; veloY = -0.3; } - - SmartDashboard.putNumber("Target/X", targetPose.getX()); - SmartDashboard.putNumber("Target/Y", targetPose.getY()); - SmartDashboard.putNumber("Robot/X", drivePose.getX()); - SmartDashboard.putNumber("Robot/Y", drivePose.getY()); - - return alignRequest - .withVelocityX(veloX * maxSpeed) - .withVelocityY(veloY * maxSpeed) - .withRotationalRate(rotationalRate * maxAngularRate); + if (Math.abs(veloY) < 0.1 ){ + veloY = 0; + } + // Apply the request: radial (distance maintenance) + tangential (circling) + return alignRequest + .withVelocityX(veloX * maxSpeed) + .withVelocityY(veloY * maxSpeed) + .withRotationalRate(rotationalRate * maxAngularRate*1.5); }); } - private final SwerveRequest.RobotCentric chaseRequest = new SwerveRequest.RobotCentric(); +private final SwerveRequest.RobotCentric chaseRequest = new SwerveRequest.RobotCentric(); public Command detectChase(Vision vision, double maxSpeed, double maxAngularRate) { return applyRequest(() -> { @@ -533,38 +525,20 @@ public Command detectChase(Vision vision, double maxSpeed, double maxAngularRate }); } - public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ - return applyRequest(() ->{ - return alignRequest.withVelocityX(-joystick.getLeftY() * MaxSpeed).withDeadband(Constants.DriveConstants.TranslationDeadband) // Drive forward with negative Y (forward) DriveStraight = robot centric - .withVelocityY(-joystick.getLeftX() * MaxSpeed).withDeadband(Constants.DriveConstants.TranslationDeadband) // Drive left with negative X (left) - .withRotationalRate(-joystick.getRightX() * MaxAngularRate).withDeadband(Constants.DriveConstants.RotationDeadband);} // Drive counterclockwise with negative X (left) - ); } - - double futureDistance = 0.0; - public double GetFutureDistMeters(){ - Pose2d robotPose = getPose(); - double distance = getDistance(); - ChassisSpeeds fieldSpeeds = getState().Speeds; - Pose2d actualHub = getHubPose().toPose2d(); - - // Convert field speeds to robot-relative - ChassisSpeeds robotSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds( - fieldSpeeds.vxMetersPerSecond, - fieldSpeeds.vyMetersPerSecond, - fieldSpeeds.omegaRadiansPerSecond, - robotPose.getRotation().unaryMinus() // Inverse rotation - ); - - // Step 2: Predict where ROBOT will be - double futureRobotX = robotPose.getX() + (robotSpeeds.vxMetersPerSecond); - double futureRobotY = robotPose.getY() + (robotSpeeds.vyMetersPerSecond); - Translation2d futureRobotPos = new Translation2d(futureRobotX, futureRobotY); + +public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ + return applyRequest(() -> { + // Apply 10% deadband to joystick inputs + double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); + double ySpeed = MathUtil.applyDeadband(-joystick.getLeftX(), 0.1); + double rotSpeed = MathUtil.applyDeadband(-joystick.getRightX(), 0.1); - // Step 3: Calculate angle from future robot position to hub - Translation2d futureToHub = actualHub.getTranslation().minus(futureRobotPos); - futureDistance = futureToHub.getNorm(); - return futureDistance; - } + return alignRequest + .withVelocityX(xSpeed * MaxSpeed) + .withVelocityY(ySpeed * MaxSpeed) + .withRotationalRate(rotSpeed * MaxAngularRate); + }); +} //BASED ON MECH A PRAISE THE FRC GODS FOR OPEN ALLIANCE public Command shootOnTheMoveIterative(CommandXboxController controller, double maxSpeed, double maxAngularRate, String tuning) { @@ -651,20 +625,6 @@ public Command shootOnTheMoveIterative(CommandXboxController controller, double // Visualization ShootingLocation = new Pose2d(lookaheadShooterPosition, aimAngle); - // Debug - SmartDashboard.putNumber("Shot/Robot Vx", robotRelativeSpeeds.vxMetersPerSecond); - SmartDashboard.putNumber("Shot/Robot Vy", robotRelativeSpeeds.vyMetersPerSecond); - SmartDashboard.putNumber("Shot/Field Vx", fieldVelocity.vxMetersPerSecond); - SmartDashboard.putNumber("Shot/Field Vy", fieldVelocity.vyMetersPerSecond); - SmartDashboard.putNumber("Shot/Lookahead Distance", lookaheadDistance); - SmartDashboard.putNumber("Shot/Time of Flight", timeOfFlight); - SmartDashboard.putNumber("Shot/Current Angle", currentAngle.getDegrees()); - SmartDashboard.putNumber("Shot/Aim Angle", aimAngle.getDegrees()); - SmartDashboard.putNumber("Shot/Rotational Rate", rotationalRate); - SmartDashboard.putNumber("Shot/Prediction Offset X", lookaheadShooterPosition.getX() - currentPose.getX()); - SmartDashboard.putNumber("Shot/Prediction Offset Y", lookaheadShooterPosition.getY() - currentPose.getY()); - SmartDashboard.putNumber("Shot/Angle Error Deg", aimAngle.minus(currentAngle).getDegrees()); - return alignRequest .withVelocityX(veloX * maxSpeed) .withVelocityY(veloY * maxSpeed) @@ -679,7 +639,7 @@ public boolean isAimedAtTarget() { // Calculate required aim angle (same as headingLocktoHub) Translation2d target = getHubPose().toPose2d().getTranslation(); Translation2d toTarget = target.minus(currentPose.getTranslation()); - Rotation2d targetAngle = toTarget.getAngle(); // Face toward hub + Rotation2d targetAngle = toTarget.getAngle(); // Face towards from hub .plus(Krot180) or sum idk // Calculate angle error double errorDegrees = Math.abs(targetAngle.minus(currentAngle).getDegrees()); @@ -688,6 +648,37 @@ public boolean isAimedAtTarget() { return errorDegrees <= Constants.DriveConstants.RotationalToleranceDegrees; } + public Command bumpLockCommand(SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, Double MaxSpeed, double MaxAngularRate){ + return applyRequest(() -> { + double closeTrench = (double)getCloseBumpY(drivetrain.getPose()).in(Meters); + double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); + distanceController.setSetpoint(closeTrench); + double yVel = distanceController.calculate(drivetrain.getPose().getY()); + if (distanceController.atSetpoint()) { + yVel = 0; + } + + Rotation2d currentRot = drivetrain.getState().Pose.getRotation(); + + // Returns whichever of 0 or 180 the robot is facing closest to + Rotation2d rotSetpoint = Math.abs(currentRot.getDegrees()) < 90 + ? Rotation2d.kZero // closer to 0 + : Rotation2d.fromDegrees(180); // closer to 180 + rotationController.setSetpoint(rotSetpoint.getRadians()); + + double rotSpeedToStraight = + rotationController.calculate(drivetrain.getPose().getRotation().getRadians()); + if (rotationController.atSetpoint()) { + rotSpeedToStraight = 0; + } + + return drive + .withVelocityX(xSpeed*MaxSpeed) // + .withVelocityY(-yVel *MaxSpeed) + .withRotationalRate(rotSpeedToStraight*MaxAngularRate); + }); + } + public Command getSnakeDriveCommand(SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, Double MaxSpeed, double MaxAngularRate) { return applyRequest(() -> { // Get joystick inputs @@ -728,17 +719,13 @@ public Command getSnakeDriveCommand(SwerveRequest.FieldCentric drive, CommandSwe .withRotationalRate(rotationRate); }); } -/*Open limelight-front.local:5801 in a browser -Create a neural detector pipeline -Check if there's a built-in model for your game piece — if yes, use it, if no, you'll need to train one -Verify you can see detections in the camera stream */ @Override public void periodic() { - SmartDashboard.putNumber("ShootingDistance", Constants.ShooterConstants.ShootingDistance*3.28084); //3.28084 Feet per Meter (converting Meters to Feet) if(ShootingLocation != null){field.getObject("Shooting Target").setPose(ShootingLocation);} field.setRobotPose(getPose()); + SmartDashboard.putNumber("distanceToCenterHubInches", getDistance() * 39.3701); /* * Periodically try to apply the operator perspective. diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java new file mode 100644 index 0000000..b273748 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -0,0 +1,299 @@ +/* Generated by Phoenix Tuner X */ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + +import java.util.Set; +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.Utils; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.sim.ChassisReference; +import com.ctre.phoenix6.sim.TalonFXSimState; + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.AngleUnit; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.ElevatorSim; +import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d; +import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.Constants; + +public class Elevator extends SubsystemBase { + /** Position setpoints for the elevator. */ + public enum Setpoint { + IN(Rotations.of(Constants.IntakeConstants.IntakeSlideInSetPoint)), //0 + Middle(Rotations.of(Constants.IntakeConstants.IntakeSlideMiddleSetPoint)), + OUT(Rotations.of(Constants.IntakeConstants.IntakeSlideOutSetPoint)); + + /** The position target of the setpoint in angular units. */ + public final Angle target; + /** The position target of the setpoint in linear units. */ + public final Distance targetDist; + + private Setpoint(Angle target) { + this.target = target; + this.targetDist = kDrumRadius.times(target.in(Radians)); + } + private Setpoint(Distance target) { + this.target = Radians.of(target.div(kDrumRadius).magnitude()); + this.targetDist = target; + } + } + + private static final int kNumConfigAttempts = 2; + + private static final double kGearRatio = 6; + private static final Distance kDrumRadius = Meters.of(0.0254); + private static final Distance kMaxHeight = Inches.of(13.3); + + /* leader and follower motors */ + private final CANBus kCANBus = new CANBus("Upper"); + private final TalonFX motor_id_35 = new TalonFX(Constants.IntakeConstants.IntakeSlideMotorID, "Upper"); + + /* device status signals */ + private final StatusSignal motor_id_35Position = motor_id_35.getPosition(false); + private final StatusSignal motor_id_35Velocity = motor_id_35.getVelocity(false); + private final StatusSignal motor_id_35TorqueCurrent = motor_id_35.getTorqueCurrent(false); + + /* controls used by the leader motors */ + private final MotionMagicVoltage setpointRequest = new MotionMagicVoltage(0); + private final DutyCycleOut manualRequest = new DutyCycleOut(0); + private final DutyCycleOut calibrationRequestIn = new DutyCycleOut(-0.1) + .withIgnoreHardwareLimits(true) + .withIgnoreSoftwareLimits(true); + private final DutyCycleOut calibrationRequestOut = new DutyCycleOut(0.1) + .withIgnoreHardwareLimits(true) + .withIgnoreSoftwareLimits(true); + + /** Trigger to detect when the elevator drives into a hard stop. */ + public final Trigger isHardStop = new Trigger(() -> { + return motor_id_35Velocity.getValue().abs(RotationsPerSecond) < 1 && + motor_id_35TorqueCurrent.getValue().abs(Amps) > 10; + }).debounce(0.1); + + /* simulation */ + private final ElevatorSim elevatorSim_motor_id_35 = new ElevatorSim( + DCMotor.getKrakenX60Foc(1), + kGearRatio, 5, kDrumRadius.in(Meters), + 0.0, kMaxHeight.in(Meters), false, 0.0 + ); + + private static final double kSimLoopPeriod = 0.002; // 2 ms + private Notifier simNotifier = null; + private double lastSimTime = 0.0; + + /* Mechanism2d visualization of the elevator */ + private final Mechanism2d mech2d = new Mechanism2d(1, kMaxHeight.in(Meters)); + private final MechanismLigament2d motor_id_35Mech2d = mech2d.getRoot("motor_id_35 Root", 0.500, 0.4) + .append(new MechanismLigament2d("motor_id_35", elevatorSim_motor_id_35.getPositionMeters(), 200)); + + /** Configs common across all motors. */ + private static final TalonFXConfiguration motorInitialConfigs = new TalonFXConfiguration(); + + /** Configs common across just the leader motors. */ + private static final TalonFXConfiguration leaderInitialConfigs = motorInitialConfigs.clone(); + + /** Configs for {@link #motor_id_35}. */ + private final TalonFXConfiguration motor_id_35Configs = leaderInitialConfigs.clone(); + + static { + // Configure MotionMagic for the leader motors + leaderInitialConfigs.Slot0.kP = 12.0; // Tune these values! 8 + leaderInitialConfigs.Slot0.kI = 0.0; + leaderInitialConfigs.Slot0.kD = 0.1; + + + leaderInitialConfigs.MotionMagic.MotionMagicCruiseVelocity = 50*6; // rps + leaderInitialConfigs.MotionMagic.MotionMagicAcceleration = 160*6; // rps/s + leaderInitialConfigs.MotionMagic.MotionMagicJerk = 1600*2; // rps/s/s + } + + public Elevator() { + for (int i = 0; i < kNumConfigAttempts; ++i) { + var status = motor_id_35.getConfigurator().apply(motor_id_35Configs); + if (status.isOK()) break; + } + + + /* set the default command to neutral output */ + setDefaultCommand(manualDrive(() -> 0.0)); + /* alternatively, the default command can hold position */ + // setDefaultCommand(holdPosition()); + + SmartDashboard.putData("Elevator", mech2d); + + if (Utils.isSimulation()) { + startSimThread(); + } + } + + /** + * @return The Position of the elevator + */ + public Angle getPosition() { + return motor_id_35Position.getValue(); + } + + /** + * @return The Velocity of the elevator + */ + public AngularVelocity getVelocity() { + return motor_id_35Velocity.getValue(); + } + + /** + * @return The TorqueCurrent of the elevator + */ + public Current getTorqueCurrent() { + return motor_id_35TorqueCurrent.getValue(); + } + + /** + * Holds the elevator at the current position using PID. + * + * @return Command to run + */ + public Command holdPosition() { + return runOnce(() -> + setpointRequest.withPosition(motor_id_35Position.getValue()) + ).andThen(run(() -> { + motor_id_35.setControl(setpointRequest); + })); + } + + /** + * Drives the elevator to the provided position setpoint. + * + * @param setpoint Function returning the setpoint to apply + * @return Command to run + */ + public Command goToSetpoint(Supplier setpoint) { + return run(() -> { + setpointRequest.withPosition(setpoint.get().target); + motor_id_35.setControl(setpointRequest); + }); + } + + /** + * Manually drives the elevator with the provided duty cycle output. + * + * @param manualOutput Function returning the duty cycle to apply + * @return Command to run + */ + public Command manualDrive(DoubleSupplier manualOutput) { + return run(() -> { + manualRequest.withOutput(manualOutput.getAsDouble()); + motor_id_35.setControl(manualRequest); + }); + } + + /** + * Recalibrates the elevator zero point. This slowly drives the elevator + * down until we see a drop in velocity and a spike in stator current, + * indicating that we've hit a hard stop.da + * + * @return Command to run + */ + public Command calibrateZeroIn() { + return run(() -> { + motor_id_35.setControl(calibrationRequestIn); + }) + .until(isHardStop) + .andThen( + manualDrive(() -> 0.0).withTimeout(0.25) + .finallyDo(() -> { + motor_id_35.setPosition(Rotations.of(0)); + }) + ); + } + + public Command calibrateZeroOut() { + return run(() -> { + motor_id_35.setControl(calibrationRequestOut); + }) + .until(isHardStop) + .andThen( + manualDrive(() -> 0.0).withTimeout(0.25) + .finallyDo(() -> { + motor_id_35.setPosition(Rotations.of(Constants.IntakeConstants.IntakeSlideOutHardStop)); + }) + ); + } + + public boolean isAtSetpoint(Elevator.Setpoint setpoint){ + return getPosition().isNear(setpoint.target, Rotations.of(0.2)); + } + + @Override + public void periodic() { + /* refresh all status signals */ + BaseStatusSignal.refreshAll( + motor_id_35Position, + motor_id_35Velocity, + motor_id_35TorqueCurrent + ); + + motor_id_35Mech2d.setLength( + motor_id_35Position.getValueAsDouble() * kDrumRadius.in(Meters) * 2 * Math.PI + ); + + SmartDashboard.putString("LintakePosition", getPosition().toString()); + SmartDashboard.putBoolean("LintakeAtSetpoint", isAtSetpoint(Elevator.Setpoint.IN)); + } + + private void startSimThread() { + motor_id_35.getSimState().Orientation = ChassisReference.CounterClockwise_Positive; + motor_id_35.getSimState().setMotorType(TalonFXSimState.MotorType.KrakenX60); + + lastSimTime = Utils.getCurrentTimeSeconds(); + + /* Run simulation at a faster rate so PID gains behave more reasonably */ + simNotifier = new Notifier(() -> { + /* Calculate the time delta */ + final double currentTime = Utils.getCurrentTimeSeconds(); + final double deltaTime = currentTime - lastSimTime; + lastSimTime = currentTime; + + final var motor_id_35Sim = motor_id_35.getSimState(); + + /* First set the supply voltage of all the devices */ + motor_id_35Sim.setSupplyVoltage(RobotController.getBatteryVoltage()); + + /* Then calculate the new position and velocity of the simulated elevator */ + elevatorSim_motor_id_35.setInputVoltage(motor_id_35Sim.getMotorVoltage()); + elevatorSim_motor_id_35.update(deltaTime); + + /* Apply the new rotor position and velocity to the motors (before gear ratio) */ + motor_id_35Sim.setRawRotorPosition( + Radians.of(elevatorSim_motor_id_35.getPositionMeters() / kDrumRadius.in(Meters) * kGearRatio) + ); + motor_id_35Sim.setRotorVelocity( + RadiansPerSecond.of(elevatorSim_motor_id_35.getVelocityMetersPerSecond() / kDrumRadius.in(Meters) * kGearRatio) + ); + }); + simNotifier.startPeriodic(kSimLoopPeriod); + } +} diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 97e1fe1..5663a93 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/HopperSubsystem.java @@ -15,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.Commands; public class HopperSubsystem extends SubsystemBase { - private final TalonFX hopperMotor = new TalonFX(Constants.HopperConstants.HopperMotorID); + private final TalonFX hopperMotor = new TalonFX(Constants.HopperConstants.HopperMotorID, "Upper"); /** Creates a new Intake. */ public HopperSubsystem() {} diff --git a/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java index 470478e..fe63441 100644 --- a/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerHighSubsystem.java @@ -15,12 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands; public class IndexerHighSubsystem extends SubsystemBase { - private final TalonFX indexerHighMotor = new TalonFX(Constants.IndexerConstants.IndexerHighMotorID); + private final TalonFX indexerHighMotor = new TalonFX(Constants.IndexerConstants.HighIndexerMotorID, "Upper"); /** Creates a new Intake. */ public IndexerHighSubsystem() {} public void set(double speed){ - //System.out.print(speed); indexerHighMotor.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java b/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java index 6713cf0..78676d0 100644 --- a/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IndexerLowSubsystem.java @@ -15,12 +15,11 @@ import edu.wpi.first.wpilibj2.command.Commands; public class IndexerLowSubsystem extends SubsystemBase { - private final TalonFX indexerLowMotor = new TalonFX(Constants.IndexerConstants.IndexerLowMotorID); + private final TalonFX indexerLowMotor = new TalonFX(Constants.IndexerConstants.LowIndexerMotorID, "Upper"); /** Creates a new Intake. */ public IndexerLowSubsystem() {} public void set(double speed){ - //System.out.print(speed); indexerLowMotor.set(speed); } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 245bfc3..7ff682a 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -10,23 +10,24 @@ import edu.wpi.first.wpilibj.motorcontrol.MotorController; import edu.wpi.first.wpilibj.motorcontrol.Talon; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; + public class IntakeSubsystem extends SubsystemBase { - private final TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID); - private final TalonFX intakeFollowerMotor = new TalonFX(Constants.IntakeConstants.IntakeFollowerReversedMotorID); - /** Creates a new Intake. */ + private final TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID, "Upper"); + private final TalonFX intakeFollowerMotor = new TalonFX(Constants.IntakeConstants.IntakeFollowerReversedMotorID, "Upper"); + public IntakeSubsystem() { intakeFollowerMotor.setControl(new Follower(Constants.IntakeConstants.IntakeMotorID, MotorAlignmentValue.Opposed)); } - public void set(double speed){ - //System.out.print(speed); intakeMotor.set(speed); + SmartDashboard.putBoolean("ranSetMethod", true); //return Commands.run(() -> intakeMotor.set(speed)); } @@ -38,4 +39,4 @@ public void stop(){ public void periodic() { // This method will be called once per scheduler run } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 2797015..b5b5115 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -20,14 +20,20 @@ public Vision(CommandSwerveDrivetrain drivetrain) { * Updates pose estimator with Limelight vision measurements * Called automatically in periodic(). */ - - private final String PosLimelight = "limelight-front"; - public void updateVisionMeasurements() { + + // All Pose estimating limelights names + private final String[] LLNames = { + "limelight-fl", + "limelight-fr" + // add more as needed + }; + + public void updateVisionMeasurements(String LLName) { boolean doRejectUpdate = false; if (!useMegaTag2) { // MegaTag1 mode - LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(PosLimelight); + LimelightHelpers.PoseEstimate mt1 = LimelightHelpers.getBotPoseEstimate_wpiBlue(LLName); if (mt1 == null){ return; @@ -60,12 +66,12 @@ public void updateVisionMeasurements() { // MegaTag2 mode // Set robot orientation for MegaTag2 LimelightHelpers.SetRobotOrientation( - PosLimelight, + LLName, drivetrain.getState().Pose.getRotation().getDegrees(), 0, 0, 0, 0, 0 ); - LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(PosLimelight); + LimelightHelpers.PoseEstimate mt2 = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2(LLName); if (mt2 == null){ return; @@ -82,9 +88,6 @@ public void updateVisionMeasurements() { } if (!doRejectUpdate) { - /* - drivetrain.setVisionMeasurementStdDevs(VecBuilder.fill(0.7, 0.7, 0.9));//9999999 to 0.9 - drivetrain.addVisionMeasurement(mt2.pose, mt2.timestampSeconds); */ // Add vision measurement to drivetrain drivetrain.addVisionMeasurement( mt2.pose, @@ -95,25 +98,49 @@ public void updateVisionMeasurements() { } } + @Override public void periodic() { - updateVisionMeasurements();} + for (String limeLight : LLNames) { + updateVisionMeasurements(limeLight); + } + } - public boolean hasDetection() { - return LimelightHelpers.getRawDetections(PosLimelight).length > 0; -} + public boolean hasDetection() { + for (String ll : LLNames) { + if (LimelightHelpers.getRawDetections(ll).length > 0) return true; + } + return false; + } -public double getDetectionTX() { - LimelightHelpers.RawDetection[] detections = LimelightHelpers.getRawDetections(PosLimelight); - if (detections.length == 0) return 0.0; - return detections[0].txnc; + public double getDetectionTX() { + RawDetection best = null; + for (String ll : LLNames) { + for (RawDetection d : LimelightHelpers.getRawDetections(ll)) { + if (best == null || Math.abs(d.txnc) < Math.abs(best.txnc)) { + best = d; + } + } + } + return best != null ? best.txnc : 0.0; } -public double[] getFuelData() { - return LimelightHelpers.getPythonScriptData(PosLimelight); -} + public double[] getFuelData() { + for (String ll : LLNames) { + double[] data = LimelightHelpers.getPythonScriptData(ll); + if (data != null && data.length > 0) return data; + } + return new double[0]; + } -public RawDetection[] getAllFuel() { - return LimelightHelpers.getRawDetections(PosLimelight); + public RawDetection[] getAllFuel() { + java.util.List all = new java.util.ArrayList<>(); + for (String ll : LLNames) { + for (RawDetection d : LimelightHelpers.getRawDetections(ll)) { + all.add(d); + } + } + return all.toArray(new RawDetection[0]); } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java index 83ecf8b..fbdebb1 100644 --- a/src/main/java/frc/robot/utils/HubShiftUtil.java +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -36,7 +36,7 @@ public record ShiftInfo( private static final double maxFuelCountDelay = 2.0; private static final double shiftEndFuelCountExtension = 3.0; private static final double minTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MinShootingDistanceInches + 0.330879; -private static final double maxTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MaxShootingDistanceInches + 0.330879; + private static final double maxTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MaxShootingDistanceInches + 0.330879; private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); private static final double endingActiveFudge = shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); From 670dad7d37dacd86370633baca469e86bb9a1092 Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Thu, 12 Mar 2026 19:07:19 -0700 Subject: [PATCH 11/12] AAAAA Hubshiftv2 a --- src/main/java/frc/robot/Constants.java | 12 ++ src/main/java/frc/robot/RobotContainer.java | 7 - .../robot/handlers/IndexerHighHandler.java | 66 +++---- .../frc/robot/handlers/IndexerLowHandler.java | 15 +- .../robot/subsystems/ShooterSubsystem.java | 171 ------------------ .../java/frc/robot/utils/HubShiftUtil.java | 13 -- 6 files changed, 48 insertions(+), 236 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/ShooterSubsystem.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fa5fb71..a2ab65f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -25,6 +25,18 @@ public static abstract class ShiftConstants { public static final double MaxShootingDistanceInches = 200.0; //random number not too pivotal rn } +public static abstract class ClimberConstants { + public static final int ClimberMotorID = 32; //change me ts NOT a real motor + public static final double ClimbSpeed = 0.7; + public static final double DescendSpeed = -0.5; + + public static final Distance HighSetPoint = Inches.of(63.0); + public static final Distance MiddleSetPoint = Inches.of(45.0); + public static final Distance LowSetPoint = Inches.of(27.0); + + public static final Distance LowSetPointDown = Inches.of(23.0); + } + public static abstract class ShooterConstants { public static final int ShooterLeaderID = 20; public static final int ShooterFollowerID = 21; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0988d3b..4dddf81 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -267,13 +267,6 @@ private void configureBindings() { double r = info.remainingTime(); return r <= 1.0 && r > 0.0; }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); - - new Trigger(() -> { - double remaining = HubShiftUtil.getOfficialShiftInfo().remainingTime(); - return remaining <= 1.0 && HubShiftUtil.isNextShiftActive(); - }).onTrue(Commands.runOnce(() -> { - System.out.println("Early shoot window opened, remaining: " + HubShiftUtil.getOfficialShiftInfo().remainingTime()); - })); } public Command getAutonomousCommand() { diff --git a/src/main/java/frc/robot/handlers/IndexerHighHandler.java b/src/main/java/frc/robot/handlers/IndexerHighHandler.java index 0f4ad39..09dacc9 100644 --- a/src/main/java/frc/robot/handlers/IndexerHighHandler.java +++ b/src/main/java/frc/robot/handlers/IndexerHighHandler.java @@ -1,7 +1,3 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - package frc.robot.handlers; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -12,6 +8,8 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.utils.HubShiftUtil; +import frc.robot.utils.HubShiftUtil.ShiftInfo; public class IndexerHighHandler extends SubsystemBase implements StateSubsystem { @@ -28,7 +26,6 @@ public enum IndexerHighState implements State { private IndexerHighState desiredState = IndexerHighState.OFF; private IndexerHighState currentState = IndexerHighState.OFF; - /** Creates a new IntakeHandler. */ private IndexerHighHandler() {} public static IndexerHighHandler getInstance(){ @@ -50,33 +47,40 @@ public void handleStateTransition() { update(); } - @Override - public void update() { - if((currentState != desiredState)){ - switch (desiredState) { - case FAST: - index.set(Constants.IndexerConstants.FastRoll); - break; - case SLOWINTAKE: - index.set(Constants.IndexerConstants.SlowRoll); - break; - case FASTREVERSE: - index.set(Constants.IndexerConstants.FastOutRoll); - break; - case SLOWREVERSE: - index.set(Constants.IndexerConstants.SlowOutRoll); - break; - case OFF: - index.stop(); - break; - default: - index.stop(); - break; - } + @Override + public void update() { + if (currentState != desiredState) { + currentState = desiredState; + } + + switch (currentState) { + case FAST: + if (HubShiftUtil.getShiftedShiftInfo().active()) { + index.set(Constants.IndexerConstants.FastRoll); + } else { + index.stop(); + } + break; + case SLOWINTAKE: + if (HubShiftUtil.getShiftedShiftInfo().active()) { + index.set(Constants.IndexerConstants.SlowRoll); + } else { + index.stop(); + } + break; + case FASTREVERSE: + index.set(Constants.IndexerConstants.FastOutRoll); + break; + case SLOWREVERSE: + index.set(Constants.IndexerConstants.SlowOutRoll); + break; + case OFF: + default: + index.stop(); + break; + } currentState = desiredState; } - } - public IndexerHighState getCurrentState() { return currentState; } @@ -86,4 +90,4 @@ public void periodic() { update(); // This method will be called once per scheduler run } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/IndexerLowHandler.java b/src/main/java/frc/robot/handlers/IndexerLowHandler.java index 1b9e631..6e779e9 100644 --- a/src/main/java/frc/robot/handlers/IndexerLowHandler.java +++ b/src/main/java/frc/robot/handlers/IndexerLowHandler.java @@ -57,24 +57,11 @@ public void handleStateTransition() { public void update() { if((currentState != desiredState)){ switch (desiredState) { - case FAST: - ShiftInfo shiftInfo = HubShiftUtil.getOfficialShiftInfo(); - boolean earlyShoot = !shiftInfo.active() && shiftInfo.remainingTime() <= 1.0 && HubShiftUtil.isNextShiftActive(); - System.out.println("active: " + shiftInfo.active() + " | remainingTime: " + shiftInfo.remainingTime() + " | earlyShoot: " + earlyShoot); - - if (!shiftInfo.active() && !earlyShoot) { + case FAST: index.set(Constants.IndexerConstants.FastRoll); - } - break; case SLOWINTAKE: - ShiftInfo shiftInfo2 = HubShiftUtil.getOfficialShiftInfo(); - boolean earlyShoot2 = !shiftInfo2.active() && shiftInfo2.remainingTime() <= 1.0 && HubShiftUtil.isNextShiftActive(); - System.out.println("active: " + shiftInfo2.active() + " | remainingTime: " + shiftInfo2.remainingTime() + " | earlyShoot: " + earlyShoot2); - - if (!shiftInfo2.active() && !earlyShoot2) { index.set(Constants.IndexerConstants.SlowRoll); - } break; case FASTREVERSE: index.set(Constants.IndexerConstants.FastOutRoll); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java deleted file mode 100644 index 6a90bcd..0000000 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ /dev/null @@ -1,171 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. -package frc.robot.subsystems; - -import static edu.wpi.first.units.Units.*; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; - -import edu.wpi.first.math.controller.BangBangController; -import edu.wpi.first.wpilibj.Encoder; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -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.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.math.system.plant.DCMotor; -import frc.robot.Constants; -import frc.robot.handlers.IntakeHandler; -import frc.robot.handlers.Superstructure.SuperstructureState; -import edu.wpi.first.units.measure.AngularVelocity; -import com.ctre.phoenix6.sim.TalonFXSimState; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.StrictFollower; -import com.ctre.phoenix6.controls.VelocityVoltage; - -import edu.wpi.first.wpilibj.smartdashboard.*; -import com.ctre.phoenix6.signals.MotorAlignmentValue; - - -public class ShooterSubsystem extends SubsystemBase { - - // motors - private TalonFX shooterLeader = new TalonFX(Constants.ShooterConstants.ShooterLeaderID); - private TalonFX shooterFollower1 = new TalonFX(Constants.ShooterConstants.ShooterFollowerID); - private TalonFX shooterFollower2 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversedID); - private TalonFX shooterFollower3 = new TalonFX(Constants.ShooterConstants.ShooterFollowerReversed2ID); - - private TalonFXSimState shooterMotorSim = shooterLeader.getSimState(); - - private double f = 0.4; - - AngularVelocity setVelo = RPM.of(0); - - private CommandSwerveDrivetrain drivetrain; - - - // Creates a BangBangController - BangBangController controller = new BangBangController(); - - public ShooterSubsystem(){ - shooterFollower1.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - shooterFollower2.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - shooterFollower3.setControl(new Follower(Constants.ShooterConstants.ShooterLeaderID, MotorAlignmentValue.Aligned)); - } - - private static ShooterSubsystem instance; - public static ShooterSubsystem getInstance(){ - if (instance == null){ - instance = new ShooterSubsystem(); - } - return instance; - } - - public void initialize(CommandSwerveDrivetrain drivetrain) { - this.drivetrain = drivetrain; - } - - // Get current velocity from the motor - public AngularVelocity getVelocity() { - // TalonFX getVelocity() returns rotations per second, convert to RPM - double rps = shooterLeader.getVelocity().getValueAsDouble(); - return RPM.of(rps * 60.0); - } - - public Command setVelocity(AngularVelocity speed) { - return Commands.runOnce(() -> { - AngularVelocity finalSpeed = speed; - if (speed.gte(Constants.ShooterConstants.SetRPMHardStop)) { - finalSpeed = Constants.ShooterConstants.SetRPMHardStop; - } - setVelo = finalSpeed; - }, this); -} - - // Stop the shooter - public Command stop() { - return Commands.runOnce(() -> { - setVelo = RPM.of(0); - shooterLeader.set(0); - }, this); -} - - - //Checking if shooter RPM is at threashold (waiting for it to spinup) - public boolean isAtTargetVelo(){ - double currentRPM = getVelocity().in(RPM); - double targetRPM = setVelo.in(RPM); - - SmartDashboard.putBoolean("Shooter/At Target", currentRPM >= targetRPM); - - return currentRPM >= targetRPM; - } - - public AngularVelocity getCalcedRPM(double DistMeters){ - - //double distanceMeters = drivetrain.getDistance(); - double distanceInches = DistMeters * 39.3701; - AngularVelocity velo = RPM.of(-0.0312466 * Math.pow(distanceInches, 2) + 29.07009 * distanceInches + 828.29202); - - if (velo.gte(Constants.ShooterConstants.SetRPMHardStop)) { - velo = Constants.ShooterConstants.SetRPMHardStop; - } - - return velo; -} - - public boolean isReadyToShoot() { - // Check if at target velocity and drivetrain is aimed - return isAtTargetVelo() && drivetrain != null && isAimedAtTarget(); - } - - public boolean isAimedAtTarget() { - if (drivetrain == null) return false; - - Pose2d robotPose = drivetrain.getPose(); - Pose2d targetPose = CommandSwerveDrivetrain.getHubPose().toPose2d(); - - // Calculate angle error - Translation2d toTarget = targetPose.getTranslation().minus(robotPose.getTranslation()); - Rotation2d targetAngle = toTarget.getAngle().rotateBy(Rotation2d.k180deg); - Rotation2d currentAngle = robotPose.getRotation(); - - double errorDegrees = Math.abs(targetAngle.minus(currentAngle).getDegrees()); - - SmartDashboard.putNumber("Shooter/Aim Error (deg)", errorDegrees); - - return errorDegrees < 2.0; // Within 2 degrees - } - - @Override - public void periodic(){ - double output = controller.calculate(getVelocity().in(RPM), setVelo.in(RPM)); - shooterLeader.set(output); - - SmartDashboard.putNumber("Request RPM", setVelo.in(RPM)); - SmartDashboard.putNumber("Real RPM", getVelocity().in(RPM)); - SmartDashboard.putNumber("Bang-Bang Output", output); - } - - @Override - public void simulationPeriodic() { - // This method will be called once per scheduler run during simulation - - // Log voltages for all motors - double leaderVoltage = shooterLeader.getSimState().getMotorVoltage(); - shooterFollower1.getSimState().setSupplyVoltage(leaderVoltage); - shooterFollower2.getSimState().setSupplyVoltage(leaderVoltage); - shooterFollower3.getSimState().setSupplyVoltage(leaderVoltage); - - SmartDashboard.putNumber("Shooter/Leader Voltage", shooterLeader.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/Follower Voltage", shooterFollower1.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/FollowerReversed Voltage", shooterFollower2.getSimState().getMotorVoltage()); - SmartDashboard.putNumber("Shooter/FollowerReversed2 Voltage", shooterFollower3.getSimState().getMotorVoltage()); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java index fbdebb1..5c0bdde 100644 --- a/src/main/java/frc/robot/utils/HubShiftUtil.java +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -185,17 +185,4 @@ public static ShiftInfo getShiftedShiftInfo() { }; return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); } - - public static boolean isNextShiftActive() { - boolean[] schedule = getSchedule(); - double currentTime = shiftTimer.get(); - - for (int i = 0; i < shiftStartTimes.length - 1; i++) { - if (currentTime >= shiftStartTimes[i] && currentTime < shiftEndTimes[i]) { - return schedule[i + 1]; - } - } - return false; - } - } \ No newline at end of file From b2bec43078e235e1a2758472d4d4e3dd1a795c10 Mon Sep 17 00:00:00 2001 From: mateo-s09 Date: Tue, 24 Mar 2026 16:43:13 -0700 Subject: [PATCH 12/12] jakebranchpush --- .../deploy/pathplanner/autos/4414-973.auto | 61 ++++++ src/main/deploy/pathplanner/autos/Depot.auto | 12 ++ .../pathplanner/autos/Gay Maybe Yay.auto | 25 +++ .../pathplanner/autos/Low Mid NO OUTPOST.auto | 12 ++ .../deploy/pathplanner/autos/Low Mid.auto | 49 +++++ .../deploy/pathplanner/autos/Mid Depot.auto | 55 +++++ src/main/deploy/pathplanner/autos/Mid.auto | 56 ++++++ .../pathplanner/autos/Taxi Preload.auto | 25 +++ src/main/deploy/pathplanner/paths/D1-D2.path | 16 +- src/main/deploy/pathplanner/paths/D2-Hub.path | 10 +- src/main/deploy/pathplanner/paths/D2-O.path | 8 +- src/main/deploy/pathplanner/paths/F1-F2.path | 54 +++++ src/main/deploy/pathplanner/paths/F2-Hub.path | 87 ++++++++ src/main/deploy/pathplanner/paths/F3-ok.path | 54 +++++ .../pathplanner/paths/Fart-Outpost.path | 54 +++++ .../deploy/pathplanner/paths/Gay Yay.path | 162 +++++++++++++++ .../deploy/pathplanner/paths/Hub-Depot.path | 66 ++++++ .../deploy/pathplanner/paths/Hub-Wall.path | 54 +++++ .../deploy/pathplanner/paths/LowFuel-Hub.path | 71 +++++++ .../pathplanner/paths/MidHub-LowFuel.path | 59 ++++++ src/main/deploy/pathplanner/paths/Move.path | 54 +++++ .../deploy/pathplanner/paths/New Path.path | 54 +++++ .../deploy/pathplanner/paths/Outpost-Hub.path | 54 +++++ src/main/deploy/pathplanner/paths/S1-D1.path | 12 +- src/main/deploy/pathplanner/paths/S1-F1.path | 71 +++++++ src/main/deploy/pathplanner/paths/S4-F3.path | 71 +++++++ .../deploy/pathplanner/paths/S4-Wait.path | 66 ++++++ src/main/deploy/pathplanner/paths/Taxi.path | 54 +++++ .../deploy/pathplanner/paths/Wait-Hub.path | 54 +++++ src/main/deploy/pathplanner/paths/ok-Hub.path | 71 +++++++ src/main/java/frc/robot/Constants.java | 32 +-- src/main/java/frc/robot/Robot.java | 45 ++--- src/main/java/frc/robot/RobotContainer.java | 190 +++++++++++------- .../java/frc/robot/handlers/DriveHandler.java | 4 + .../robot/handlers/IndexerHighHandler.java | 66 +++--- .../frc/robot/handlers/IndexerLowHandler.java | 6 +- .../robot/handlers/IntakeSlideHandler.java | 2 + .../frc/robot/handlers/ShooterHandler.java | 6 +- .../frc/robot/handlers/Superstructure.java | 101 +++++++--- .../subsystems/BangBangShooterSubsystem.java | 69 ++++++- .../subsystems/CommandSwerveDrivetrain.java | 118 ++++++----- .../frc/robot/subsystems/IntakeSubsystem.java | 2 +- .../java/frc/robot/subsystems/Vision.java | 44 +--- .../java/frc/robot/utils/HubShiftUtil.java | 12 +- .../java/frc/robot/utils/RumbleUtils.java | 24 +++ 45 files changed, 1934 insertions(+), 338 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/4414-973.auto create mode 100644 src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto create mode 100644 src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto create mode 100644 src/main/deploy/pathplanner/autos/Low Mid.auto create mode 100644 src/main/deploy/pathplanner/autos/Mid Depot.auto create mode 100644 src/main/deploy/pathplanner/autos/Mid.auto create mode 100644 src/main/deploy/pathplanner/autos/Taxi Preload.auto create mode 100644 src/main/deploy/pathplanner/paths/F1-F2.path create mode 100644 src/main/deploy/pathplanner/paths/F2-Hub.path create mode 100644 src/main/deploy/pathplanner/paths/F3-ok.path create mode 100644 src/main/deploy/pathplanner/paths/Fart-Outpost.path create mode 100644 src/main/deploy/pathplanner/paths/Gay Yay.path create mode 100644 src/main/deploy/pathplanner/paths/Hub-Depot.path create mode 100644 src/main/deploy/pathplanner/paths/Hub-Wall.path create mode 100644 src/main/deploy/pathplanner/paths/LowFuel-Hub.path create mode 100644 src/main/deploy/pathplanner/paths/MidHub-LowFuel.path create mode 100644 src/main/deploy/pathplanner/paths/Move.path create mode 100644 src/main/deploy/pathplanner/paths/New Path.path create mode 100644 src/main/deploy/pathplanner/paths/Outpost-Hub.path create mode 100644 src/main/deploy/pathplanner/paths/S1-F1.path create mode 100644 src/main/deploy/pathplanner/paths/S4-F3.path create mode 100644 src/main/deploy/pathplanner/paths/S4-Wait.path create mode 100644 src/main/deploy/pathplanner/paths/Taxi.path create mode 100644 src/main/deploy/pathplanner/paths/Wait-Hub.path create mode 100644 src/main/deploy/pathplanner/paths/ok-Hub.path create mode 100644 src/main/java/frc/robot/utils/RumbleUtils.java diff --git a/src/main/deploy/pathplanner/autos/4414-973.auto b/src/main/deploy/pathplanner/autos/4414-973.auto new file mode 100644 index 0000000..fa1a9a0 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/4414-973.auto @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S4-Wait" + } + }, + { + "type": "path", + "data": { + "pathName": "Wait-Hub" + } + }, + { + "type": "path", + "data": { + "pathName": "MidHub-LowFuel" + } + }, + { + "type": "path", + "data": { + "pathName": "LowFuel-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 4.0 + } + }, + { + "type": "named", + "data": { + "name": "ShooterOff" + } + }, + { + "type": "path", + "data": { + "pathName": "Hub-Wall" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Depot.auto b/src/main/deploy/pathplanner/autos/Depot.auto index 33b1edf..71fa50e 100644 --- a/src/main/deploy/pathplanner/autos/Depot.auto +++ b/src/main/deploy/pathplanner/autos/Depot.auto @@ -27,6 +27,18 @@ "data": { "name": "Shoot" } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "path", + "data": { + "pathName": "Move" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto b/src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto new file mode 100644 index 0000000..1e6da8b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Gay Maybe Yay.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Gay Yay" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto b/src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto new file mode 100644 index 0000000..440a1ea --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Low Mid NO OUTPOST.auto @@ -0,0 +1,12 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Low Mid.auto b/src/main/deploy/pathplanner/autos/Low Mid.auto new file mode 100644 index 0000000..9566419 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Low Mid.auto @@ -0,0 +1,49 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S4-F3" + } + }, + { + "type": "path", + "data": { + "pathName": "F3-ok" + } + }, + { + "type": "path", + "data": { + "pathName": "ok-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + }, + { + "type": "named", + "data": { + "name": "ShooterOff" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid Depot.auto b/src/main/deploy/pathplanner/autos/Mid Depot.auto new file mode 100644 index 0000000..9586de1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid Depot.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S1-F1" + } + }, + { + "type": "path", + "data": { + "pathName": "F1-F2" + } + }, + { + "type": "path", + "data": { + "pathName": "F2-Hub" + } + }, + { + "type": "path", + "data": { + "pathName": "Hub-Depot" + } + }, + { + "type": "path", + "data": { + "pathName": "D1-D2" + } + }, + { + "type": "path", + "data": { + "pathName": "D2-Hub" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Mid.auto b/src/main/deploy/pathplanner/autos/Mid.auto new file mode 100644 index 0000000..84dc835 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Mid.auto @@ -0,0 +1,56 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "S1-F1" + } + }, + { + "type": "path", + "data": { + "pathName": "F1-F2" + } + }, + { + "type": "path", + "data": { + "pathName": "F2-Hub" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Shoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "ShooterOff" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Taxi Preload.auto b/src/main/deploy/pathplanner/autos/Taxi Preload.auto new file mode 100644 index 0000000..2e44f91 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Taxi Preload.auto @@ -0,0 +1,25 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Taxi" + } + }, + { + "type": "named", + "data": { + "name": "Shoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/D1-D2.path b/src/main/deploy/pathplanner/paths/D1-D2.path index bcf3584..7e26ff5 100644 --- a/src/main/deploy/pathplanner/paths/D1-D2.path +++ b/src/main/deploy/pathplanner/paths/D1-D2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 0.6368556537195638, - "y": 6.982748610619782 + "x": 0.7659077380952382, + "y": 6.977909226190477 }, "prevControl": null, "nextControl": { - "x": 0.6368556537195639, - "y": 5.982748610619782 + "x": 0.7659077380952383, + "y": 5.977909226190477 }, "isLocked": false, "linkedName": "D1" }, { "anchor": { - "x": 0.6368556537195638, - "y": 5.029978324456286 + "x": 0.7659077380952382, + "y": 5.0920982142857145 }, "prevControl": { - "x": 0.6368556537195638, - "y": 5.84299565391915 + "x": 0.7659077380952382, + "y": 5.905115543748579 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/D2-Hub.path b/src/main/deploy/pathplanner/paths/D2-Hub.path index 880bc2f..c595d8a 100644 --- a/src/main/deploy/pathplanner/paths/D2-Hub.path +++ b/src/main/deploy/pathplanner/paths/D2-Hub.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.6368556537195638, - "y": 5.029978324456286 + "x": 0.7659077380952382, + "y": 5.0920982142857145 }, "prevControl": null, "nextControl": { - "x": 1.3119677039106872, - "y": 5.247272024545016 + "x": 1.4410197882863613, + "y": 5.309391914374445 }, "isLocked": false, "linkedName": "D2" @@ -25,7 +25,7 @@ }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "hubok" } ], "rotationTargets": [], diff --git a/src/main/deploy/pathplanner/paths/D2-O.path b/src/main/deploy/pathplanner/paths/D2-O.path index 35ec47b..d1715a5 100644 --- a/src/main/deploy/pathplanner/paths/D2-O.path +++ b/src/main/deploy/pathplanner/paths/D2-O.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 0.6368556537195638, - "y": 5.029978324456286 + "x": 0.7659077380952382, + "y": 5.0920982142857145 }, "prevControl": null, "nextControl": { - "x": 3.4849047047979993, - "y": 4.997786665183881 + "x": 3.613956789173673, + "y": 5.05990655501331 }, "isLocked": false, "linkedName": "D2" diff --git a/src/main/deploy/pathplanner/paths/F1-F2.path b/src/main/deploy/pathplanner/paths/F1-F2.path new file mode 100644 index 0000000..eca676b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F1-F2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.109041703637978, + "y": 6.9909598214285715 + }, + "prevControl": null, + "nextControl": { + "x": 8.109041703637978, + "y": 6.1440530006096745 + }, + "isLocked": false, + "linkedName": "F1" + }, + { + "anchor": { + "x": 8.109041703637978, + "y": 4.4080403225806455 + }, + "prevControl": { + "x": 8.109041703637978, + "y": 5.29056308056252 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F2" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F2-Hub.path b/src/main/deploy/pathplanner/paths/F2-Hub.path new file mode 100644 index 0000000..5a4155d --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F2-Hub.path @@ -0,0 +1,87 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.109041703637978, + "y": 4.4080403225806455 + }, + "prevControl": null, + "nextControl": { + "x": 5.373451612903226, + "y": 4.1812903225806455 + }, + "isLocked": false, + "linkedName": "F2" + }, + { + "anchor": { + "x": 5.699032738100059, + "y": 5.705476190473729 + }, + "prevControl": { + "x": 6.762475509200177, + "y": 5.501199048002521 + }, + "nextControl": { + "x": 4.54405505952863, + "y": 5.927336309521348 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.397232142857143, + "y": 5.02031994047619 + }, + "prevControl": { + "x": 3.74796875, + "y": 5.9012351190476195 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "ScoringDepot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "okkkkk", + "waypointRelativePos": 0.5059055118110236, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/F3-ok.path b/src/main/deploy/pathplanner/paths/F3-ok.path new file mode 100644 index 0000000..47d9684 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/F3-ok.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.992048387096775, + "y": 0.904387096774194 + }, + "prevControl": null, + "nextControl": { + "x": 7.992048387096775, + "y": 2.65640839631369 + }, + "isLocked": false, + "linkedName": "F3" + }, + { + "anchor": { + "x": 8.021306451612903, + "y": 3.6985322580645166 + }, + "prevControl": { + "x": 8.021306451612903, + "y": 2.754988028682996 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "okyay" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Fart-Outpost.path b/src/main/deploy/pathplanner/paths/Fart-Outpost.path new file mode 100644 index 0000000..3d513d9 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Fart-Outpost.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.754854838709678, + "y": 2.806161290322582 + }, + "prevControl": null, + "nextControl": { + "x": 1.196577380952381, + "y": 2.2927455357142854 + }, + "isLocked": false, + "linkedName": "fart" + }, + { + "anchor": { + "x": 0.5897247023809524, + "y": 0.6418452380952386 + }, + "prevControl": { + "x": 2.1166443452380976, + "y": 0.5896428571428578 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Outpost" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Gay Yay.path b/src/main/deploy/pathplanner/paths/Gay Yay.path new file mode 100644 index 0000000..dd26b41 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Gay Yay.path @@ -0,0 +1,162 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.617462797619048, + "y": 2.5733333333333337 + }, + "prevControl": null, + "nextControl": { + "x": 5.790386904761905, + "y": 2.8473958333333336 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.886636904761905, + "y": 1.7772470238095242 + }, + "prevControl": { + "x": 5.886636904761905, + "y": 1.7772470238095244 + }, + "nextControl": { + "x": 7.886636904761905, + "y": 1.7772470238095242 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.8850074404761905, + "y": 1.7772470238095242 + }, + "prevControl": { + "x": 8.40067005470561, + "y": 1.7626182971647182 + }, + "nextControl": { + "x": 6.964940476190476, + "y": 1.8033482142857142 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.579947916666666, + "y": 1.7772470238095242 + }, + "prevControl": { + "x": 7.1812827764407565, + "y": 1.7951973181311385 + }, + "nextControl": { + "x": 6.142752976190476, + "y": 1.764196428571429 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.650096726190476, + "y": 2.5733333333333337 + }, + "prevControl": { + "x": 6.136227678571429, + "y": 2.605959821428572 + }, + "nextControl": { + "x": 8.262420121002597, + "y": 2.560136708445141 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.0693377976190477, + "y": 2.9648511904761903 + }, + "prevControl": { + "x": 4.635409226190475, + "y": 2.0643601190476186 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.8739352640545147, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.9212598425196845, + "maxWaypointRelativePos": 2.0, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + }, + { + "name": "Constraints Zone", + "minWaypointRelativePos": 3.553149606299209, + "maxWaypointRelativePos": 4.202755905511809, + "constraints": { + "maxVelocity": 0.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0.4990157480314956, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Hub-Depot.path b/src/main/deploy/pathplanner/paths/Hub-Depot.path new file mode 100644 index 0000000..9e157d2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Hub-Depot.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.397232142857143, + "y": 5.02031994047619 + }, + "prevControl": null, + "nextControl": { + "x": 1.7189662173968498, + "y": 5.27996100961303 + }, + "isLocked": false, + "linkedName": "ScoringDepot" + }, + { + "anchor": { + "x": 0.7659077380952382, + "y": 6.977909226190477 + }, + "prevControl": { + "x": 1.4104430147998999, + "y": 7.378144841563533 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "D1" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Intake", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Hub-Wall.path b/src/main/deploy/pathplanner/paths/Hub-Wall.path new file mode 100644 index 0000000..10ce8e8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Hub-Wall.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.7300223214285713, + "y": 2.782142857142858 + }, + "prevControl": null, + "nextControl": { + "x": 1.3466592261904762, + "y": 2.051309523809524 + }, + "isLocked": false, + "linkedName": "HubThing" + }, + { + "anchor": { + "x": 0.4331175595238095, + "y": 2.475453869047619 + }, + "prevControl": { + "x": 1.6925000000000026, + "y": 2.384099702380952 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LowFuel-Hub.path b/src/main/deploy/pathplanner/paths/LowFuel-Hub.path new file mode 100644 index 0000000..89dc935 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LowFuel-Hub.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.96983630952381, + "y": 1.3792038690476192 + }, + "prevControl": null, + "nextControl": { + "x": 6.188430059523809, + "y": 2.6646874999999994 + }, + "isLocked": false, + "linkedName": "LowFuel" + }, + { + "anchor": { + "x": 2.7300223214285713, + "y": 2.782142857142858 + }, + "prevControl": { + "x": 4.615833333333331, + "y": 2.2274925595238106 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "HubThing" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.2810902896081766, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "IntakeOff", + "waypointRelativePos": 0.2864173228346455, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidHub-LowFuel.path b/src/main/deploy/pathplanner/paths/MidHub-LowFuel.path new file mode 100644 index 0000000..a29be29 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MidHub-LowFuel.path @@ -0,0 +1,59 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.849114583333333, + "y": 3.9762723214285716 + }, + "prevControl": null, + "nextControl": { + "x": 6.553846726190476, + "y": 5.0529464285714285 + }, + "isLocked": false, + "linkedName": "MidHub" + }, + { + "anchor": { + "x": 7.96983630952381, + "y": 1.3792038690476192 + }, + "prevControl": { + "x": 9.203117559523811, + "y": 3.5912797619047625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "LowFuel" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.549403747870527, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 150.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Move.path b/src/main/deploy/pathplanner/paths/Move.path new file mode 100644 index 0000000..ac748f5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Move.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.628411712511091, + "y": 4.819671694764862 + }, + "prevControl": null, + "nextControl": { + "x": 2.207998511904762, + "y": 6.2079241071428575 + }, + "isLocked": false, + "linkedName": "hubok" + }, + { + "anchor": { + "x": 2.0122395833333333, + "y": 6.840877976190477 + }, + "prevControl": { + "x": 2.207998511904762, + "y": 6.1296205357142854 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -29.999999999999996 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..8397ce6 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.526108630952381, + "y": 4.021949404761905 + }, + "prevControl": null, + "nextControl": { + "x": 2.0057142857142853, + "y": 5.13125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.661510416666666, + "y": 5.640223214285714 + }, + "prevControl": { + "x": 3.6615104166666663, + "y": 5.640223214285714 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Outpost-Hub.path b/src/main/deploy/pathplanner/paths/Outpost-Hub.path new file mode 100644 index 0000000..9de9810 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Outpost-Hub.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.5897247023809524, + "y": 0.6418452380952386 + }, + "prevControl": null, + "nextControl": { + "x": 1.5897247023809524, + "y": 0.6418452380952386 + }, + "isLocked": false, + "linkedName": "Outpost" + }, + { + "anchor": { + "x": 2.5668898809523806, + "y": 3.3237425595238097 + }, + "prevControl": { + "x": 1.5668898809523806, + "y": 3.3237425595238097 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S1-D1.path b/src/main/deploy/pathplanner/paths/S1-D1.path index 9079154..5f1f2af 100644 --- a/src/main/deploy/pathplanner/paths/S1-D1.path +++ b/src/main/deploy/pathplanner/paths/S1-D1.path @@ -12,16 +12,16 @@ "y": 7.145519077193867 }, "isLocked": false, - "linkedName": "S1" + "linkedName": null }, { "anchor": { - "x": 0.6368556537195638, - "y": 6.982748610619782 + "x": 0.7659077380952382, + "y": 6.977909226190477 }, "prevControl": { - "x": 0.7405657518219149, - "y": 7.629981208715258 + "x": 0.8696178361975893, + "y": 7.625141824285953 }, "nextControl": null, "isLocked": false, @@ -60,7 +60,7 @@ "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S1-F1.path b/src/main/deploy/pathplanner/paths/S1-F1.path new file mode 100644 index 0000000..86a831b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S1-F1.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5539219165927243, + "y": 5.56812777284827 + }, + "prevControl": null, + "nextControl": { + "x": 6.040727595386525, + "y": 4.626521739136872 + }, + "isLocked": false, + "linkedName": "S2" + }, + { + "anchor": { + "x": 8.109041703637978, + "y": 6.9909598214285715 + }, + "prevControl": { + "x": 6.958189884650058, + "y": 8.109619981151067 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F1" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4612326043737585, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "yay", + "waypointRelativePos": 0.4675196850393696, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S4-F3.path b/src/main/deploy/pathplanner/paths/S4-F3.path new file mode 100644 index 0000000..cd62068 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S4-F3.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6325967741935488, + "y": 2.557467741935485 + }, + "prevControl": null, + "nextControl": { + "x": 6.6608064516129035, + "y": 2.9597661290322583 + }, + "isLocked": false, + "linkedName": "S4" + }, + { + "anchor": { + "x": 7.992048387096775, + "y": 0.904387096774194 + }, + "prevControl": { + "x": 7.311798387096775, + "y": -0.06844354838709554 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "F3" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.3644289450741062, + "rotationDegrees": 180.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "spinnnynnyny", + "waypointRelativePos": 0.322097378277154, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/S4-Wait.path b/src/main/deploy/pathplanner/paths/S4-Wait.path new file mode 100644 index 0000000..817a027 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/S4-Wait.path @@ -0,0 +1,66 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6325967741935488, + "y": 2.557467741935485 + }, + "prevControl": null, + "nextControl": { + "x": 4.632596774193549, + "y": 2.5574677419354845 + }, + "isLocked": false, + "linkedName": "S4" + }, + { + "anchor": { + "x": 5.679456845238096, + "y": 2.557467741935485 + }, + "prevControl": { + "x": 4.679456845238096, + "y": 2.557467741935485 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Wait" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "okkkkk", + "waypointRelativePos": 1.0, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "Intake" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path new file mode 100644 index 0000000..ad9ee46 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.604412202380953, + "y": 4.015424107142858 + }, + "prevControl": null, + "nextControl": { + "x": 2.6213636091500776, + "y": 4.015424107142858 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.397232142857143, + "y": 4.015424107142858 + }, + "prevControl": { + "x": 3.3972321428571433, + "y": 4.015424107142858 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 45.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Wait-Hub.path b/src/main/deploy/pathplanner/paths/Wait-Hub.path new file mode 100644 index 0000000..433d4e3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Wait-Hub.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 5.679456845238096, + "y": 2.557467741935485 + }, + "prevControl": null, + "nextControl": { + "x": 5.705558035714286, + "y": 3.0170535714285722 + }, + "isLocked": false, + "linkedName": "Wait" + }, + { + "anchor": { + "x": 5.849114583333333, + "y": 3.9762723214285716 + }, + "prevControl": { + "x": 5.914367559523809, + "y": 3.4542485119047615 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "MidHub" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 150.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/ok-Hub.path b/src/main/deploy/pathplanner/paths/ok-Hub.path new file mode 100644 index 0000000..f54edfe --- /dev/null +++ b/src/main/deploy/pathplanner/paths/ok-Hub.path @@ -0,0 +1,71 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.021306451612903, + "y": 3.6985322580645166 + }, + "prevControl": null, + "nextControl": { + "x": 4.941895161290323, + "y": 5.468645161290323 + }, + "isLocked": false, + "linkedName": "okyay" + }, + { + "anchor": { + "x": 2.754854838709678, + "y": 2.806161290322582 + }, + "prevControl": { + "x": 7.889645161290307, + "y": 1.0433629032258083 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "fart" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.08369659982563099, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "asdasd", + "waypointRelativePos": 0.6882022471910065, + "endWaypointRelativePos": null, + "command": { + "type": "named", + "data": { + "name": "IntakeOff" + } + } + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a2ab65f..871a4a4 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -19,30 +19,14 @@ /** Add your docs here. */ public class Constants { -//TODO change - public static abstract class ShiftConstants { - public static final double MinShootingDistanceInches = 40.0; - public static final double MaxShootingDistanceInches = 200.0; //random number not too pivotal rn -} - -public static abstract class ClimberConstants { - public static final int ClimberMotorID = 32; //change me ts NOT a real motor - public static final double ClimbSpeed = 0.7; - public static final double DescendSpeed = -0.5; - - public static final Distance HighSetPoint = Inches.of(63.0); - public static final Distance MiddleSetPoint = Inches.of(45.0); - public static final Distance LowSetPoint = Inches.of(27.0); - - public static final Distance LowSetPointDown = Inches.of(23.0); - } + public static abstract class ShooterConstants { public static final int ShooterLeaderID = 20; public static final int ShooterFollowerID = 21; public static final int ShooterFollowerReversedID = 22; // public static final int ShooterFollowerReversed2ID = 23; // - public static final double FastShot = 25; // IN RPS + public static final double FastShot = 100; // IN RPS public static final double SlowShot = 20; //HESKEL CHANGE ME SLOW public static final double RPSHardStop = 100.0; public static final double RPSTolarance = 1; @@ -53,8 +37,8 @@ public static abstract class ShooterConstants { public static abstract class IntakeConstants { public static final int IntakeMotorID = 30; public static final int IntakeFollowerReversedMotorID = 31; - public static final double FastIntake = 0.87; //used to be 1 but 0.87 is more effechient or sum - public static final double SlowIntake = 0.5; + public static final double FastIntake = 0.87; + public static final double SlowIntake = 0.67; public static final double SlowReverse = -0.3; public static final double FastReverse = -0.87; @@ -104,25 +88,25 @@ public static abstract class FieldConstants{ new Rotation3d() ); public static final Pose2d blueTargetHighPose = new Pose2d( - Units.Inches.of(50), + Units.Inches.of(20), Units.Inches.of(265), new Rotation2d() ); public static final Pose2d blueTargetLowPose = new Pose2d( - Units.Inches.of(50), + Units.Inches.of(20), Units.Inches.of(50), new Rotation2d() ); public static final Pose2d redTargetHighPose = new Pose2d( - Units.Inches.of(600), + Units.Inches.of(630), Units.Inches.of(265), new Rotation2d() ); public static final Pose2d redTargetLowPose = new Pose2d( - Units.Inches.of(600), + Units.Inches.of(630), Units.Inches.of(50), new Rotation2d() ); diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 332fc3d..4a5014c 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -7,15 +7,13 @@ import com.ctre.phoenix6.HootAutoReplay; import edu.wpi.first.cameraserver.CameraServer; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.TimedRobot; import edu.wpi.first.wpilibj.simulation.RoboRioSim; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc.robot.utils.HubShiftUtil; -import edu.wpi.first.wpilibj.simulation.BatterySim; -import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import frc.robot.utils.HubShiftUtil.ShiftInfo; public class Robot extends TimedRobot { @@ -28,8 +26,6 @@ public class Robot extends TimedRobot { .withTimestampReplay() .withJoystickReplay(); - private final boolean kUseLimelight = false; - public Robot() { m_robotContainer = new RobotContainer(); } @@ -39,27 +35,23 @@ public void robotPeriodic() { m_timeAndJoystickReplay.update(); CommandScheduler.getInstance().run(); - /* - * This example of adding Limelight is very simple and may not be sufficient for on-field use. - * Users typically need to provide a standard deviation that scales with the distance to target - * and changes with number of tags available. - * - * This example is sufficient to show that vision integration is possible, though exact implementation - * of how to use vision should be tuned per-robot and to the team's specification. - */ - - - if (kUseLimelight) { - var driveState = m_robotContainer.drivetrain.getState(); - double headingDeg = driveState.Pose.getRotation().getDegrees(); - double omegaRps = Units.radiansToRotations(driveState.Speeds.omegaRadiansPerSecond); - - LimelightHelpers.SetRobotOrientation("limelight", headingDeg, 0, 0, 0, 0, 0); - var llMeasurement = LimelightHelpers.getBotPoseEstimate_wpiBlue_MegaTag2("limelight"); - if (llMeasurement != null && llMeasurement.tagCount > 0 && Math.abs(omegaRps) < 2.0) { - m_robotContainer.drivetrain.addVisionMeasurement(llMeasurement.pose, llMeasurement.timestampSeconds); - } - } + ShiftInfo official = HubShiftUtil.getOfficialShiftInfo(); + ShiftInfo shifted = HubShiftUtil.getShiftedShiftInfo(); + + // Official shift info + SmartDashboard.putString("Shift/Official/CurrentShift", official.currentShift().toString()); + SmartDashboard.putNumber("Shift/Official/RemainingTime", Math.round(official.remainingTime()*10)/10.0); + SmartDashboard.putNumber("Shift/Official/ElapsedTime", Math.round(official.elapsedTime()*10)/10.0); + SmartDashboard.putNumber("Shift/Official/MatchTime", HubShiftUtil.getMatchTime()); + SmartDashboard.putBoolean("Shift/Official/Active", official.active()); + + // Shifted shift info + SmartDashboard.putString("Shift/Shifted/CurrentShift", shifted.currentShift().toString()); + SmartDashboard.putNumber("Shift/Shifted/ElapsedTime", shifted.elapsedTime()); + SmartDashboard.putNumber("Shift/Shifted/RemainingTime", shifted.remainingTime()); + SmartDashboard.putBoolean("Shift/Shifted/Active", shifted.active()); + + } @@ -89,7 +81,6 @@ public void autonomousExit() {} @Override public void teleopInit() { - HubShiftUtil.initialize(); CameraServer.startAutomaticCapture(); if (m_autonomousCommand != null) { CommandScheduler.getInstance().cancel(m_autonomousCommand); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4dddf81..1817c09 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,40 +4,36 @@ package frc.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; -import org.ejml.sparse.csc.linsol.qr.LinearSolverQrLeftLooking_DSCC; +import java.util.Optional; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.FollowPathCommand; -import com.pathplanner.lib.events.OneShotTriggerEvent; import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction; import frc.robot.generated.TunerConstants; import frc.robot.handlers.DriveHandler; -import frc.robot.handlers.IntakeHandler; import frc.robot.handlers.ShooterHandler; import frc.robot.handlers.Superstructure; -import frc.robot.handlers.IntakeHandler.IntakeState; -import frc.robot.handlers.IntakeSlideHandler; -import frc.robot.handlers.IntakeSlideHandler.IntakeSlideState; import frc.robot.handlers.Superstructure.SuperstructureState; import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -48,6 +44,7 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.Vision; import frc.robot.utils.HubShiftUtil; +import frc.robot.utils.RumbleUtils; public class RobotContainer { @@ -66,6 +63,7 @@ public class RobotContainer { private final Telemetry logger = new Telemetry(MaxSpeed); private final CommandXboxController joystick = new CommandXboxController(0); + private final CommandXboxController opJoystick = new CommandXboxController(1); public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); @@ -80,6 +78,35 @@ public class RobotContainer { public final Superstructure superstructure = Superstructure.getInstance(); + Trigger fiveSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 5.0;}); + + Trigger threeSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 3.0;}); + + Trigger twoSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 2.0;}); + + Trigger oneSecWarning = new Trigger(() -> { + var info = HubShiftUtil.getOfficialShiftInfo(); + return info.remainingTime() <= 1.0;}); + + Trigger noButtonsHeld = new Trigger(() -> + !joystick.a().getAsBoolean() && + !joystick.b().getAsBoolean() && + !joystick.y().getAsBoolean() && + !joystick.x().getAsBoolean() && + !joystick.rightTrigger().getAsBoolean() && + !joystick.leftTrigger().getAsBoolean() && + !joystick.rightBumper().getAsBoolean() && + !joystick.leftTrigger().getAsBoolean() && + !joystick.rightBumper().getAsBoolean() + ); + + /* Path follower */ private final SendableChooser autoChooser; @@ -97,7 +124,6 @@ public RobotContainer() { // Warmup PathPlanner to avoid Java pauses FollowPathCommand.warmupCommand().schedule(); - } private void configurePathPlanner() { @@ -109,13 +135,23 @@ private void configurePathPlanner() { NamedCommands.registerCommand("Shoot", new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); + + NamedCommands.registerCommand("ShooterOff", + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); NamedCommands.registerCommand("ShootSafe", new SequentialCommandGroup( new InstantCommand( () -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP)), - Commands.waitSeconds(2), + new WaitCommand(2.0), new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT)) )); + + NamedCommands.registerCommand("ShootTest", + new SequentialCommandGroup( + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP)).withTimeout(0.1), + new WaitCommand(3.0), + new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE)) + )); } private void configureBindings() { @@ -160,6 +196,7 @@ private void configureBindings() { Units.degreesToRadians(540), Units.degreesToRadians(720)); //ROTATE 90 degreese + /* joystick.leftBumper().onTrue(Commands.runOnce(() -> { Rotation2d targetRotation = drivetrain.getPose().getRotation().plus(Rotation2d.fromDegrees(90)); @@ -175,26 +212,41 @@ private void configureBindings() { .withRotationalRate(rotationalRate * 6); // Max angular rate }).withTimeout(2.0).schedule(); })); + */ - - + /* Main driver Controller: + * RT - Hold to spin up (and shoot hopefully) - relase to idle + * RB - Shoot (dont use unless robot broken) + * LT - Hold to Intake - release to idle + * B - Spin up Fast (use if broken) + * Y - FAST SHOT (use if broken) + * A - Hold to bump assist - release to idle + * X - Robot off + * Left Stick in - Reverse shi + */ joystick.rightTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUP))); - - joystick.rightTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + + //joystick.rightBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT))); + joystick.rightBumper().onTrue(new SequentialCommandGroup( + Commands.runOnce(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST)), + Commands.waitSeconds(0.5), + Commands.runOnce(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT)))); //joystick.leftBumper().onTrue(new InstantCommand(() -:drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, MaxSpeed, MaxAngularRate, "no"))); //shoot while stationary joystick.b().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST))); joystick.y().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT))); - joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.TUNING))); - + joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState. BUMP))); + joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); - joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); + joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKESLOW))); + joystick.leftBumper().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKESLOWSLOW))); joystick.leftStick().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.REVERSE))); + + noButtonsHeld.onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.IDLE))); //joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState((Superstructure.SuperstructureState.TUNING)))); @@ -205,72 +257,66 @@ private void configureBindings() { joystick.rightStick().onTrue(new InstantCommand(() -> drivetrain.seedFieldCentric())); - - //FOR HESHEL - /* - joystick.leftTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.INTAKE))); - joystick.leftTrigger().onFalse(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); + //joystick.povDown().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROIN))); + //joystick.povUp().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROOUT))); //in RPM - joystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.SPINUPFAST))); - joystick.y().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.FASTSHOT))); - joystick.x().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.OFF))); - */ + joystick.povRight().whileTrue(IntakeSlide.manualDrive(() -> 0.67)); // out + joystick.povLeft().whileTrue(IntakeSlide.manualDrive(() -> -0.67)); //in - /*BINDS: - Right bumper = auto angle - B = spinup - Y = Shoot with auto shooter speed - Left Trigger = hold for intake/ release for idle - Left stick in = reverse (unstuck shi) - A = Tuning mode (Dpad up or down to change speed by 250 RPM) - */ - - - + //CONTROLLER 2 / debug controller + /* + * RT - Shoot + * B - re-zero intake IN + * Y - overide alliance winner (on a switch) + * DPAD - shift hub by 0.1 M in direction (up is away, down is closer) + * A - Reset Shifted Hub to where it should be + */ + + opJoystick.rightTrigger().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.STATIONARYSHOT))); + opJoystick.b().onTrue(new InstantCommand(() -> IntakeSlide.calibrateZeroIn())); + //AUTO WINNER OVERIDES + opJoystick.y().onTrue(Commands.runOnce(() -> { + var current = HubShiftUtil.getAllianceWinOverride(); + HubShiftUtil.setAllianceWinOverride(() -> Optional.of(current.orElse(true) == false)); + })); - - //joystick.povDown().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROIN))); - //joystick.povUp().onTrue(Commands.runOnce(() -> IntakeSlideHandler.getInstance().setDesiredState(IntakeSlideState.REZEROOUT))); //in RPM + opJoystick.a().onTrue(Commands.runOnce(() -> drivetrain.resetHubOffset())); + opJoystick.a().onTrue(Commands.runOnce(() -> shooter.resetShooterMult())); + + // Change Shooter Power + opJoystick.povUp().onTrue(Commands.runOnce(() -> shooter.changeShooterMult(0.05))); + opJoystick.povDown().onTrue(Commands.runOnce(() -> shooter.changeShooterMult(-0.05))); + + // Adjust Y offset + opJoystick.povRight().onTrue(Commands.runOnce(() -> drivetrain.setHubOffset(0.0, 0.1))); + opJoystick.povLeft().onTrue(Commands.runOnce(() -> drivetrain.setHubOffset(0.0, -0.1))); + + opJoystick.x().onTrue(Commands.runOnce(() -> shooter.toggleShooterMult())); + + /* + opJoystick.a().onTrue(new InstantCommand(() -> superstructure.setDesiredState(Superstructure.SuperstructureState.TUNING))); joystick.povUp().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(1))); joystick.povDown().onTrue(Commands.runOnce(() -> ShooterHandler.getInstance().adjustFastShot(-1))); - - //joystick.povLeft().onTrue(IntakeSlide.goToSetpoint(()-> Elevator.Setpoint.OUT)); - //joystick.povRight().onTrue(IntakeSlide.goToSetpoint(()-> Elevator.Setpoint.IN)); + */ - joystick.povRight().whileTrue(IntakeSlide.manualDrive(() -> 0.67)); // out - joystick.povLeft().whileTrue(IntakeSlide.manualDrive(() -> -0.67)); //in - Runnable rumbleOn = () -> joystick.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 1.0); - Runnable rumbleOff = () -> joystick.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0); - - new Trigger(() -> { - var info = HubShiftUtil.getOfficialShiftInfo(); - if (info == null) return false; - double r = info.remainingTime(); - return r <= 5.0 && r > 4.5; - }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); - - new Trigger(() -> { - var info = HubShiftUtil.getOfficialShiftInfo(); - if (info == null) return false; - double r = info.remainingTime(); - return r <= 3.0 && r > 2.5; - }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); - - new Trigger(() -> { - var info = HubShiftUtil.getOfficialShiftInfo(); - if (info == null) return false; - double r = info.remainingTime(); - return r <= 1.0 && r > 0.0; - }).whileTrue(Commands.run(rumbleOn)).onFalse(Commands.runOnce(rumbleOff)); - } + RobotModeTriggers.teleop().onTrue(Commands.runOnce(HubShiftUtil::initialize)); + RobotModeTriggers.autonomous().onTrue(Commands.runOnce(HubShiftUtil::initialize)); + + fiveSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 0.5)); + threeSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 0.25)); + twoSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 0.25)); + oneSecWarning.onTrue(RumbleUtils.rumble(joystick, 0.5, 1)); + + fiveSecWarning.onTrue(new InstantCommand(() -> SmartDashboard.putBoolean("isTsWorking", true))); + } public Command getAutonomousCommand() { /* Run the path selected from the auto chooser */ return autoChooser.getSelected(); } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/handlers/DriveHandler.java b/src/main/java/frc/robot/handlers/DriveHandler.java index 988f26e..ac50b2d 100644 --- a/src/main/java/frc/robot/handlers/DriveHandler.java +++ b/src/main/java/frc/robot/handlers/DriveHandler.java @@ -26,6 +26,7 @@ public class DriveHandler extends SubsystemBase { public enum DriveState implements State { TELEOPDRIVE, + TELEOPDRIVESLOW, AUTOALLIGN, SHOOTONTHEMOVE, SNAKE, @@ -110,6 +111,9 @@ public void update() { case TELEOPDRIVE: drivetrain.setDefaultCommand(drivetrain.TeleopDrive(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); break; + case TELEOPDRIVESLOW: + drivetrain.setDefaultCommand(drivetrain.TeleopDriveSLOW(joystick, maxSpeed, maxAngularRate, drive, drivetrain)); + break; case AUTOALLIGN: drivetrain.setDefaultCommand(drivetrain.headingLocktoHub(joystick, maxSpeed, maxAngularRate)); break; diff --git a/src/main/java/frc/robot/handlers/IndexerHighHandler.java b/src/main/java/frc/robot/handlers/IndexerHighHandler.java index 09dacc9..0f4ad39 100644 --- a/src/main/java/frc/robot/handlers/IndexerHighHandler.java +++ b/src/main/java/frc/robot/handlers/IndexerHighHandler.java @@ -1,3 +1,7 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package frc.robot.handlers; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -8,8 +12,6 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IndexerHighSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.utils.HubShiftUtil; -import frc.robot.utils.HubShiftUtil.ShiftInfo; public class IndexerHighHandler extends SubsystemBase implements StateSubsystem { @@ -26,6 +28,7 @@ public enum IndexerHighState implements State { private IndexerHighState desiredState = IndexerHighState.OFF; private IndexerHighState currentState = IndexerHighState.OFF; + /** Creates a new IntakeHandler. */ private IndexerHighHandler() {} public static IndexerHighHandler getInstance(){ @@ -47,40 +50,33 @@ public void handleStateTransition() { update(); } - @Override - public void update() { - if (currentState != desiredState) { - currentState = desiredState; - } - - switch (currentState) { - case FAST: - if (HubShiftUtil.getShiftedShiftInfo().active()) { - index.set(Constants.IndexerConstants.FastRoll); - } else { - index.stop(); - } - break; - case SLOWINTAKE: - if (HubShiftUtil.getShiftedShiftInfo().active()) { - index.set(Constants.IndexerConstants.SlowRoll); - } else { - index.stop(); - } - break; - case FASTREVERSE: - index.set(Constants.IndexerConstants.FastOutRoll); - break; - case SLOWREVERSE: - index.set(Constants.IndexerConstants.SlowOutRoll); - break; - case OFF: - default: - index.stop(); - break; - } + @Override + public void update() { + if((currentState != desiredState)){ + switch (desiredState) { + case FAST: + index.set(Constants.IndexerConstants.FastRoll); + break; + case SLOWINTAKE: + index.set(Constants.IndexerConstants.SlowRoll); + break; + case FASTREVERSE: + index.set(Constants.IndexerConstants.FastOutRoll); + break; + case SLOWREVERSE: + index.set(Constants.IndexerConstants.SlowOutRoll); + break; + case OFF: + index.stop(); + break; + default: + index.stop(); + break; + } currentState = desiredState; } + } + public IndexerHighState getCurrentState() { return currentState; } @@ -90,4 +86,4 @@ public void periodic() { update(); // This method will be called once per scheduler run } -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/handlers/IndexerLowHandler.java b/src/main/java/frc/robot/handlers/IndexerLowHandler.java index 6e779e9..30aeeff 100644 --- a/src/main/java/frc/robot/handlers/IndexerLowHandler.java +++ b/src/main/java/frc/robot/handlers/IndexerLowHandler.java @@ -13,8 +13,6 @@ import frc.robot.subsystems.IndexerHighSubsystem; import frc.robot.subsystems.IndexerLowSubsystem; import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.utils.HubShiftUtil; -import frc.robot.utils.HubShiftUtil.ShiftInfo; public class IndexerLowHandler extends SubsystemBase implements StateSubsystem { @@ -57,8 +55,8 @@ public void handleStateTransition() { public void update() { if((currentState != desiredState)){ switch (desiredState) { - case FAST: - index.set(Constants.IndexerConstants.FastRoll); + case FAST: + index.set(Constants.IndexerConstants.FastRoll); break; case SLOWINTAKE: index.set(Constants.IndexerConstants.SlowRoll); diff --git a/src/main/java/frc/robot/handlers/IntakeSlideHandler.java b/src/main/java/frc/robot/handlers/IntakeSlideHandler.java index 59a7366..aa9572b 100644 --- a/src/main/java/frc/robot/handlers/IntakeSlideHandler.java +++ b/src/main/java/frc/robot/handlers/IntakeSlideHandler.java @@ -6,6 +6,8 @@ import static edu.wpi.first.units.Units.Rotations; +import com.ctre.phoenix6.signals.Enable5VRailValue; + import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; diff --git a/src/main/java/frc/robot/handlers/ShooterHandler.java b/src/main/java/frc/robot/handlers/ShooterHandler.java index a3f5e82..39d0656 100644 --- a/src/main/java/frc/robot/handlers/ShooterHandler.java +++ b/src/main/java/frc/robot/handlers/ShooterHandler.java @@ -14,8 +14,6 @@ import frc.robot.Constants; import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.utils.HubShiftUtil; -import frc.robot.utils.HubShiftUtil.ShiftInfo; import frc.robot.RobotContainer; public class ShooterHandler extends SubsystemBase implements StateSubsystem { @@ -98,7 +96,7 @@ public void update() { private void handleStateChange(){ switch (desiredState) { case SHOOTING: - shooter.setTargetVeloDistance(DistMeters); + shooter.setTargetVeloDistance(DistMeters); break; case SLOW: shooter.setTargetVelocity(Constants.ShooterConstants.SlowShot); @@ -128,5 +126,5 @@ public void periodic() { DistMeters = drivetrain.getDistance(); update(); // Handle state transitions SmartDashboard.putString("ShooterState", currentState.toString()); - } +} } \ No newline at end of file diff --git a/src/main/java/frc/robot/handlers/Superstructure.java b/src/main/java/frc/robot/handlers/Superstructure.java index 3d0dd0f..661c20c 100644 --- a/src/main/java/frc/robot/handlers/Superstructure.java +++ b/src/main/java/frc/robot/handlers/Superstructure.java @@ -1,33 +1,23 @@ package frc.robot.handlers; +import static edu.wpi.first.units.Units.Degrees; + import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc.robot.Constants.ShooterConstants; -import frc.robot.handlers.IndexerHighHandler.IndexerHighState; -import frc.robot.handlers.IndexerLowHandler.IndexerLowState; -import frc.robot.handlers.IntakeSlideHandler.IntakeSlideState; import frc.robot.subsystems.BangBangShooterSubsystem; import frc.robot.subsystems.CommandSwerveDrivetrain; -import frc.robot.subsystems.HopperSubsystem; -import frc.robot.subsystems.IndexerHighSubsystem; -import frc.robot.subsystems.IndexerLowSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -import static edu.wpi.first.units.Units.Degrees; - -import edu.wpi.first.math.geometry.Pose2d; public class Superstructure extends SubsystemBase { // Define your states public enum SuperstructureState { STATIONARYSHOT, + SHOOTONTHEMOVE, + SHOOTONTHEMOVESPINUP, INTAKE, + INTAKESLOW, + INTAKESLOWSLOW, SLOWSHOT, REVERSE, SPINUP, @@ -37,7 +27,8 @@ public enum SuperstructureState { SPINUPSLOW, SPINUPFAST, FASTSHOT, - AIM + AIM, + BUMP } private static Superstructure instance; @@ -117,6 +108,24 @@ private void handleStateTransition() { driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVE); intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); break; + case INTAKESLOW: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTINTAKE); + hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVESLOW); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); + break; + case INTAKESLOWSLOW: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.SLOWINTAKE); + hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.TELEOPDRIVESLOW); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); + break; case SPINUP: shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); //change to shooting intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); @@ -135,13 +144,30 @@ private void handleStateTransition() { driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); break; + case SHOOTONTHEMOVE: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); + hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); + driveHandler.setDesiredState(DriveHandler.DriveState.SHOOTONTHEMOVE); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); + break; + case SHOOTONTHEMOVESPINUP: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.SHOOTING); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); + hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.SHOOTONTHEMOVE); + break; case TUNING: //dont use shooterHandler.setDesiredState(ShooterHandler.ShooterState.TUNING); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); - driveHandler.setDesiredState(DriveHandler.DriveState.BUMP_LOCK); + driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); break; case SLOWSHOT: //dont use shooterHandler.setDesiredState(ShooterHandler.ShooterState.SLOW); @@ -152,35 +178,46 @@ private void handleStateTransition() { //driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); break; case SPINUPFAST: //dont use - shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); //SWITCHEWD TO TUNING SWITCH BACK TO FAST SHOT + shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); //change to shooting intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); - indexerHighHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); + ShooterAtVelo = false; break; case FASTSHOT: //dont use - shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); //SWITCHED TO TUNABLE FAST SHOT SWITCH BACK TO FAST - intakeHandler.setDesiredState(IntakeHandler.IntakeState.SLOWINTAKE); + shooterHandler.setDesiredState(ShooterHandler.ShooterState.FAST); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.FAST); - indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.FAST); - indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FAST); - //driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); - //intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWINTAKE); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.SLOWINTAKE); + driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.SLOWIN); break; case REVERSE: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); intakeHandler.setDesiredState(IntakeHandler.IntakeState.FASTREVERSE); + hopperHandler.setDesiredState(HopperHandler.HopperState.FASTOUT); + indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.SLOWREVERSE); + indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.FASTREVERSE); + intakeSlideHandler.setDesiredState(IntakeSlideHandler.IntakeSlideState.OUT); + break; + case AIM: + shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); + intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); + driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); break; - case AIM: + case BUMP: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); intakeHandler.setDesiredState(IntakeHandler.IntakeState.OFF); hopperHandler.setDesiredState(HopperHandler.HopperState.OFF); indexerHighHandler.setDesiredState(IndexerHighHandler.IndexerHighState.OFF); indexerLowHandler.setDesiredState(IndexerLowHandler.IndexerLowState.OFF); - driveHandler.setDesiredState(DriveHandler.DriveState.AUTOALLIGN); + driveHandler.setDesiredState(DriveHandler.DriveState.BUMP_LOCK); break; case OFF: shooterHandler.setDesiredState(ShooterHandler.ShooterState.OFF); @@ -213,7 +250,13 @@ public void periodic() { } DTaimed = drivetrain.isAimedAtTarget(); - ShooterAtVelo = shooter.atTargetVelo(); + if (CommandSwerveDrivetrain.isInAllianceZone(drivetrain.getPose())){ + ShooterAtVelo = shooter.atTargetVelo(); + } + else { + ShooterAtVelo = shooter.atTargetVeloPassing(); + } + if (currentState == SuperstructureState.SPINUP && ShooterAtVelo && DTaimed){ //checks if its at target velo and angle diff --git a/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java b/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java index 5ef14b0..6b28d62 100644 --- a/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BangBangShooterSubsystem.java @@ -32,25 +32,35 @@ public class BangBangShooterSubsystem extends SubsystemBase { //inches to center hub from Robot orign , RPS static { - Shooter1Map.put(138.4 ,100.0); + Shooter1Map.put(138.4 ,105.0); Shooter1Map.put(121.4 ,77.0); Shooter1Map.put(104.6 ,70.0); Shooter1Map.put(85.8, 64.0); Shooter1Map.put(69.0, 60.0); + Shooter1Map.put(56.7,52.0); - Shooter2Map.put(138.4 ,100.0); + Shooter2Map.put(138.4 ,105.0); Shooter2Map.put(121.4 ,77.0); Shooter2Map.put(104.6 ,70.0); + Shooter2Map.put(85.8, 64.0); + Shooter2Map.put(69.0, 60.0); + Shooter2Map.put(56.7,52.0); - Shooter3Map.put(138.4 ,100.0); + + Shooter3Map.put(138.4 ,105.0); Shooter3Map.put(121.4 ,77.0); Shooter3Map.put(104.6 ,70.0); + Shooter3Map.put(85.8, 64.0); + Shooter3Map.put(69.0, 60.0); + Shooter3Map.put(56.7,52.0); + - Shooter4Map.put(138.4 ,100.0); + Shooter4Map.put(138.4 ,105.0); Shooter4Map.put(121.4 ,77.0); Shooter4Map.put(104.6 ,70.0); - - + Shooter4Map.put(85.8, 64.0); + Shooter4Map.put(69.0, 60.0); + Shooter4Map.put(56.7,52.0); } @@ -63,7 +73,14 @@ public class BangBangShooterSubsystem extends SubsystemBase { private double targetVeloRPS3 = 0; private double targetVeloRPS4 = 0; + private double ShooterMult = 0.95; + + + private boolean shooterEnabled = false; + + private boolean MaxRPM = false; + private boolean MultiplierOn = true; public BangBangShooterSubsystem() { @@ -84,6 +101,20 @@ public static BangBangShooterSubsystem getInstance(){ return instance; } + public void changeShooterMult(double multAdd){ + ShooterMult += multAdd; + } + + public void toggleShooterMult(){ + if (MultiplierOn) {MultiplierOn = false;} + if (!MultiplierOn) {MultiplierOn = true;} + } + + public void resetShooterMult(){ + ShooterMult = 1.0; //change to 0.95 + } + + // without mult public void setTargetVelocity(double VelocityRPS) { if (VelocityRPS > Constants.ShooterConstants.RPSHardStop) { targetVeloRPS1 = Constants.ShooterConstants.RPSHardStop; @@ -100,19 +131,26 @@ public void setTargetVelocity(double VelocityRPS) { } shooterEnabled = true; } - + // with mult public void setTargetVeloDistance(double distance) { double inches = distance * 39.3701; - targetVeloRPS1 = Shooter1Map.get(inches); - targetVeloRPS2 = Shooter2Map.get(inches); - targetVeloRPS3 = Shooter3Map.get(inches); - targetVeloRPS4 = Shooter4Map.get(inches); + targetVeloRPS1 = Shooter1Map.get(inches) * ShooterMult; + targetVeloRPS2 = Shooter2Map.get(inches)* ShooterMult; + targetVeloRPS3 = Shooter3Map.get(inches)* ShooterMult; + targetVeloRPS4 = Shooter4Map.get(inches)* ShooterMult; if (targetVeloRPS1 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS1 = Constants.ShooterConstants.RPSHardStop;} if (targetVeloRPS2 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS2 = Constants.ShooterConstants.RPSHardStop;} if (targetVeloRPS3 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS3 = Constants.ShooterConstants.RPSHardStop;} if (targetVeloRPS4 >= Constants.ShooterConstants.RPSHardStop) { targetVeloRPS4 = Constants.ShooterConstants.RPSHardStop;} + if (targetVeloRPS1 >= 104.0){ + MaxRPM = true; + } + else{ + MaxRPM = false; + } + shooterEnabled = true; } @@ -128,6 +166,11 @@ public boolean atTargetVelo() { boolean atTargBelo = Math.abs(ShooterMotor.getVelocity().getValueAsDouble()) >= targetVeloRPS1-2.0; return atTargBelo; } + + public boolean atTargetVeloPassing() { + boolean atTargBelo = (Math.abs(ShooterMotor.getVelocity().getValueAsDouble()) >= targetVeloRPS1-2.0) || ShooterMotor.getVelocity().getValueAsDouble() >= 98.0; + return atTargBelo; + } @Override public void periodic() { @@ -175,5 +218,9 @@ public void periodic() { SmartDashboard.putNumber("TargetRPS2", targetVeloRPS2); SmartDashboard.putNumber("TargetRPS3", targetVeloRPS3); SmartDashboard.putNumber("TargetRPS4", targetVeloRPS4); + + SmartDashboard.putNumber("ShooterMult", ShooterMult); + SmartDashboard.putBoolean("MaxRPM?", MaxRPM); + SmartDashboard.putBoolean("MultpilerOn?", MultiplierOn); } } diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java index 2ea0a02..e470f38 100644 --- a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java +++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java @@ -42,7 +42,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; -import frc.robot.Constants.DriveConstants; import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain; /** * Class that extends the Phoenix 6 SwerveDrivetrain class and implements @@ -349,21 +348,42 @@ public Optional samplePoseAt(double timestampSeconds) { // --------------------- END GENERATED CODE ------------------- + private double hubOffsetX = 0.0; + private double hubOffsetY = 0.0; + + public void setHubOffset(double offsetX, double offsetY) { + hubOffsetX += offsetX; + hubOffsetY += offsetY; + } + + public void resetHubOffset() { + hubOffsetX = 0.0; + hubOffsetY = 0.0; + } // Get hub pose based on alliance - public static Pose3d getHubPose() { - return DriverStation.getAlliance() - .map(alliance -> alliance == Alliance.Red ? Constants.FieldConstants.redHubPose : Constants.FieldConstants.blueHubPose) - .orElse(Constants.FieldConstants.blueHubPose); // Default to blue if alliance unknown - } +// Replace the existing static getHubPose() with this instance version +public Pose3d getHubPose() { + Pose3d base = DriverStation.getAlliance() + .map(alliance -> alliance == Alliance.Red ? Constants.FieldConstants.redHubPose : Constants.FieldConstants.blueHubPose).orElse(Constants.FieldConstants.blueHubPose); + + // Apply offsets relative to alliance + // For Red alliance, flip X offset direction since field is mirrored + double adjustedX = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red + ? base.getX() - hubOffsetX + : base.getX() + hubOffsetX; + + double adjustedY = base.getY() + hubOffsetY; + + return new Pose3d(adjustedX, adjustedY, base.getZ(), base.getRotation()); +} public static Distance getCloseBumpY(Pose2d currentPose){ - if (currentPose.getMeasureY().gt(Inches.of(158.845))){ //if closer to blue left + if (currentPose.getMeasureY().gt(Inches.of(158.845))){ return Inches.of(218.84); - } - else{ - return Inches.of(98.84); //closer to blue right Bump + } else { + return Inches.of(98.84); } } @@ -472,61 +492,31 @@ public Command headingLocktoHub(CommandXboxController controller, double maxSpee currentAngle.getRadians(), desiredAngle.getRadians() ); - double veloX = -controller.getLeftY(); - if (Math.abs(veloX) < 0.1 ){ - veloX = 0; - } + double xSpeed = MathUtil.applyDeadband(-controller.getLeftY(), 0.1); + double ySpeed = MathUtil.applyDeadband(-controller.getLeftX(), 0.1); - double veloY = -controller.getLeftX(); - if (Math.abs(veloY) < 0.1 ){ - veloY = 0; - } - // Apply the request: radial (distance maintenance) + tangential (circling) return alignRequest - .withVelocityX(veloX * maxSpeed) - .withVelocityY(veloY * maxSpeed) + .withVelocityX(xSpeed * maxSpeed) + .withVelocityY(ySpeed * maxSpeed) .withRotationalRate(rotationalRate * maxAngularRate*1.5); }); } -private final SwerveRequest.RobotCentric chaseRequest = new SwerveRequest.RobotCentric(); - -public Command detectChase(Vision vision, double maxSpeed, double maxAngularRate) { +public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ return applyRequest(() -> { - double[] data = vision.getFuelData(); - - // data[0] = cluster count, [1] = best angle X, [2] = angle Y, - // [3] = distance inches, [4] = score, [5] = fuel count - if (data == null || data.length < 6 || data[0] == 0) { - return chaseRequest.withVelocityX(0).withVelocityY(0).withRotationalRate(0); - } - - double tx = data[1]; // horizontal angle to best cluster, negative = left - - double targetAngle = getState().Pose.getRotation().getRadians() + Math.toRadians(tx); - double rotationalRate = rotationController.calculate( - getState().Pose.getRotation().getRadians(), - targetAngle - ); - - // only drive forward once aimed at target - double forwardSpeed = Math.abs(tx) < DriveConstants.RotationalToleranceDegrees ? maxSpeed : 0; - - SmartDashboard.putNumber("Chase/TX", tx); - SmartDashboard.putNumber("Chase/Distance", data[3]); - SmartDashboard.putNumber("Chase/FuelCount", data[5]); - SmartDashboard.putNumber("Chase/Score", data[4]); - SmartDashboard.putNumber("Chase/RotationalRate", rotationalRate); - - return chaseRequest - .withVelocityX(forwardSpeed) - .withVelocityY(0) - .withRotationalRate(rotationalRate); + // Apply 10% deadband to joystick inputs + double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); + double ySpeed = MathUtil.applyDeadband(-joystick.getLeftX(), 0.1); + double rotSpeed = MathUtil.applyDeadband(-joystick.getRightX(), 0.1); + + return alignRequest + .withVelocityX(xSpeed * MaxSpeed) + .withVelocityY(ySpeed * MaxSpeed) + .withRotationalRate(rotSpeed * MaxAngularRate); }); } - -public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ +public Command TeleopDriveSLOW(CommandXboxController joystick, double MaxSpeed, double MaxAngularRate, SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain){ return applyRequest(() -> { // Apply 10% deadband to joystick inputs double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); @@ -534,8 +524,8 @@ public Command TeleopDrive(CommandXboxController joystick, double MaxSpeed, doub double rotSpeed = MathUtil.applyDeadband(-joystick.getRightX(), 0.1); return alignRequest - .withVelocityX(xSpeed * MaxSpeed) - .withVelocityY(ySpeed * MaxSpeed) + .withVelocityX(xSpeed * MaxSpeed*0.5) + .withVelocityY(ySpeed * MaxSpeed*0.5) .withRotationalRate(rotSpeed * MaxAngularRate); }); } @@ -637,7 +627,7 @@ public boolean isAimedAtTarget() { Rotation2d currentAngle = currentPose.getRotation(); // Calculate required aim angle (same as headingLocktoHub) - Translation2d target = getHubPose().toPose2d().getTranslation(); + Translation2d target = getTargetPose(currentPose).getTranslation(); Translation2d toTarget = target.minus(currentPose.getTranslation()); Rotation2d targetAngle = toTarget.getAngle(); // Face towards from hub .plus(Krot180) or sum idk @@ -648,7 +638,7 @@ public boolean isAimedAtTarget() { return errorDegrees <= Constants.DriveConstants.RotationalToleranceDegrees; } - public Command bumpLockCommand(SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, Double MaxSpeed, double MaxAngularRate){ +public Command bumpLockCommand(SwerveRequest.FieldCentric drive, CommandSwerveDrivetrain drivetrain, CommandXboxController joystick, Double MaxSpeed, double MaxAngularRate){ return applyRequest(() -> { double closeTrench = (double)getCloseBumpY(drivetrain.getPose()).in(Meters); double xSpeed = MathUtil.applyDeadband(-joystick.getLeftY(), 0.1); @@ -672,9 +662,12 @@ public Command bumpLockCommand(SwerveRequest.FieldCentric drive, CommandSwerveDr rotSpeedToStraight = 0; } + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red){ + yVel = -yVel; + } return drive - .withVelocityX(xSpeed*MaxSpeed) // - .withVelocityY(-yVel *MaxSpeed) + .withVelocityX(xSpeed*MaxSpeed) // + .withVelocityY(yVel *MaxSpeed) .withRotationalRate(rotSpeedToStraight*MaxAngularRate); }); } @@ -726,6 +719,9 @@ public void periodic() { if(ShootingLocation != null){field.getObject("Shooting Target").setPose(ShootingLocation);} field.setRobotPose(getPose()); SmartDashboard.putNumber("distanceToCenterHubInches", getDistance() * 39.3701); + SmartDashboard.putString("close trench X val", getCloseBumpY(getPose()).toShortString()); + SmartDashboard.putNumber("Hub/OffsetX", hubOffsetX); + SmartDashboard.putNumber("Hub/OffsetY", hubOffsetY); /* * Periodically try to apply the operator perspective. diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7ff682a..2e1fbd0 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -19,7 +19,7 @@ public class IntakeSubsystem extends SubsystemBase { - private final TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID, "Upper"); + private final TalonFX intakeMotor = new TalonFX(Constants.IntakeConstants.IntakeMotorID, "Upper"); private final TalonFX intakeFollowerMotor = new TalonFX(Constants.IntakeConstants.IntakeFollowerReversedMotorID, "Upper"); public IntakeSubsystem() { diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index b5b5115..cd33b7b 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -3,8 +3,6 @@ import edu.wpi.first.math.VecBuilder; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.LimelightHelpers; -import frc.robot.LimelightHelpers.RawDetection; -import frc.robot.LimelightHelpers.RawFiducial; public class Vision extends SubsystemBase { @@ -22,7 +20,7 @@ public Vision(CommandSwerveDrivetrain drivetrain) { */ // All Pose estimating limelights names - private final String[] LLNames = { + private final String[] PosLimelights = { "limelight-fl", "limelight-fr" // add more as needed @@ -101,46 +99,8 @@ public void updateVisionMeasurements(String LLName) { @Override public void periodic() { - for (String limeLight : LLNames) { + for (String limeLight : PosLimelights) { updateVisionMeasurements(limeLight); } } - - public boolean hasDetection() { - for (String ll : LLNames) { - if (LimelightHelpers.getRawDetections(ll).length > 0) return true; - } - return false; - } - - public double getDetectionTX() { - RawDetection best = null; - for (String ll : LLNames) { - for (RawDetection d : LimelightHelpers.getRawDetections(ll)) { - if (best == null || Math.abs(d.txnc) < Math.abs(best.txnc)) { - best = d; - } - } - } - return best != null ? best.txnc : 0.0; - } - - public double[] getFuelData() { - for (String ll : LLNames) { - double[] data = LimelightHelpers.getPythonScriptData(ll); - if (data != null && data.length > 0) return data; - } - return new double[0]; - } - - public RawDetection[] getAllFuel() { - java.util.List all = new java.util.ArrayList<>(); - for (String ll : LLNames) { - for (RawDetection d : LimelightHelpers.getRawDetections(ll)) { - all.add(d); - } - } - return all.toArray(new RawDetection[0]); - } - } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/HubShiftUtil.java b/src/main/java/frc/robot/utils/HubShiftUtil.java index 5c0bdde..fd04a20 100644 --- a/src/main/java/frc/robot/utils/HubShiftUtil.java +++ b/src/main/java/frc/robot/utils/HubShiftUtil.java @@ -34,12 +34,11 @@ public record ShiftInfo( private static final double minFuelCountDelay = 1.0; private static final double maxFuelCountDelay = 2.0; + private static final double minTimeOfFlight = 1.0; //made ts up + private static final double maxTimeOfFlight = 3.0; //made ts up private static final double shiftEndFuelCountExtension = 3.0; - private static final double minTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MinShootingDistanceInches + 0.330879; - private static final double maxTimeOfFlight = 0.0115177 * Constants.ShiftConstants.MaxShootingDistanceInches + 0.330879; private static final double approachingActiveFudge = -1 * (minTimeOfFlight + minFuelCountDelay); - private static final double endingActiveFudge = - shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); + private static final double endingActiveFudge = shiftEndFuelCountExtension + -1 * (maxTimeOfFlight + maxFuelCountDelay); public static final double autoEndTime = 20.0; public static final double teleopDuration = 140.0; @@ -137,7 +136,6 @@ private static ShiftInfo getShiftInfo( active = currentSchedule[currentShiftIndex]; currentShift = shiftsEnums[currentShiftIndex]; } - SmartDashboard.putBoolean("Shift Active", active); ShiftInfo shiftInfo = new ShiftInfo(currentShift, stateTimeElapsed, stateTimeRemaining, active); return shiftInfo; } @@ -185,4 +183,8 @@ public static ShiftInfo getShiftedShiftInfo() { }; return getShiftInfo(shiftSchedule, shiftedShiftStartTimes, shiftedShiftEndTimes); } + + public static double getMatchTime(){ + return 140.0-shiftTimer.get(); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/utils/RumbleUtils.java b/src/main/java/frc/robot/utils/RumbleUtils.java new file mode 100644 index 0000000..208eac0 --- /dev/null +++ b/src/main/java/frc/robot/utils/RumbleUtils.java @@ -0,0 +1,24 @@ +package frc.robot.utils; + +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; + +public class RumbleUtils { + + /** + * Creates a command that rumbles a controller for a set duration. + * + * @param controller The controller to rumble + * @param strength Rumble strength from 0.0 to 1.0 + * @param seconds How long to rumble in seconds + * @return Command that can be scheduled + */ + public static Command rumble(CommandXboxController controller, double strength, double seconds) { + return Commands.startEnd( + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, strength), + () -> controller.getHID().setRumble(GenericHID.RumbleType.kBothRumble, 0.0) + ).withTimeout(seconds); + } +} \ No newline at end of file