From 67b308c6bf2e9c6998d98ab7bff3edcf4fca0f8b Mon Sep 17 00:00:00 2001 From: micw1417 Date: Mon, 16 Feb 2026 16:02:04 -0600 Subject: [PATCH 01/21] commands for intake outale whole and shooter intake modifacations --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 41 ++- .../frc/robot/commands/InhaleCommand.java | 33 ++ .../frc/robot/commands/ShootFuelCommand.java | 27 ++ .../frc/robot/generated/TunerConstants.java | 298 +++++++++++++++++- .../subsystems/hopper/CarpetConstants.java | 3 +- .../frc/robot/subsystems/intake/Intake.java | 25 +- .../subsystems/intake/IntakeConstants.java | 6 +- .../frc/robot/subsystems/shooter/Shooter.java | 37 +-- .../subsystems/shooter/ShooterConstants.java | 28 +- 10 files changed, 435 insertions(+), 65 deletions(-) create mode 100644 src/main/java/frc/robot/commands/InhaleCommand.java create mode 100644 src/main/java/frc/robot/commands/ShootFuelCommand.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 16a2bd6..15aa26a 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,7 +20,7 @@ public final class Constants { public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; // CAN bus that the devices are located on; - public static final CANBus CANBUS = new CANBus("canivore", "./logs/example.hoot"); + public static final CANBus CANBUS = new CANBus("Canivore", "./logs/example.hoot"); public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index de14205..22ad2bb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.commands.InhaleCommand; +import frc.robot.commands.ShootFuelCommand; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberIO; @@ -29,10 +31,15 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOTalonFX; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederIO; +import frc.robot.subsystems.feeder.FeederIOSim; +import frc.robot.subsystems.feeder.FeederIOSparkFlex; import frc.robot.subsystems.hopper.Carpet; import frc.robot.subsystems.hopper.CarpetIO; import frc.robot.subsystems.hopper.CarpetIOSim; import frc.robot.subsystems.hopper.CarpetIOSparkFlex; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; import frc.robot.subsystems.intake.pivot.PivotIO; @@ -41,7 +48,9 @@ import frc.robot.subsystems.intake.roller.RollerIO; import frc.robot.subsystems.intake.roller.RollerIOSim; import frc.robot.subsystems.intake.roller.RollerIOSparkFlex; +import frc.robot.subsystems.intake.roller.RollerIOTalonFX; import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOSim; import frc.robot.subsystems.shooter.angler.AnglerIOTalonFX; @@ -66,6 +75,7 @@ public class RobotContainer { private final Shooter shooter; private final Carpet carpet; private final Climber climber; + private final Feeder feeder; // Controller private final CommandXboxController driver = new CommandXboxController(0); @@ -96,10 +106,11 @@ public RobotContainer() { // new VisionIOLimelight(camera0Name, drive::getRotation), // new VisionIOLimelight(camera1Name, drive::getRotation)); - intake = new Intake(new RollerIOSim(), new PivotIOSim()); - shooter = new Shooter(new AnglerIOSim(), new FlywheelIOSim()); - carpet = new Carpet(new CarpetIOSim()); + intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); + shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); + carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); + feeder = new Feeder(new FeederIOSparkFlex()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -148,6 +159,7 @@ public RobotContainer() { new Carpet(new CarpetIOSim()); climber = new Climber(new ClimberIOSim()); + feeder = new Feeder(new FeederIOSim()); break; default: // Replayed robot, disable IO implementations @@ -164,6 +176,7 @@ public RobotContainer() { shooter = new Shooter(new AnglerIO() {}, new FlywheelIO() {}); carpet = new Carpet(new CarpetIO() {}); climber = new Climber(new ClimberIO() {}); + feeder = new Feeder(new FeederIO() {}); break; } @@ -202,7 +215,7 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureDriverBindings() { - // Default command, normal field-relative drive + // Default Commands drive.setDefaultCommand( DriveCommands.joystickDrive( drive, @@ -210,6 +223,9 @@ private void configureDriverBindings() { () -> -driver.getLeftX(), () -> -driver.getRightX())); + + intake.setDefaultCommand(intake.stop()); + // shooter.setDefaultCommand(); // // Lock to 0° when A button is held // driver // .a() @@ -225,7 +241,7 @@ private void configureDriverBindings() { // Reset gyro to 0° when B button is pressed driver - .b() + .y() .onTrue( Commands.runOnce( () -> @@ -233,6 +249,12 @@ private void configureDriverBindings() { new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); + driver.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in + driver.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); + driver.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); + driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); + // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") @@ -252,13 +274,8 @@ private void configureDriverBindings() { } private void configureOperatorBindings() { - // Default Commands - intake.setDefaultCommand(intake.runIntakeeCommand(IntakeMode.OUT_IDLE)); - - - - operator.a().whileTrue(intake.runIntakeeCommand(IntakeMode.INTAKE)); - operator.b().whileTrue(intake.runIntakeeCommand(IntakeMode.OUTTAKE)); + // operator.a().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); + // operator.b().whileTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/InhaleCommand.java b/src/main/java/frc/robot/commands/InhaleCommand.java new file mode 100644 index 0000000..d3d8a01 --- /dev/null +++ b/src/main/java/frc/robot/commands/InhaleCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; + + +public class InhaleCommand extends ParallelCommandGroup { + + + public InhaleCommand(Intake intake, Carpet carpet, Feeder feeder, boolean isInhaling) { + if (isInhaling) { + addCommands( + intake.runIntakeCommand(IntakeMode.INTAKE), + carpet.runCarpetCommand(CarpetModes.INTAKE), + feeder.runFeederCommand(FeederModes.FEEDER) + ); + } else { + addCommands( + intake.runIntakeCommand(IntakeMode.OUTTAKE), + carpet.runCarpetCommand(CarpetModes.OUTTAKE), + feeder.runFeederCommand(FeederModes.REVERSE) + ); + } + addRequirements(intake, carpet, feeder); + } + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java new file mode 100644 index 0000000..770145f --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; + +public class ShootFuelCommand extends SequentialCommandGroup { + public ShootFuelCommand(Intake intake, Carpet carpet, Feeder feeder, Shooter shooter) { + addCommands( + new ParallelRaceGroup( + feeder.runFeederCommand(FeederModes.REVERSE), + new WaitCommand(0.5)), + new ParallelCommandGroup( + new InhaleCommand(intake, carpet, feeder, true).withName("ShootFuelCommand.Inhale")), + shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withName("ShootFuelCommand.Shoot") + ); + + addRequirements(intake, carpet, feeder, shooter); + } +} diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index bd1daa8..78b7de0 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -127,7 +127,7 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.301513671875); + private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.298095703125); private static final boolean kFrontLeftSteerMotorInverted = false; private static final boolean kFrontLeftEncoderInverted = true; @@ -138,7 +138,7 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 3; private static final int kFrontRightEncoderId = 10; - private static final Angle kFrontRightEncoderOffset = Rotations.of(0.318603515625); + private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.320556640625); private static final boolean kFrontRightSteerMotorInverted = false; private static final boolean kFrontRightEncoderInverted = true; @@ -149,7 +149,7 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 12; - private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181396484375); + private static final Angle kBackLeftEncoderOffset = Rotations.of(0.18017578125); private static final boolean kBackLeftSteerMotorInverted = false; private static final boolean kBackLeftEncoderInverted = true; @@ -160,7 +160,7 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 5; private static final int kBackRightSteerMotorId = 6; private static final int kBackRightEncoderId = 11; - private static final Angle kBackRightEncoderOffset = Rotations.of(-0.458984375); + private static final Angle kBackRightEncoderOffset = Rotations.of(0.4580078125); private static final boolean kBackRightSteerMotorInverted = false; private static final boolean kBackRightEncoderInverted = true; @@ -260,10 +260,10 @@ public TunerSwerveDrivetrain( * unspecified or set to 0 Hz, this is 250 Hz on * CAN FD, and 100 Hz on CAN 2.0. * @param odometryStandardDeviation The standard deviation for odometry calculation - * in the form [x, y, theta]áµ€, with units in meters + * in the form [x, y, theta]ᵀ, with units in meters * and radians * @param visionStandardDeviation The standard deviation for vision calculation - * in the form [x, y, theta]áµ€, with units in meters + * in the form [x, y, theta]ᵀ, with units in meters * and radians * @param modules Constants for each specific module */ @@ -282,3 +282,289 @@ public TunerSwerveDrivetrain( } } } + +// package frc.robot.generated; + +// import static edu.wpi.first.units.Units.*; + +// import com.ctre.phoenix6.CANBus; +// import com.ctre.phoenix6.configs.*; +// import com.ctre.phoenix6.hardware.*; +// import com.ctre.phoenix6.signals.*; +// import com.ctre.phoenix6.swerve.*; +// import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.numbers.N1; +// import edu.wpi.first.math.numbers.N3; +// import edu.wpi.first.units.measure.*; + +// // Generated by the 2026 Tuner X Swerve Project Generator +// // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +// public class TunerConstants { +// // Both sets of gains need to be tuned to your individual robot. + +// // The steer motor uses any SwerveModule.SteerRequestType control request with the +// // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput +// private static final Slot0Configs steerGains = new Slot0Configs() +// .withKP(100).withKI(0).withKD(0.5) +// .withKS(0.1).withKV(2.33).withKA(0) +// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); +// // When using closed-loop control, the drive motor uses the control +// // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput +// private static final Slot0Configs driveGains = new Slot0Configs() +// .withKP(0.1).withKI(0).withKD(0) +// .withKS(0).withKV(0.124); + +// // The closed-loop output type to use for the steer motors; +// // This affects the PID/FF gains for the steer motors +// private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; +// // The closed-loop output type to use for the drive motors; +// // This affects the PID/FF gains for the drive motors +// private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + +// // The type of motor used for the drive motor +// private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; +// // The type of motor used for the drive motor +// private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + +// // The remote sensor feedback type to use for the steer motors; +// // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* +// private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + +// // The stator current at which the wheels start to slip; +// // This needs to be tuned to your individual robot +// private static final Current kSlipCurrent = Amps.of(120); + +// // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. +// // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. +// private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); +// private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() +// .withCurrentLimits( +// new CurrentLimitsConfigs() +// // Swerve azimuth does not require much torque output, so we can set a relatively low +// // stator current limit to help avoid brownouts without impacting performance. +// .withStatorCurrentLimit(Amps.of(60)) +// .withStatorCurrentLimitEnable(true) +// ); +// private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); +// // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs +// private static final Pigeon2Configuration pigeonConfigs = null; + +// // CAN bus that the devices are located on; +// // All swerve devices must share the same CAN bus +// public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + +// // Theoretical free speed (m/s) at 12 V applied output; +// // This needs to be tuned to your individual robot +// public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); + +// // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; +// // This may need to be tuned to your individual robot +// private static final double kCoupleRatio = 3.125; + +// private static final double kDriveGearRatio = 5.902777777777778; +// private static final double kSteerGearRatio = 18.75; +// private static final Distance kWheelRadius = Inches.of(2); + +// private static final boolean kInvertLeftSide = false; +// private static final boolean kInvertRightSide = true; + +// private static final int kPigeonId = 49; + +// // These are only used for simulation +// private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); +// private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); +// // Simulated voltage necessary to overcome friction +// private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); +// private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + +// public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() +// .withCANBusName(kCANBus.getName()) +// .withPigeon2Id(kPigeonId) +// .withPigeon2Configs(pigeonConfigs); + +// private static final SwerveModuleConstantsFactory ConstantCreator = +// new SwerveModuleConstantsFactory() +// .withDriveMotorGearRatio(kDriveGearRatio) +// .withSteerMotorGearRatio(kSteerGearRatio) +// .withCouplingGearRatio(kCoupleRatio) +// .withWheelRadius(kWheelRadius) +// .withSteerMotorGains(steerGains) +// .withDriveMotorGains(driveGains) +// .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) +// .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) +// .withSlipCurrent(kSlipCurrent) +// .withSpeedAt12Volts(kSpeedAt12Volts) +// .withDriveMotorType(kDriveMotorType) +// .withSteerMotorType(kSteerMotorType) +// .withFeedbackSource(kSteerFeedbackType) +// .withDriveMotorInitialConfigs(driveInitialConfigs) +// .withSteerMotorInitialConfigs(steerInitialConfigs) +// .withEncoderInitialConfigs(encoderInitialConfigs) +// .withSteerInertia(kSteerInertia) +// .withDriveInertia(kDriveInertia) +// .withSteerFrictionVoltage(kSteerFrictionVoltage) +// .withDriveFrictionVoltage(kDriveFrictionVoltage); + + +// // Front Left +// private static final int kFrontLeftDriveMotorId = 1; +// private static final int kFrontLeftSteerMotorId = 2; +// private static final int kFrontLeftEncoderId = 9; +// private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.304199); +// private static final boolean kFrontLeftSteerMotorInverted = false; +// private static final boolean kFrontLeftEncoderInverted = true; + +// private static final Distance kFrontLeftXPos = Inches.of(11); +// private static final Distance kFrontLeftYPos = Inches.of(11.5); + +// // Front Right +// private static final int kFrontRightDriveMotorId = 4; +// private static final int kFrontRightSteerMotorId = 3; +// private static final int kFrontRightEncoderId = 10; +// private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.365234); +// private static final boolean kFrontRightSteerMotorInverted = false; +// private static final boolean kFrontRightEncoderInverted = true; + +// private static final Distance kFrontRightXPos = Inches.of(11); +// private static final Distance kFrontRightYPos = Inches.of(-11.5); + +// // Back Left +// private static final int kBackLeftDriveMotorId = 7; +// private static final int kBackLeftSteerMotorId = 8; +// private static final int kBackLeftEncoderId = 12; +// private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.363037); +// private static final boolean kBackLeftSteerMotorInverted = false; +// private static final boolean kBackLeftEncoderInverted = true; + +// private static final Distance kBackLeftXPos = Inches.of(-11); +// private static final Distance kBackLeftYPos = Inches.of(11.5); + +// // Back Right +// private static final int kBackRightDriveMotorId = 5; +// private static final int kBackRightSteerMotorId = 6; +// private static final int kBackRightEncoderId = 11; +// private static final Angle kBackRightEncoderOffset = Rotations.of(-0.082275); +// private static final boolean kBackRightSteerMotorInverted = false; +// private static final boolean kBackRightEncoderInverted = true; + +// private static final Distance kBackRightXPos = Inches.of(-11); +// private static final Distance kBackRightYPos = Inches.of(-11.5); + + +// public static final SwerveModuleConstants FrontLeft = +// ConstantCreator.createModuleConstants( +// kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, +// kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted +// ); +// public static final SwerveModuleConstants FrontRight = +// ConstantCreator.createModuleConstants( +// kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, +// kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted +// ); +// public static final SwerveModuleConstants BackLeft = +// ConstantCreator.createModuleConstants( +// kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, +// kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted +// ); +// public static final SwerveModuleConstants BackRight = +// ConstantCreator.createModuleConstants( +// kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, +// kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted +// ); + +// // /** +// // * Creates a CommandSwerveDrivetrain instance. +// // * This should only be called once in your robot program,. +// // */ +// // public static CommandSwerveDrivetrain createDrivetrain() { +// // return new CommandSwerveDrivetrain( +// // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight +// // ); +// // } + + +// /** +// * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. +// */ +// public static class TunerSwerveDrivetrain extends SwerveDrivetrain { +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, modules +// ); +// } + +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// * unspecified or set to 0 Hz, this is 250 Hz on +// * CAN FD, and 100 Hz on CAN 2.0. +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// double odometryUpdateFrequency, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, odometryUpdateFrequency, modules +// ); +// } + +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// * unspecified or set to 0 Hz, this is 250 Hz on +// * CAN FD, and 100 Hz on CAN 2.0. +// * @param odometryStandardDeviation The standard deviation for odometry calculation +// * in the form [x, y, theta]áµ€, with units in meters +// * and radians +// * @param visionStandardDeviation The standard deviation for vision calculation +// * in the form [x, y, theta]áµ€, with units in meters +// * and radians +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// double odometryUpdateFrequency, +// Matrix odometryStandardDeviation, +// Matrix visionStandardDeviation, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, odometryUpdateFrequency, +// odometryStandardDeviation, visionStandardDeviation, modules +// ); +// } +// } +// } + diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index e8b6f20..e8ead93 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -12,7 +12,8 @@ public class CarpetConstants { public enum CarpetModes { IDLE(Units.Volts.of(0.0)), - INTAKE(Units.Volts.of(8.4)); + INTAKE(Units.Volts.of(5.4)), + OUTTAKE(Units.Volts.of(-2.8)); public Voltage voltage; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index e3ce7ce..cd512ce 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -45,21 +45,22 @@ public void runIntakeEnum(IntakeMode intakeMode) { pivotIO.setPosition(intakeMode.position.magnitude()); } - public Command runIntakeeCommand(IntakeMode intakeMode) { + public Command runIntakeCommand(IntakeMode intakeMode) { return new RunCommand(() -> { this.runIntakeEnum(intakeMode); - }, this).withName("Intake.runIntakeeCommand" + intakeMode.toString()); + }, this).withName("Intake.runIntakeCommand" + intakeMode.toString()); } - // public Command runPivotCommand(IntakeMode intakeMode) { - // return new RunCommand(() -> { - // this.runPivotEnum(intakeMode); - // }, this).withName("Intake.runPivotCommand" + intakeMode.toString()); - // } + public Command stop() { + // return new RunCommand(() -> { + // this.pivotIO.setPivotVoltage(0); + // this.pivotIO.setPivotVoltage(0); + // }, this).withName("Intake.stopAll"); + + return run(() -> { + rollerIO.setRollerVoltage(0); + pivotIO.setPivotVoltage(0); + }).withName("Intake.Stop"); - // public Command runRollerCommand(IntakeMode rollerMode) { - // return new RunCommand(() -> { - // this.runRollerEnum(rollerMode); - // }, this).withName("Intake.runRollerCommand" + rollerMode.toString()); - // } + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index a33f971..8f1a88b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -15,8 +15,8 @@ public enum IntakeMode { STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), - INTAKE(Units.Volts.of(8.4), Units.Degrees.of(0.0)), - OUTTAKE(Units.Volts.of(-4.8), Units.Degrees.of(180.0)); + INTAKE(Units.Volts.of(5.4), Units.Degrees.of(0.0)), + OUTTAKE(Units.Volts.of(-2.8), Units.Degrees.of(180.0)); public Voltage voltage; public Angle position; @@ -71,7 +71,7 @@ public static final class IntakePivotConstants { public static final boolean invert = false; public static final boolean gravityType = false; - public static final boolean breakType = false; + public static final boolean breakType = true; public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; public static final FeedbackSensor feedbackSensorREV = FeedbackSensor.kAbsoluteEncoder; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index d455c1d..7c36313 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,7 +2,10 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; @@ -14,11 +17,13 @@ public class Shooter extends SubsystemBase { private final AnglerIOInputsAutoLogged anglerInputs = new AnglerIOInputsAutoLogged(); private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); - + + private ShooterModes shooterMode; public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { this.anglerIO = anglerIO; this.flywheelIO = flywheelIO; + this.shooterMode = ShooterModes.IDLE; } @Override @@ -28,23 +33,19 @@ public void periodic() { flywheelIO.updateInputs(flywheelInputs); Logger.processInputs("Shooter/Flywheel", flywheelInputs); - // Logger.recordOutput("Intake/Mode", mode); + + Logger.recordOutput("Shooter/Mode", shooterMode); } - // public void runFlywheelEnum(IntakeMode intakeMode) { - // this.mode = intakeMode; - // flywheelIO.setRPM(mode.voltage.baseUnitMagnitude()); - // } - - // public void runAnglerEnum(IntakeMode intakeMode) { - // this.mode = intakeMode; - // anglerIO.setPosition(mode.position.magnitude()); - // } - - // public Command runIntakeCommand(IntakeMode intakeMode) { - // return new RunCommand(() -> { - // this.runFlywheelEnum(intakeMode); - // this.runAnglerEnum(intakeMode); - // }, this).withName("Shooter.runIntakeEnum"); - // } + public void runShooterEnum(ShooterModes mode) { + this.shooterMode = mode; + flywheelIO.setRPM(mode.speed.baseUnitMagnitude()); + anglerIO.setPosition(mode.angle.magnitude()); + } + + public Command runShooterCommand(ShooterModes mode) { + return new RunCommand(() -> { + this.runShooterEnum(mode); + }, this).withName("Shooter.runShooterEnum" + mode.toString()); + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 72be9c4..c36907e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -9,21 +9,25 @@ public class ShooterConstants { - // public enum ShooterModes { - // SHOOT( Units.RPM.of(2000)), - // IDLE( Units.RPM.of(0)); + public enum ShooterModes { + SHOOT_FAR( Units.RPM.of(200), Units.Degree.of(30)), + SHOOT_CLOSE(Units.RPM.of(150), Units.Degree.of(60)), + IDLE( Units.RPM.of(0), Units.Degree.of(0.0)); - // public AngularVelocity speed; + public AngularVelocity speed; + public Angle angle; - // ShooterModes(AngularVelocity speed) { - // this.speed = speed; - // } - // } + ShooterModes(AngularVelocity speed, Angle angle) { + this.speed = speed; + this.angle = angle; + } + } public static class ShooterFlywheelConstants { public static final boolean attached = true; - public static final int followerId = 50; - public static final int leaderId = 50; + public static final int leaderId = 13; + public static final int followerId = 14; + public static final boolean inverted = false; public static final boolean breakType = false; public static final double gearRatio = 1 / 1; @@ -43,10 +47,10 @@ public static class ShooterFlywheelConstants { public static class ShooterAnglerConstants { public static final boolean attached = true; - public static final int id = 50; + public static final int id = 15; public static final boolean inverted = false; public static final boolean breakType = false; - public static final double gearRatio = 3.0 / 1; + public static final double gearRatio = 1 / 1; public static final double p = 1; public static final double i = 0; From d168cf254313b683ed7fc472c616960ef1f5d952 Mon Sep 17 00:00:00 2001 From: AlbedoLinguine Date: Tue, 17 Feb 2026 16:19:52 -0600 Subject: [PATCH 02/21] IDs --- .../climber/ClimberIOSparkFlex.java | 3 +- .../subsystems/feeder/FeederConstants.java | 2 +- .../subsystems/hopper/CarpetConstants.java | 2 +- .../subsystems/intake/IntakeConstants.java | 4 +- .../intake/pivot/PivotIOSparkFlex.java | 3 +- .../intake/roller/RollerIOSparkFlex.java | 3 +- vendordeps/REVLib.json | 264 +++++++++--------- vendordeps/photonlib.json | 140 +++++----- 8 files changed, 212 insertions(+), 209 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java index 2de40a0..230fe6f 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSparkFlex.java @@ -5,6 +5,7 @@ import com.revrobotics.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.SparkFlex; import static frc.robot.util.SparkUtil.*; @@ -12,7 +13,7 @@ import frc.team5431.titan.core.subsystem.REVMechanism; public class ClimberIOSparkFlex implements ClimberIO { - private final SparkFlex sparkFlex = new SparkFlex(ClimberConstants.id, null); + private final SparkFlex sparkFlex = new SparkFlex(ClimberConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class PivotSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 7b8d5e6..4f23545 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -15,7 +15,7 @@ public enum FeederState { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 16; public static final double p = 1; public static final double i = 0; diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index e8ead93..317b6db 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -25,7 +25,7 @@ public enum CarpetModes { public static final class CarpetRollerConstants { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 17; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 8f1a88b..f057c6a 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -31,7 +31,7 @@ public static final class IntakeRollerConstants { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 19; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; @@ -60,7 +60,7 @@ public static final class IntakeRollerConstants { public static final class IntakePivotConstants { public static final boolean attached = true; - public static final int id = -1; + public static final int id = 18; public static final double p = 1; public static final double i = 0; diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java index 5e7485a..f9f60e4 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -8,12 +8,13 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class PivotIOSparkFlex implements PivotIO { - private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class PivotSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java index 488d0a8..90ae857 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOSparkFlex.java @@ -8,12 +8,13 @@ import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; import frc.robot.subsystems.intake.IntakeConstants.IntakeRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class RollerIOSparkFlex implements RollerIO { - private final SparkFlex sparkFlex = new SparkFlex(IntakeRollerConstants.id, null); + private final SparkFlex sparkFlex = new SparkFlex(IntakeRollerConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class RollerIOSparkFlexConfig extends REVMechanism.Config { public RollerIOSparkFlexConfig() { diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index bb613bf..d8ba7ce 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,133 +1,133 @@ { - "fileName": "REVLib.json", - "name": "REVLib", - "version": "2026.0.1", - "frcYear": "2026", - "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", - "mavenUrls": [ - "https://maven.revrobotics.com/" - ], - "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", - "javaDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-java", - "version": "2026.0.1" - } - ], - "jniDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2026.0.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-cpp", - "version": "2026.0.1", - "libName": "REVLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "REVLib-driver", - "version": "2026.0.1", - "libName": "REVLibDriver", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibBackendDriver", - "version": "2026.0.1", - "libName": "BackendDriver", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.revrobotics.frc", - "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.1", - "libName": "REVLibWpi", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - } - ] -} + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2026.0.2", + "frcYear": "2026", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2026.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2026.0.2" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2026.0.2", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2026.0.2", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibBackendDriver", + "version": "2026.0.2", + "libName": "BackendDriver", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "RevLibWpiBackendDriver", + "version": "2026.0.2", + "libName": "REVLibWpi", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index 16e6991..fbb5c80 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,71 +1,71 @@ { - "fileName": "photonlib.json", - "name": "photonlib", - "version": "v2026.1.1-rc-2", - "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", - "frcYear": "2026", - "mavenUrls": [ - "https://maven.photonvision.org/repository/internal", - "https://maven.photonvision.org/repository/snapshots" - ], - "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", - "jniDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "cppDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-cpp", - "version": "v2026.1.1-rc-2", - "libName": "photonlib", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-cpp", - "version": "v2026.1.1-rc-2", - "libName": "photontargeting", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxathena", - "linuxx86-64", - "osxuniversal" - ] - } - ], - "javaDependencies": [ - { - "groupId": "org.photonvision", - "artifactId": "photonlib-java", - "version": "v2026.1.1-rc-2" - }, - { - "groupId": "org.photonvision", - "artifactId": "photontargeting-java", - "version": "v2026.1.1-rc-2" - } - ] -} + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.2.2", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.2.2", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.2.2", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.2.2", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.2.2" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.2.2" + } + ] +} \ No newline at end of file From 12c1fe52fed3e30fe86935c34199c95b5fce0973 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Tue, 17 Feb 2026 18:58:21 -0600 Subject: [PATCH 03/21] 2/17 changes; Carpet actually uppercased in commit. Ids almost everything and in right canbusses --- src/main/java/frc/robot/Constants.java | 3 +- .../frc/robot/generated/TunerConstants.java | 2 +- .../frc/robot/subsystems/climber/Climber.java | 1 + .../subsystems/climber/ClimberIOTalonFX.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../subsystems/drive/ModuleIOTalonFX.java | 6 +- .../drive/PhoenixOdometryThread.java | 2 +- .../subsystems/feeder/FeederIOTalonFX.java | 4 +- .../hopper/{carpet.java => Carpet.java} | 0 .../subsystems/hopper/CarpetIOSparkFlex.java | 6 +- .../subsystems/hopper/CarpetIOTalonFX.java | 4 +- .../intake/pivot/PivotIOTalonFX.java | 4 +- .../intake/roller/RollerIOTalonFX.java | 4 +- .../shooter/angler/AnglerIOTalonFX.java | 4 +- .../shooter/flywheel/FlywheelIOTalonFX.java | 6 +- vendordeps/AdvantageKit.json | 68 +-- vendordeps/Phoenix6-26.1.1.json | 449 ++++++++++++++++++ vendordeps/Phoenix6.json | 449 ------------------ 19 files changed, 512 insertions(+), 508 deletions(-) rename src/main/java/frc/robot/subsystems/hopper/{carpet.java => Carpet.java} (100%) create mode 100644 vendordeps/Phoenix6-26.1.1.json delete mode 100644 vendordeps/Phoenix6.json diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 15aa26a..9a4b404 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -20,7 +20,8 @@ public final class Constants { public static final Mode currentMode = RobotBase.isReal() ? Mode.REAL : simMode; // CAN bus that the devices are located on; - public static final CANBus CANBUS = new CANBus("Canivore", "./logs/example.hoot"); + public static final CANBus CANIVORE_CANBUS = new CANBus("Canivore", "./logs/example.hoot"); + public static final CANBus RIO_CANBUS = new CANBus(); public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 78b7de0..d90dafc 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -85,7 +85,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 49; + private static final int kPigeonId = 30; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index ea6267c..3fbaae6 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,3 +1,4 @@ + package frc.robot.subsystems.climber; import org.littletonrobotics.junction.Logger; diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java index 1dd4b97..a58dd88 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOTalonFX.java @@ -15,11 +15,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class ClimberIOTalonFX implements ClimberIO { - private final TalonFX talon = new TalonFX(ClimberConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(ClimberConstants.id, Constants.CANIVORE_CANBUS); public static class ClimberTalonFXConfig extends CTREMechanism.Config { public ClimberTalonFXConfig() { - super("ClimberTalonFX", Constants.CANBUS); + super("ClimberTalonFX", Constants.CANIVORE_CANBUS); configPIDGains(ClimberConstants.p, ClimberConstants.i, ClimberConstants.d); configNeutralBrakeMode(ClimberConstants.breakType); configFeedbackSensorSource(ClimberConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index ec515c8..3205414 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -50,7 +50,7 @@ public class Drive extends SubsystemBase { // TunerConstants doesn't include these constants, so they are declared locally - static final double ODOMETRY_FREQUENCY = Constants.CANBUS.isNetworkFD() ? 250.0 : 100.0; + static final double ODOMETRY_FREQUENCY = Constants.CANIVORE_CANBUS.isNetworkFD() ? 250.0 : 100.0; public static final double DRIVE_BASE_RADIUS = Math.max( Math.max( diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index e3d3d20..160a02b 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -23,7 +23,7 @@ /** IO implementation for Pigeon 2. */ public class GyroIOPigeon2 implements GyroIO { private final Pigeon2 pigeon = - new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, Constants.CANBUS); + new Pigeon2(TunerConstants.DrivetrainConstants.Pigeon2Id, Constants.CANIVORE_CANBUS); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 8cf20bb..7afb295 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -96,9 +96,9 @@ public ModuleIOTalonFX( SwerveModuleConstants constants) { this.constants = constants; - driveTalon = new TalonFX(constants.DriveMotorId, Constants.CANBUS); - turnTalon = new TalonFX(constants.SteerMotorId, Constants.CANBUS); - cancoder = new CANcoder(constants.EncoderId, Constants.CANBUS); + driveTalon = new TalonFX(constants.DriveMotorId, Constants.CANIVORE_CANBUS); + turnTalon = new TalonFX(constants.SteerMotorId, Constants.CANIVORE_CANBUS); + cancoder = new CANcoder(constants.EncoderId, Constants.CANIVORE_CANBUS); // Configure drive motor var driveConfig = constants.DriveMotorInitialConfigs; diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 5cc1fae..70cc71a 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -37,7 +37,7 @@ public class PhoenixOdometryThread extends Thread { private final List> genericQueues = new ArrayList<>(); private final List> timestampQueues = new ArrayList<>(); - private static boolean isCANFD = Constants.CANBUS.isNetworkFD(); + private static boolean isCANFD = Constants.CANIVORE_CANBUS.isNetworkFD(); private static PhoenixOdometryThread instance = null; public static PhoenixOdometryThread getInstance() { diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java index dc90de4..0b3a682 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOTalonFX.java @@ -14,11 +14,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class FeederIOTalonFX implements FeederIO { - private final TalonFX talon = new TalonFX(FeederConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(FeederConstants.id, Constants.RIO_CANBUS); public static class FeederIOTalonFXConfig extends CTREMechanism.Config { public FeederIOTalonFXConfig() { - super("Feeder", Constants.CANBUS); + super("Feeder", Constants.RIO_CANBUS); configNeutralBrakeMode(FeederConstants.breakType); configStatorCurrentLimit(FeederConstants.stallLimit); diff --git a/src/main/java/frc/robot/subsystems/hopper/carpet.java b/src/main/java/frc/robot/subsystems/hopper/Carpet.java similarity index 100% rename from src/main/java/frc/robot/subsystems/hopper/carpet.java rename to src/main/java/frc/robot/subsystems/hopper/Carpet.java diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java index a6265a1..d47b5a6 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.hopper; +import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; @@ -10,8 +11,9 @@ import frc.robot.subsystems.hopper.CarpetConstants.CarpetRollerConstants; import frc.team5431.titan.core.subsystem.REVMechanism; -public class CarpetIOSparkFlex extends CarpetIOTalonFX { - private final SparkFlex sparkFlex = new SparkFlex(CarpetRollerConstants.id, null); +public class CarpetIOSparkFlex implements CarpetIO { + // Neo Vortex + private final SparkFlex sparkFlex = new SparkFlex(CarpetRollerConstants.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); public static class CarpetIOSparkFlexConfig extends REVMechanism.Config { diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java index 563ba28..95594db 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOTalonFX.java @@ -15,11 +15,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class CarpetIOTalonFX implements CarpetIO { - private final TalonFX talon = new TalonFX(CarpetRollerConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(CarpetRollerConstants.id, Constants.RIO_CANBUS); public static class CarpetIOTalonFXConfig extends CTREMechanism.Config { public CarpetIOTalonFXConfig() { - super("RollerTalonFX",Constants.CANBUS); + super("RollerTalonFX",Constants.RIO_CANBUS); configPIDGains(CarpetRollerConstants.p, CarpetRollerConstants.i, CarpetRollerConstants.d); configNeutralBrakeMode(CarpetRollerConstants.breakType); configFeedbackSensorSource(CarpetRollerConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 92baf0b..1d37025 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -17,11 +17,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class PivotIOTalonFX implements PivotIO { - private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakePivotConstants.id, Constants.RIO_CANBUS); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { - super("PivotTalonFX", Constants.CANBUS); + super("PivotTalonFX", Constants.RIO_CANBUS); configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); configNeutralBrakeMode(IntakePivotConstants.breakType); configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java index 53b06b2..34c10df 100644 --- a/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/roller/RollerIOTalonFX.java @@ -15,11 +15,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class RollerIOTalonFX implements RollerIO { - private final TalonFX talon = new TalonFX(IntakeRollerConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(IntakeRollerConstants.id, Constants.RIO_CANBUS); public static class RollerTalonFXConfig extends CTREMechanism.Config { public RollerTalonFXConfig() { - super("RollerTalonFX",Constants.CANBUS); + super("RollerTalonFX", Constants.RIO_CANBUS); configPIDGains(IntakeRollerConstants.p, IntakeRollerConstants.i, IntakeRollerConstants.d); configNeutralBrakeMode(IntakeRollerConstants.breakType); configFeedbackSensorSource(IntakeRollerConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index f7041a8..71da7e0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -16,11 +16,11 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class AnglerIOTalonFX implements AnglerIO { - private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANBUS); + private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANIVORE_CANBUS); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { - super("AnglerTalonFX", Constants.CANBUS); + super("AnglerTalonFX", Constants.CANIVORE_CANBUS); configPIDGains(ShooterAnglerConstants.p, ShooterAnglerConstants.i, ShooterAnglerConstants.d); configNeutralBrakeMode(ShooterAnglerConstants.breakType); configFeedbackSensorSource(ShooterAnglerConstants.feedbackSensorCTRE); diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index d37be19..dbb2091 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -19,12 +19,12 @@ import frc.team5431.titan.core.subsystem.CTREMechanism; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANBUS); - private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANBUS); + private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANIVORE_CANBUS); + private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANIVORE_CANBUS); public static class FlywheelTalonFXConfig extends CTREMechanism.Config { public FlywheelTalonFXConfig() { - super("FlywheelTalonFX", Constants.CANBUS); + super("FlywheelTalonFX", Constants.CANIVORE_CANBUS); configNeutralBrakeMode(ShooterFlywheelConstants.breakType); configFeedbackSensorSource(ShooterFlywheelConstants.feedbackSensorCTRE); configNeutralBrakeMode(ShooterFlywheelConstants.breakType); diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 2faa4db..162ad66 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,35 +1,35 @@ { - "fileName": "AdvantageKit.json", - "name": "AdvantageKit", - "version": "26.0.0", - "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", - "frcYear": "2026", - "mavenUrls": [ - "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" - ], - "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", - "javaDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-java", - "version": "26.0.0" - } - ], - "jniDependencies": [ - { - "groupId": "org.littletonrobotics.akit", - "artifactId": "akit-wpilibio", - "version": "26.0.0", - "skipInvalidPlatforms": false, - "isJar": false, - "validPlatforms": [ - "linuxathena", - "linuxx86-64", - "linuxarm64", - "osxuniversal", - "windowsx86-64" - ] - } - ], - "cppDependencies": [] -} + "fileName": "AdvantageKit.json", + "name": "AdvantageKit", + "version": "26.0.0", + "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", + "frcYear": "2026", + "mavenUrls": [ + "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/" + ], + "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json", + "javaDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-java", + "version": "26.0.0" + } + ], + "jniDependencies": [ + { + "groupId": "org.littletonrobotics.akit", + "artifactId": "akit-wpilibio", + "version": "26.0.0", + "skipInvalidPlatforms": false, + "isJar": false, + "validPlatforms": [ + "linuxathena", + "linuxx86-64", + "linuxarm64", + "osxuniversal", + "windowsx86-64" + ] + } + ], + "cppDependencies": [] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6-26.1.1.json b/vendordeps/Phoenix6-26.1.1.json new file mode 100644 index 0000000..7a0eca0 --- /dev/null +++ b/vendordeps/Phoenix6-26.1.1.json @@ -0,0 +1,449 @@ +{ + "fileName": "Phoenix6-26.1.1.json", + "name": "CTRE-Phoenix (v6)", + "version": "26.1.1", + "frcYear": "2026", + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", + "offlineFileName": "Phoenix6-replay-frc2026-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-java", + "version": "26.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "api-cpp", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "api-cpp-sim", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix6", + "artifactId": "wpiapi-cpp", + "version": "26.1.1", + "libName": "CTRE_Phoenix6_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6", + "artifactId": "tools", + "version": "26.1.1", + "libName": "CTRE_PhoenixTools", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "26.1.1", + "libName": "CTRE_Phoenix6_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "tools-sim", + "version": "26.1.1", + "libName": "CTRE_PhoenixTools_Sim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simTalonSRX", + "version": "26.1.1", + "libName": "CTRE_SimTalonSRX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simVictorSPX", + "version": "26.1.1", + "libName": "CTRE_SimVictorSPX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simPigeonIMU", + "version": "26.1.1", + "libName": "CTRE_SimPigeonIMU", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFX", + "version": "26.1.1", + "libName": "CTRE_SimProTalonFX", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProTalonFXS", + "version": "26.1.1", + "libName": "CTRE_SimProTalonFXS", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANcoder", + "version": "26.1.1", + "libName": "CTRE_SimProCANcoder", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProPigeon2", + "version": "26.1.1", + "libName": "CTRE_SimProPigeon2", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANrange", + "version": "26.1.1", + "libName": "CTRE_SimProCANrange", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdi", + "version": "26.1.1", + "libName": "CTRE_SimProCANdi", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix6.sim", + "artifactId": "simProCANdle", + "version": "26.1.1", + "libName": "CTRE_SimProCANdle", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} \ No newline at end of file diff --git a/vendordeps/Phoenix6.json b/vendordeps/Phoenix6.json deleted file mode 100644 index 5d2d04d..0000000 --- a/vendordeps/Phoenix6.json +++ /dev/null @@ -1,449 +0,0 @@ -{ - "fileName": "Phoenix6-26.1.0.json", - "name": "CTRE-Phoenix (v6)", - "version": "26.1.0", - "frcYear": "2026", - "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", - "mavenUrls": [ - "https://maven.ctr-electronics.com/release/" - ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2026-latest.json", - "conflictsWith": [ - { - "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", - "errorMessage": "Users can not have both the replay and regular Phoenix 6 vendordeps in their robot program.", - "offlineFileName": "Phoenix6-replay-frc2026-latest.json" - } - ], - "javaDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-java", - "version": "26.1.0" - } - ], - "jniDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "api-cpp", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "api-cpp-sim", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "26.1.0", - "isJar": false, - "skipInvalidPlatforms": true, - "validPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ], - "cppDependencies": [ - { - "groupId": "com.ctre.phoenix6", - "artifactId": "wpiapi-cpp", - "version": "26.1.0", - "libName": "CTRE_Phoenix6_WPI", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6", - "artifactId": "tools", - "version": "26.1.0", - "libName": "CTRE_PhoenixTools", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "linuxathena" - ], - "simMode": "hwsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "wpiapi-cpp-sim", - "version": "26.1.0", - "libName": "CTRE_Phoenix6_WPISim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "tools-sim", - "version": "26.1.0", - "libName": "CTRE_PhoenixTools_Sim", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simTalonSRX", - "version": "26.1.0", - "libName": "CTRE_SimTalonSRX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simVictorSPX", - "version": "26.1.0", - "libName": "CTRE_SimVictorSPX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simPigeonIMU", - "version": "26.1.0", - "libName": "CTRE_SimPigeonIMU", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFX", - "version": "26.1.0", - "libName": "CTRE_SimProTalonFX", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProTalonFXS", - "version": "26.1.0", - "libName": "CTRE_SimProTalonFXS", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANcoder", - "version": "26.1.0", - "libName": "CTRE_SimProCANcoder", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProPigeon2", - "version": "26.1.0", - "libName": "CTRE_SimProPigeon2", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANrange", - "version": "26.1.0", - "libName": "CTRE_SimProCANrange", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdi", - "version": "26.1.0", - "libName": "CTRE_SimProCANdi", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - }, - { - "groupId": "com.ctre.phoenix6.sim", - "artifactId": "simProCANdle", - "version": "26.1.0", - "libName": "CTRE_SimProCANdle", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "linuxarm64", - "osxuniversal" - ], - "simMode": "swsim" - } - ] -} From 4004cef7e639f836bd47a015a5889e32da8d817c Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Tue, 17 Feb 2026 20:13:56 -0600 Subject: [PATCH 04/21] FIX RPM WHY NO WORK? rn its hardcoded voltage. --- elastic-layout.json | 3 +++ src/main/java/frc/robot/RobotContainer.java | 9 +++++++-- .../java/frc/robot/subsystems/feeder/Feeder.java | 2 +- .../robot/subsystems/feeder/FeederConstants.java | 16 ++++++++-------- .../robot/subsystems/intake/IntakeConstants.java | 4 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 7 +++++++ .../subsystems/shooter/ShooterConstants.java | 4 ++-- .../shooter/flywheel/FlywheelIOTalonFX.java | 11 ++++++++--- 8 files changed, 38 insertions(+), 18 deletions(-) create mode 100644 elastic-layout.json diff --git a/elastic-layout.json b/elastic-layout.json new file mode 100644 index 0000000..544b7b4 --- /dev/null +++ b/elastic-layout.json @@ -0,0 +1,3 @@ +{ + +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22ad2bb..183e654 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,6 +35,7 @@ import frc.robot.subsystems.feeder.FeederIO; import frc.robot.subsystems.feeder.FeederIOSim; import frc.robot.subsystems.feeder.FeederIOSparkFlex; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; import frc.robot.subsystems.hopper.Carpet; import frc.robot.subsystems.hopper.CarpetIO; import frc.robot.subsystems.hopper.CarpetIOSim; @@ -106,8 +107,8 @@ public RobotContainer() { // new VisionIOLimelight(camera0Name, drive::getRotation), // new VisionIOLimelight(camera1Name, drive::getRotation)); - intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); - shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); + intake = new Intake(new RollerIOSparkFlex(), new PivotIOSim()); + shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); feeder = new Feeder(new FeederIOSparkFlex()); @@ -225,6 +226,9 @@ private void configureDriverBindings() { intake.setDefaultCommand(intake.stop()); + carpet.setDefaultCommand(carpet.runCarpetCommand(CarpetModes.IDLE)); + feeder.setDefaultCommand(feeder.runFeederCommand(FeederModes.IDLE)); + shooter.setDefaultCommand(shooter.stop()); // shooter.setDefaultCommand(); // // Lock to 0° when A button is held // driver @@ -254,6 +258,7 @@ private void configureDriverBindings() { driver.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); + driver.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); // // Auto aim command example; code from AKit template. diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 43d6c7f..9a079ac 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -56,7 +56,7 @@ public void periodic() { public void runFeederEnum(FeederModes feederMode) { this.feederMode = feederMode; - feederIO.setPercentOutput(feederMode.output); + feederIO.setFeederVoltage(feederMode.output); } public Command runFeederCommand(FeederModes feederMode) { diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 4f23545..cfb8864 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -30,19 +30,19 @@ public enum FeederState { public static final double maxForwardOutput = 0.5; public static final double maxReverseOutput = -0.5; - public static final double FeederSpeed = 0.5; - public static final double reverseSpeed = -0.5; - public static final double idleSpeed = 0.0; + // public static final double FeederSpeed = 0.5; + // public static final double reverseSpeed = -0.5; + // public static final double idleSpeed = 0.0; public enum FeederModes { - FEEDER(FeederSpeed), - REVERSE(reverseSpeed), - IDLE(idleSpeed); + FEEDER(8), + REVERSE(-6), + IDLE(0); public double output; - FeederModes(double output) { - this.output = output; + FeederModes(double voltage) { + this.output = voltage; } } } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index f057c6a..df5da4b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -15,8 +15,8 @@ public enum IntakeMode { STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), - INTAKE(Units.Volts.of(5.4), Units.Degrees.of(0.0)), - OUTTAKE(Units.Volts.of(-2.8), Units.Degrees.of(180.0)); + INTAKE(Units.Volts.of(-5.4), Units.Degrees.of(0.0)), + OUTTAKE(Units.Volts.of(2.8), Units.Degrees.of(180.0)); public Voltage voltage; public Angle position; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 7c36313..24fcc27 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -48,4 +48,11 @@ public Command runShooterCommand(ShooterModes mode) { this.runShooterEnum(mode); }, this).withName("Shooter.runShooterEnum" + mode.toString()); } + + public Command stop() { + return run(() -> { + flywheelIO.setRPM(0); + }).withName("Intake.Stop"); + + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index c36907e..3ada159 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -10,8 +10,8 @@ public class ShooterConstants { public enum ShooterModes { - SHOOT_FAR( Units.RPM.of(200), Units.Degree.of(30)), - SHOOT_CLOSE(Units.RPM.of(150), Units.Degree.of(60)), + SHOOT_FAR( Units.RPM.of(500), Units.Degree.of(30)), + SHOOT_CLOSE(Units.RPM.of(400), Units.Degree.of(60)), IDLE( Units.RPM.of(0), Units.Degree.of(0.0)); public AngularVelocity speed; diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index dbb2091..4694fd0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -61,7 +61,7 @@ public FlywheelIOTalonFX() { config.applyTalonConfig(follower); // will need to config whether aligned or inverted later - follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Aligned)); + follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Opposed)); BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); } @@ -83,7 +83,12 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setRPM(double rpm) { - VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - leader.setControl(output); + // VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); + // leader.setControl(output); + // FIX RPM WHY NO WORK? rn its hardcoded voltage + if (rpm <= 0) { + leader.setVoltage(0); + } + leader.setVoltage(5); } } From f7a83eba473d51604b3c55eb559fa0a948fa4bf6 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Thu, 19 Feb 2026 19:51:49 -0600 Subject: [PATCH 05/21] Swerve working. Shooting need help --- build.gradle | 2 +- src/main/java/frc/robot/RobotContainer.java | 3 +- .../frc/robot/generated/TunerConstants.java | 319 +--------- .../robot/generated/TunerConstantsTrash.java | 570 ++++++++++++++++++ .../subsystems/climber/ClimberIOSim.java | 1 + .../frc/robot/subsystems/feeder/Feeder.java | 28 +- .../subsystems/feeder/FeederConstants.java | 8 +- .../frc/robot/subsystems/shooter/Shooter.java | 11 +- .../subsystems/shooter/ShooterConstants.java | 2 +- .../shooter/flywheel/FlywheelIOTalonFX.java | 19 +- 10 files changed, 618 insertions(+), 345 deletions(-) create mode 100644 src/main/java/frc/robot/generated/TunerConstantsTrash.java diff --git a/build.gradle b/build.gradle index 127d21b..65e0cfe 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2026.1.1" + id "edu.wpi.first.GradleRIO" version "2026.2.1" id "com.peterabeles.gversion" version "1.10" id "com.diffplug.spotless" version "6.12.0" } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 183e654..05d2fea 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -235,7 +235,8 @@ private void configureDriverBindings() { // .a() // .whileTrue( // DriveCommands.joystickDriveAtAngle( - // drive, + // drive,\[] + // () -> -driver.getLeftY(), // () -> -driver.getLeftX(), // () -> Rotation2d.kZero)); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index d90dafc..949bf95 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.units.measure.*; + // Generated by the 2026 Tuner X Swerve Project Generator // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html public class TunerConstants { @@ -127,9 +128,9 @@ public class TunerConstants { private static final int kFrontLeftDriveMotorId = 1; private static final int kFrontLeftSteerMotorId = 2; private static final int kFrontLeftEncoderId = 9; - private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.298095703125); - private static final boolean kFrontLeftSteerMotorInverted = false; - private static final boolean kFrontLeftEncoderInverted = true; + private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.30029296875); + private static final boolean kFrontLeftSteerMotorInverted = true; + private static final boolean kFrontLeftEncoderInverted = false; private static final Distance kFrontLeftXPos = Inches.of(11); private static final Distance kFrontLeftYPos = Inches.of(11.5); @@ -138,9 +139,9 @@ public class TunerConstants { private static final int kFrontRightDriveMotorId = 4; private static final int kFrontRightSteerMotorId = 3; private static final int kFrontRightEncoderId = 10; - private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.320556640625); - private static final boolean kFrontRightSteerMotorInverted = false; - private static final boolean kFrontRightEncoderInverted = true; + private static final Angle kFrontRightEncoderOffset = Rotations.of(0.320068359375); + private static final boolean kFrontRightSteerMotorInverted = true; + private static final boolean kFrontRightEncoderInverted = false; private static final Distance kFrontRightXPos = Inches.of(11); private static final Distance kFrontRightYPos = Inches.of(-11.5); @@ -149,9 +150,9 @@ public class TunerConstants { private static final int kBackLeftDriveMotorId = 7; private static final int kBackLeftSteerMotorId = 8; private static final int kBackLeftEncoderId = 12; - private static final Angle kBackLeftEncoderOffset = Rotations.of(0.18017578125); - private static final boolean kBackLeftSteerMotorInverted = false; - private static final boolean kBackLeftEncoderInverted = true; + private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181884765625); + private static final boolean kBackLeftSteerMotorInverted = true; + private static final boolean kBackLeftEncoderInverted = false; private static final Distance kBackLeftXPos = Inches.of(-11); private static final Distance kBackLeftYPos = Inches.of(11.5); @@ -160,9 +161,9 @@ public class TunerConstants { private static final int kBackRightDriveMotorId = 5; private static final int kBackRightSteerMotorId = 6; private static final int kBackRightEncoderId = 11; - private static final Angle kBackRightEncoderOffset = Rotations.of(0.4580078125); - private static final boolean kBackRightSteerMotorInverted = false; - private static final boolean kBackRightEncoderInverted = true; + private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4580078125); + private static final boolean kBackRightSteerMotorInverted = true; + private static final boolean kBackRightEncoderInverted = false; private static final Distance kBackRightXPos = Inches.of(-11); private static final Distance kBackRightYPos = Inches.of(-11.5); @@ -189,10 +190,10 @@ public class TunerConstants { kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted ); - // /** - // * Creates a CommandSwerveDrivetrain instance. - // * This should only be called once in your robot program,. - // */ + /** + * Creates a CommandSwerveDrivetrain instance. + * This should only be called once in your robot program,. + */ // public static CommandSwerveDrivetrain createDrivetrain() { // return new CommandSwerveDrivetrain( // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight @@ -282,289 +283,3 @@ public TunerSwerveDrivetrain( } } } - -// package frc.robot.generated; - -// import static edu.wpi.first.units.Units.*; - -// import com.ctre.phoenix6.CANBus; -// import com.ctre.phoenix6.configs.*; -// import com.ctre.phoenix6.hardware.*; -// import com.ctre.phoenix6.signals.*; -// import com.ctre.phoenix6.swerve.*; -// import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; - -// import edu.wpi.first.math.Matrix; -// import edu.wpi.first.math.numbers.N1; -// import edu.wpi.first.math.numbers.N3; -// import edu.wpi.first.units.measure.*; - -// // Generated by the 2026 Tuner X Swerve Project Generator -// // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html -// public class TunerConstants { -// // Both sets of gains need to be tuned to your individual robot. - -// // The steer motor uses any SwerveModule.SteerRequestType control request with the -// // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput -// private static final Slot0Configs steerGains = new Slot0Configs() -// .withKP(100).withKI(0).withKD(0.5) -// .withKS(0.1).withKV(2.33).withKA(0) -// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); -// // When using closed-loop control, the drive motor uses the control -// // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput -// private static final Slot0Configs driveGains = new Slot0Configs() -// .withKP(0.1).withKI(0).withKD(0) -// .withKS(0).withKV(0.124); - -// // The closed-loop output type to use for the steer motors; -// // This affects the PID/FF gains for the steer motors -// private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; -// // The closed-loop output type to use for the drive motors; -// // This affects the PID/FF gains for the drive motors -// private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; - -// // The type of motor used for the drive motor -// private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; -// // The type of motor used for the drive motor -// private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; - -// // The remote sensor feedback type to use for the steer motors; -// // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* -// private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; - -// // The stator current at which the wheels start to slip; -// // This needs to be tuned to your individual robot -// private static final Current kSlipCurrent = Amps.of(120); - -// // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. -// // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. -// private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); -// private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() -// .withCurrentLimits( -// new CurrentLimitsConfigs() -// // Swerve azimuth does not require much torque output, so we can set a relatively low -// // stator current limit to help avoid brownouts without impacting performance. -// .withStatorCurrentLimit(Amps.of(60)) -// .withStatorCurrentLimitEnable(true) -// ); -// private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); -// // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs -// private static final Pigeon2Configuration pigeonConfigs = null; - -// // CAN bus that the devices are located on; -// // All swerve devices must share the same CAN bus -// public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); - -// // Theoretical free speed (m/s) at 12 V applied output; -// // This needs to be tuned to your individual robot -// public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); - -// // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; -// // This may need to be tuned to your individual robot -// private static final double kCoupleRatio = 3.125; - -// private static final double kDriveGearRatio = 5.902777777777778; -// private static final double kSteerGearRatio = 18.75; -// private static final Distance kWheelRadius = Inches.of(2); - -// private static final boolean kInvertLeftSide = false; -// private static final boolean kInvertRightSide = true; - -// private static final int kPigeonId = 49; - -// // These are only used for simulation -// private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); -// private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); -// // Simulated voltage necessary to overcome friction -// private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); -// private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); - -// public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() -// .withCANBusName(kCANBus.getName()) -// .withPigeon2Id(kPigeonId) -// .withPigeon2Configs(pigeonConfigs); - -// private static final SwerveModuleConstantsFactory ConstantCreator = -// new SwerveModuleConstantsFactory() -// .withDriveMotorGearRatio(kDriveGearRatio) -// .withSteerMotorGearRatio(kSteerGearRatio) -// .withCouplingGearRatio(kCoupleRatio) -// .withWheelRadius(kWheelRadius) -// .withSteerMotorGains(steerGains) -// .withDriveMotorGains(driveGains) -// .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) -// .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) -// .withSlipCurrent(kSlipCurrent) -// .withSpeedAt12Volts(kSpeedAt12Volts) -// .withDriveMotorType(kDriveMotorType) -// .withSteerMotorType(kSteerMotorType) -// .withFeedbackSource(kSteerFeedbackType) -// .withDriveMotorInitialConfigs(driveInitialConfigs) -// .withSteerMotorInitialConfigs(steerInitialConfigs) -// .withEncoderInitialConfigs(encoderInitialConfigs) -// .withSteerInertia(kSteerInertia) -// .withDriveInertia(kDriveInertia) -// .withSteerFrictionVoltage(kSteerFrictionVoltage) -// .withDriveFrictionVoltage(kDriveFrictionVoltage); - - -// // Front Left -// private static final int kFrontLeftDriveMotorId = 1; -// private static final int kFrontLeftSteerMotorId = 2; -// private static final int kFrontLeftEncoderId = 9; -// private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.304199); -// private static final boolean kFrontLeftSteerMotorInverted = false; -// private static final boolean kFrontLeftEncoderInverted = true; - -// private static final Distance kFrontLeftXPos = Inches.of(11); -// private static final Distance kFrontLeftYPos = Inches.of(11.5); - -// // Front Right -// private static final int kFrontRightDriveMotorId = 4; -// private static final int kFrontRightSteerMotorId = 3; -// private static final int kFrontRightEncoderId = 10; -// private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.365234); -// private static final boolean kFrontRightSteerMotorInverted = false; -// private static final boolean kFrontRightEncoderInverted = true; - -// private static final Distance kFrontRightXPos = Inches.of(11); -// private static final Distance kFrontRightYPos = Inches.of(-11.5); - -// // Back Left -// private static final int kBackLeftDriveMotorId = 7; -// private static final int kBackLeftSteerMotorId = 8; -// private static final int kBackLeftEncoderId = 12; -// private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.363037); -// private static final boolean kBackLeftSteerMotorInverted = false; -// private static final boolean kBackLeftEncoderInverted = true; - -// private static final Distance kBackLeftXPos = Inches.of(-11); -// private static final Distance kBackLeftYPos = Inches.of(11.5); - -// // Back Right -// private static final int kBackRightDriveMotorId = 5; -// private static final int kBackRightSteerMotorId = 6; -// private static final int kBackRightEncoderId = 11; -// private static final Angle kBackRightEncoderOffset = Rotations.of(-0.082275); -// private static final boolean kBackRightSteerMotorInverted = false; -// private static final boolean kBackRightEncoderInverted = true; - -// private static final Distance kBackRightXPos = Inches.of(-11); -// private static final Distance kBackRightYPos = Inches.of(-11.5); - - -// public static final SwerveModuleConstants FrontLeft = -// ConstantCreator.createModuleConstants( -// kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, -// kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted -// ); -// public static final SwerveModuleConstants FrontRight = -// ConstantCreator.createModuleConstants( -// kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, -// kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted -// ); -// public static final SwerveModuleConstants BackLeft = -// ConstantCreator.createModuleConstants( -// kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, -// kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted -// ); -// public static final SwerveModuleConstants BackRight = -// ConstantCreator.createModuleConstants( -// kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, -// kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted -// ); - -// // /** -// // * Creates a CommandSwerveDrivetrain instance. -// // * This should only be called once in your robot program,. -// // */ -// // public static CommandSwerveDrivetrain createDrivetrain() { -// // return new CommandSwerveDrivetrain( -// // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight -// // ); -// // } - - -// /** -// * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. -// */ -// public static class TunerSwerveDrivetrain extends SwerveDrivetrain { -// /** -// * Constructs a CTRE SwerveDrivetrain using the specified constants. -// *

-// * This constructs the underlying hardware devices, so users should not construct -// * the devices themselves. If they need the devices, they can access them through -// * getters in the classes. -// * -// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive -// * @param modules Constants for each specific module -// */ -// public TunerSwerveDrivetrain( -// SwerveDrivetrainConstants drivetrainConstants, -// SwerveModuleConstants... modules -// ) { -// super( -// TalonFX::new, TalonFX::new, CANcoder::new, -// drivetrainConstants, modules -// ); -// } - -// /** -// * Constructs a CTRE SwerveDrivetrain using the specified constants. -// *

-// * This constructs the underlying hardware devices, so users should not construct -// * the devices themselves. If they need the devices, they can access them through -// * getters in the classes. -// * -// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive -// * @param odometryUpdateFrequency The frequency to run the odometry loop. If -// * unspecified or set to 0 Hz, this is 250 Hz on -// * CAN FD, and 100 Hz on CAN 2.0. -// * @param modules Constants for each specific module -// */ -// public TunerSwerveDrivetrain( -// SwerveDrivetrainConstants drivetrainConstants, -// double odometryUpdateFrequency, -// SwerveModuleConstants... modules -// ) { -// super( -// TalonFX::new, TalonFX::new, CANcoder::new, -// drivetrainConstants, odometryUpdateFrequency, modules -// ); -// } - -// /** -// * Constructs a CTRE SwerveDrivetrain using the specified constants. -// *

-// * This constructs the underlying hardware devices, so users should not construct -// * the devices themselves. If they need the devices, they can access them through -// * getters in the classes. -// * -// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive -// * @param odometryUpdateFrequency The frequency to run the odometry loop. If -// * unspecified or set to 0 Hz, this is 250 Hz on -// * CAN FD, and 100 Hz on CAN 2.0. -// * @param odometryStandardDeviation The standard deviation for odometry calculation -// * in the form [x, y, theta]áµ€, with units in meters -// * and radians -// * @param visionStandardDeviation The standard deviation for vision calculation -// * in the form [x, y, theta]áµ€, with units in meters -// * and radians -// * @param modules Constants for each specific module -// */ -// public TunerSwerveDrivetrain( -// SwerveDrivetrainConstants drivetrainConstants, -// double odometryUpdateFrequency, -// Matrix odometryStandardDeviation, -// Matrix visionStandardDeviation, -// SwerveModuleConstants... modules -// ) { -// super( -// TalonFX::new, TalonFX::new, CANcoder::new, -// drivetrainConstants, odometryUpdateFrequency, -// odometryStandardDeviation, visionStandardDeviation, modules -// ); -// } -// } -// } - diff --git a/src/main/java/frc/robot/generated/TunerConstantsTrash.java b/src/main/java/frc/robot/generated/TunerConstantsTrash.java new file mode 100644 index 0000000..12f568d --- /dev/null +++ b/src/main/java/frc/robot/generated/TunerConstantsTrash.java @@ -0,0 +1,570 @@ +// package frc.robot.generated; + +// import static edu.wpi.first.units.Units.*; + +// import com.ctre.phoenix6.CANBus; +// import com.ctre.phoenix6.configs.*; +// import com.ctre.phoenix6.hardware.*; +// import com.ctre.phoenix6.signals.*; +// import com.ctre.phoenix6.swerve.*; +// import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +// import edu.wpi.first.math.Matrix; +// import edu.wpi.first.math.numbers.N1; +// import edu.wpi.first.math.numbers.N3; +// import edu.wpi.first.units.measure.*; + +// // Generated by the 2026 Tuner X Swerve Project Generator +// // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +// public class TunerConstantsTrash { +// // Both sets of gains need to be tuned to your individual robot. + +// // The steer motor uses any SwerveModule.SteerRequestType control request with the +// // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput +// private static final Slot0Configs steerGains = new Slot0Configs() +// .withKP(100).withKI(0).withKD(0.5) +// .withKS(0.1).withKV(2.33).withKA(0) +// .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); +// // When using closed-loop control, the drive motor uses the control +// // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput +// private static final Slot0Configs driveGains = new Slot0Configs() +// .withKP(0.1).withKI(0).withKD(0) +// .withKS(0).withKV(0.124); + +// // The closed-loop output type to use for the steer motors; +// // This affects the PID/FF gains for the steer motors +// private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; +// // The closed-loop output type to use for the drive motors; +// // This affects the PID/FF gains for the drive motors +// private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + +// // The type of motor used for the drive motor +// private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; +// // The type of motor used for the drive motor +// private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + +// // The remote sensor feedback type to use for the steer motors; +// // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* +// private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + +// // The stator current at which the wheels start to slip; +// // This needs to be tuned to your individual robot +// private static final Current kSlipCurrent = Amps.of(120); + +// // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. +// // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. +// private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); +// private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() +// .withCurrentLimits( +// new CurrentLimitsConfigs() +// // Swerve azimuth does not require much torque output, so we can set a relatively low +// // stator current limit to help avoid brownouts without impacting performance. +// .withStatorCurrentLimit(Amps.of(60)) +// .withStatorCurrentLimitEnable(true) +// ); +// private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); +// // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs +// private static final Pigeon2Configuration pigeonConfigs = null; + +// // CAN bus that the devices are located on; +// // All swerve devices must share the same CAN bus +// public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + +// // Theoretical free speed (m/s) at 12 V applied output; +// // This needs to be tuned to your individual robot +// public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); + +// // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; +// // This may need to be tuned to your individual robot +// private static final double kCoupleRatio = 3.125; + +// private static final double kDriveGearRatio = 5.902777777777778; +// private static final double kSteerGearRatio = 18.75; +// private static final Distance kWheelRadius = Inches.of(2); + +// private static final boolean kInvertLeftSide = false; +// private static final boolean kInvertRightSide = true; + +// private static final int kPigeonId = 30; + +// // These are only used for simulation +// private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); +// private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); +// // Simulated voltage necessary to overcome friction +// private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); +// private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + +// public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() +// .withCANBusName(kCANBus.getName()) +// .withPigeon2Id(kPigeonId) +// .withPigeon2Configs(pigeonConfigs); + +// private static final SwerveModuleConstantsFactory ConstantCreator = +// new SwerveModuleConstantsFactory() +// .withDriveMotorGearRatio(kDriveGearRatio) +// .withSteerMotorGearRatio(kSteerGearRatio) +// .withCouplingGearRatio(kCoupleRatio) +// .withWheelRadius(kWheelRadius) +// .withSteerMotorGains(steerGains) +// .withDriveMotorGains(driveGains) +// .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) +// .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) +// .withSlipCurrent(kSlipCurrent) +// .withSpeedAt12Volts(kSpeedAt12Volts) +// .withDriveMotorType(kDriveMotorType) +// .withSteerMotorType(kSteerMotorType) +// .withFeedbackSource(kSteerFeedbackType) +// .withDriveMotorInitialConfigs(driveInitialConfigs) +// .withSteerMotorInitialConfigs(steerInitialConfigs) +// .withEncoderInitialConfigs(encoderInitialConfigs) +// .withSteerInertia(kSteerInertia) +// .withDriveInertia(kDriveInertia) +// .withSteerFrictionVoltage(kSteerFrictionVoltage) +// .withDriveFrictionVoltage(kDriveFrictionVoltage); + + +// // Front Left +// private static final int kFrontLeftDriveMotorId = 1; +// private static final int kFrontLeftSteerMotorId = 2; +// private static final int kFrontLeftEncoderId = 9; +// private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.298095703125); +// private static final boolean kFrontLeftSteerMotorInverted = false; +// private static final boolean kFrontLeftEncoderInverted = true; + +// private static final Distance kFrontLeftXPos = Inches.of(11); +// private static final Distance kFrontLeftYPos = Inches.of(11.5); + +// // Front Right +// private static final int kFrontRightDriveMotorId = 4; +// private static final int kFrontRightSteerMotorId = 3; +// private static final int kFrontRightEncoderId = 10; +// private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.320556640625); +// private static final boolean kFrontRightSteerMotorInverted = false; +// private static final boolean kFrontRightEncoderInverted = true; + +// private static final Distance kFrontRightXPos = Inches.of(11); +// private static final Distance kFrontRightYPos = Inches.of(-11.5); + +// // Back Left +// private static final int kBackLeftDriveMotorId = 7; +// private static final int kBackLeftSteerMotorId = 8; +// private static final int kBackLeftEncoderId = 12; +// private static final Angle kBackLeftEncoderOffset = Rotations.of(0.18017578125); +// private static final boolean kBackLeftSteerMotorInverted = false; +// private static final boolean kBackLeftEncoderInverted = true; + +// private static final Distance kBackLeftXPos = Inches.of(-11); +// private static final Distance kBackLeftYPos = Inches.of(11.5); + +// // Back Right +// private static final int kBackRightDriveMotorId = 5; +// private static final int kBackRightSteerMotorId = 6; +// private static final int kBackRightEncoderId = 11; +// private static final Angle kBackRightEncoderOffset = Rotations.of(0.4580078125); +// private static final boolean kBackRightSteerMotorInverted = false; +// private static final boolean kBackRightEncoderInverted = true; + +// private static final Distance kBackRightXPos = Inches.of(-11); +// private static final Distance kBackRightYPos = Inches.of(-11.5); + + +// public static final SwerveModuleConstants FrontLeft = +// ConstantCreator.createModuleConstants( +// kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, +// kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted +// ); +// public static final SwerveModuleConstants FrontRight = +// ConstantCreator.createModuleConstants( +// kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, +// kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted +// ); +// public static final SwerveModuleConstants BackLeft = +// ConstantCreator.createModuleConstants( +// kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, +// kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted +// ); +// public static final SwerveModuleConstants BackRight = +// ConstantCreator.createModuleConstants( +// kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, +// kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted +// ); + +// // /** +// // * Creates a CommandSwerveDrivetrain instance. +// // * This should only be called once in your robot program,. +// // */ +// // public static CommandSwerveDrivetrain createDrivetrain() { +// // return new CommandSwerveDrivetrain( +// // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight +// // ); +// // } + + +// /** +// * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. +// */ +// public static class TunerSwerveDrivetrain extends SwerveDrivetrain { +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, modules +// ); +// } + +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// * unspecified or set to 0 Hz, this is 250 Hz on +// * CAN FD, and 100 Hz on CAN 2.0. +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// double odometryUpdateFrequency, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, odometryUpdateFrequency, modules +// ); +// } + +// /** +// * Constructs a CTRE SwerveDrivetrain using the specified constants. +// *

+// * This constructs the underlying hardware devices, so users should not construct +// * the devices themselves. If they need the devices, they can access them through +// * getters in the classes. +// * +// * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// * unspecified or set to 0 Hz, this is 250 Hz on +// * CAN FD, and 100 Hz on CAN 2.0. +// * @param odometryStandardDeviation The standard deviation for odometry calculation +// * in the form [x, y, theta]ᵀ, with units in meters +// * and radians +// * @param visionStandardDeviation The standard deviation for vision calculation +// * in the form [x, y, theta]ᵀ, with units in meters +// * and radians +// * @param modules Constants for each specific module +// */ +// public TunerSwerveDrivetrain( +// SwerveDrivetrainConstants drivetrainConstants, +// double odometryUpdateFrequency, +// Matrix odometryStandardDeviation, +// Matrix visionStandardDeviation, +// SwerveModuleConstants... modules +// ) { +// super( +// TalonFX::new, TalonFX::new, CANcoder::new, +// drivetrainConstants, odometryUpdateFrequency, +// odometryStandardDeviation, visionStandardDeviation, modules +// ); +// } +// } +// } + +// // package frc.robot.generated; + +// // import static edu.wpi.first.units.Units.*; + +// // import com.ctre.phoenix6.CANBus; +// // import com.ctre.phoenix6.configs.*; +// // import com.ctre.phoenix6.hardware.*; +// // import com.ctre.phoenix6.signals.*; +// // import com.ctre.phoenix6.swerve.*; +// // import com.ctre.phoenix6.swerve.SwerveModuleConstants.*; + +// // import edu.wpi.first.math.Matrix; +// // import edu.wpi.first.math.numbers.N1; +// // import edu.wpi.first.math.numbers.N3; +// // import edu.wpi.first.units.measure.*; + +// // // Generated by the 2026 Tuner X Swerve Project Generator +// // // https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html +// // public class TunerConstants { +// // // Both sets of gains need to be tuned to your individual robot. + +// // // The steer motor uses any SwerveModule.SteerRequestType control request with the +// // // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput +// // private static final Slot0Configs steerGains = new Slot0Configs() +// // .withKP(100).withKI(0).withKD(0.5) +// // .withKS(0.1).withKV(2.33).withKA(0) +// // .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign); +// // // When using closed-loop control, the drive motor uses the control +// // // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput +// // private static final Slot0Configs driveGains = new Slot0Configs() +// // .withKP(0.1).withKI(0).withKD(0) +// // .withKS(0).withKV(0.124); + +// // // The closed-loop output type to use for the steer motors; +// // // This affects the PID/FF gains for the steer motors +// // private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage; +// // // The closed-loop output type to use for the drive motors; +// // // This affects the PID/FF gains for the drive motors +// // private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage; + +// // // The type of motor used for the drive motor +// // private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated; +// // // The type of motor used for the drive motor +// // private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated; + +// // // The remote sensor feedback type to use for the steer motors; +// // // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote* +// // private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder; + +// // // The stator current at which the wheels start to slip; +// // // This needs to be tuned to your individual robot +// // private static final Current kSlipCurrent = Amps.of(120); + +// // // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. +// // // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. +// // private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); +// // private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() +// // .withCurrentLimits( +// // new CurrentLimitsConfigs() +// // // Swerve azimuth does not require much torque output, so we can set a relatively low +// // // stator current limit to help avoid brownouts without impacting performance. +// // .withStatorCurrentLimit(Amps.of(60)) +// // .withStatorCurrentLimitEnable(true) +// // ); +// // private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration(); +// // // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs +// // private static final Pigeon2Configuration pigeonConfigs = null; + +// // // CAN bus that the devices are located on; +// // // All swerve devices must share the same CAN bus +// // public static final CANBus kCANBus = new CANBus("Canivore", "./logs/example.hoot"); + +// // // Theoretical free speed (m/s) at 12 V applied output; +// // // This needs to be tuned to your individual robot +// // public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(5.23); + +// // // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns; +// // // This may need to be tuned to your individual robot +// // private static final double kCoupleRatio = 3.125; + +// // private static final double kDriveGearRatio = 5.902777777777778; +// // private static final double kSteerGearRatio = 18.75; +// // private static final Distance kWheelRadius = Inches.of(2); + +// // private static final boolean kInvertLeftSide = false; +// // private static final boolean kInvertRightSide = true; + +// // private static final int kPigeonId = 49; + +// // // These are only used for simulation +// // private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); +// // private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01); +// // // Simulated voltage necessary to overcome friction +// // private static final Voltage kSteerFrictionVoltage = Volts.of(0.2); +// // private static final Voltage kDriveFrictionVoltage = Volts.of(0.2); + +// // public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() +// // .withCANBusName(kCANBus.getName()) +// // .withPigeon2Id(kPigeonId) +// // .withPigeon2Configs(pigeonConfigs); + +// // private static final SwerveModuleConstantsFactory ConstantCreator = +// // new SwerveModuleConstantsFactory() +// // .withDriveMotorGearRatio(kDriveGearRatio) +// // .withSteerMotorGearRatio(kSteerGearRatio) +// // .withCouplingGearRatio(kCoupleRatio) +// // .withWheelRadius(kWheelRadius) +// // .withSteerMotorGains(steerGains) +// // .withDriveMotorGains(driveGains) +// // .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput) +// // .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput) +// // .withSlipCurrent(kSlipCurrent) +// // .withSpeedAt12Volts(kSpeedAt12Volts) +// // .withDriveMotorType(kDriveMotorType) +// // .withSteerMotorType(kSteerMotorType) +// // .withFeedbackSource(kSteerFeedbackType) +// // .withDriveMotorInitialConfigs(driveInitialConfigs) +// // .withSteerMotorInitialConfigs(steerInitialConfigs) +// // .withEncoderInitialConfigs(encoderInitialConfigs) +// // .withSteerInertia(kSteerInertia) +// // .withDriveInertia(kDriveInertia) +// // .withSteerFrictionVoltage(kSteerFrictionVoltage) +// // .withDriveFrictionVoltage(kDriveFrictionVoltage); + + +// // // Front Left +// // private static final int kFrontLeftDriveMotorId = 1; +// // private static final int kFrontLeftSteerMotorId = 2; +// // private static final int kFrontLeftEncoderId = 9; +// // private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.304199); +// // private static final boolean kFrontLeftSteerMotorInverted = false; +// // private static final boolean kFrontLeftEncoderInverted = true; + +// // private static final Distance kFrontLeftXPos = Inches.of(11); +// // private static final Distance kFrontLeftYPos = Inches.of(11.5); + +// // // Front Right +// // private static final int kFrontRightDriveMotorId = 4; +// // private static final int kFrontRightSteerMotorId = 3; +// // private static final int kFrontRightEncoderId = 10; +// // private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.365234); +// // private static final boolean kFrontRightSteerMotorInverted = false; +// // private static final boolean kFrontRightEncoderInverted = true; + +// // private static final Distance kFrontRightXPos = Inches.of(11); +// // private static final Distance kFrontRightYPos = Inches.of(-11.5); + +// // // Back Left +// // private static final int kBackLeftDriveMotorId = 7; +// // private static final int kBackLeftSteerMotorId = 8; +// // private static final int kBackLeftEncoderId = 12; +// // private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.363037); +// // private static final boolean kBackLeftSteerMotorInverted = false; +// // private static final boolean kBackLeftEncoderInverted = true; + +// // private static final Distance kBackLeftXPos = Inches.of(-11); +// // private static final Distance kBackLeftYPos = Inches.of(11.5); + +// // // Back Right +// // private static final int kBackRightDriveMotorId = 5; +// // private static final int kBackRightSteerMotorId = 6; +// // private static final int kBackRightEncoderId = 11; +// // private static final Angle kBackRightEncoderOffset = Rotations.of(-0.082275); +// // private static final boolean kBackRightSteerMotorInverted = false; +// // private static final boolean kBackRightEncoderInverted = true; + +// // private static final Distance kBackRightXPos = Inches.of(-11); +// // private static final Distance kBackRightYPos = Inches.of(-11.5); + + +// // public static final SwerveModuleConstants FrontLeft = +// // ConstantCreator.createModuleConstants( +// // kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset, +// // kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted +// // ); +// // public static final SwerveModuleConstants FrontRight = +// // ConstantCreator.createModuleConstants( +// // kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset, +// // kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted +// // ); +// // public static final SwerveModuleConstants BackLeft = +// // ConstantCreator.createModuleConstants( +// // kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset, +// // kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted +// // ); +// // public static final SwerveModuleConstants BackRight = +// // ConstantCreator.createModuleConstants( +// // kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset, +// // kBackRightXPos, kBackRightYPos, kInvertRightSide, kBackRightSteerMotorInverted, kBackRightEncoderInverted +// // ); + +// // // /** +// // // * Creates a CommandSwerveDrivetrain instance. +// // // * This should only be called once in your robot program,. +// // // */ +// // // public static CommandSwerveDrivetrain createDrivetrain() { +// // // return new CommandSwerveDrivetrain( +// // // DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight +// // // ); +// // // } + + +// // /** +// // * Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. +// // */ +// // public static class TunerSwerveDrivetrain extends SwerveDrivetrain { +// // /** +// // * Constructs a CTRE SwerveDrivetrain using the specified constants. +// // *

+// // * This constructs the underlying hardware devices, so users should not construct +// // * the devices themselves. If they need the devices, they can access them through +// // * getters in the classes. +// // * +// // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// // * @param modules Constants for each specific module +// // */ +// // public TunerSwerveDrivetrain( +// // SwerveDrivetrainConstants drivetrainConstants, +// // SwerveModuleConstants... modules +// // ) { +// // super( +// // TalonFX::new, TalonFX::new, CANcoder::new, +// // drivetrainConstants, modules +// // ); +// // } + +// // /** +// // * Constructs a CTRE SwerveDrivetrain using the specified constants. +// // *

+// // * This constructs the underlying hardware devices, so users should not construct +// // * the devices themselves. If they need the devices, they can access them through +// // * getters in the classes. +// // * +// // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// // * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// // * unspecified or set to 0 Hz, this is 250 Hz on +// // * CAN FD, and 100 Hz on CAN 2.0. +// // * @param modules Constants for each specific module +// // */ +// // public TunerSwerveDrivetrain( +// // SwerveDrivetrainConstants drivetrainConstants, +// // double odometryUpdateFrequency, +// // SwerveModuleConstants... modules +// // ) { +// // super( +// // TalonFX::new, TalonFX::new, CANcoder::new, +// // drivetrainConstants, odometryUpdateFrequency, modules +// // ); +// // } + +// // /** +// // * Constructs a CTRE SwerveDrivetrain using the specified constants. +// // *

+// // * This constructs the underlying hardware devices, so users should not construct +// // * the devices themselves. If they need the devices, they can access them through +// // * getters in the classes. +// // * +// // * @param drivetrainConstants Drivetrain-wide constants for the swerve drive +// // * @param odometryUpdateFrequency The frequency to run the odometry loop. If +// // * unspecified or set to 0 Hz, this is 250 Hz on +// // * CAN FD, and 100 Hz on CAN 2.0. +// // * @param odometryStandardDeviation The standard deviation for odometry calculation +// // * in the form [x, y, theta]áµ€, with units in meters +// // * and radians +// // * @param visionStandardDeviation The standard deviation for vision calculation +// // * in the form [x, y, theta]áµ€, with units in meters +// // * and radians +// // * @param modules Constants for each specific module +// // */ +// // public TunerSwerveDrivetrain( +// // SwerveDrivetrainConstants drivetrainConstants, +// // double odometryUpdateFrequency, +// // Matrix odometryStandardDeviation, +// // Matrix visionStandardDeviation, +// // SwerveModuleConstants... modules +// // ) { +// // super( +// // TalonFX::new, TalonFX::new, CANcoder::new, +// // drivetrainConstants, odometryUpdateFrequency, +// // odometryStandardDeviation, visionStandardDeviation, modules +// // ); +// // } +// // } +// // } + diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java index 266e503..cee4500 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSim.java @@ -3,3 +3,4 @@ public class ClimberIOSim implements ClimberIO { } + diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 9a079ac..2339f4b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -8,50 +8,26 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.feeder.FeederConstants.FeederModes; import frc.robot.subsystems.feeder.FeederConstants.FeederState; -import frc.robot.subsystems.feeder.FeederIO.FeederIOInputs; import lombok.Getter; import lombok.Setter; public class Feeder extends SubsystemBase { private final FeederIO feederIO; - private final FeederIOInputs feederInputs = new FeederIOInputs(); + private final FeederIOInputsAutoLogged feederInputs = new FeederIOInputsAutoLogged(); @Getter @Setter private FeederModes feederMode; - @Getter @Setter private FeederState feederState; public Feeder(FeederIO feederIO) { this.feederIO = feederIO; this.feederMode = FeederModes.IDLE; - this.feederState = FeederState.IDLE; } @Override public void periodic() { feederIO.updateInputs(feederInputs); - // TODO: Add Logger.processInputs once auto-logged classes are generated - // Logger.processInputs("Feeder", feederInputs); - - SmartDashboard.putString("Feeder Mode", feederMode.toString()); - SmartDashboard.putBoolean("Feeder Connected", feederInputs.feederConnected); - SmartDashboard.putNumber("Feeder Voltage", feederInputs.appliedVoltage); - SmartDashboard.putNumber("Feeder RPM", feederInputs.RPM); - SmartDashboard.putNumber("Feeder Current", feederInputs.currentAmps); - + Logger.processInputs("Feeder", feederInputs); Logger.recordOutput("Feeder/Mode", feederMode); - Logger.recordOutput("Feeder/State", feederState); - - switch (this.feederMode) { - case IDLE: - this.feederState = FeederState.IDLE; - break; - case FEEDER: - this.feederState = FeederState.FEEDER; - break; - case REVERSE: - this.feederState = FeederState.REVERSE; - break; - } } public void runFeederEnum(FeederModes feederMode) { diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index cfb8864..e52b249 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -27,16 +27,16 @@ public enum FeederState { public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); - public static final double maxForwardOutput = 0.5; - public static final double maxReverseOutput = -0.5; + public static final double maxForwardOutput = 1; + public static final double maxReverseOutput = -1; // public static final double FeederSpeed = 0.5; // public static final double reverseSpeed = -0.5; // public static final double idleSpeed = 0.0; public enum FeederModes { - FEEDER(8), - REVERSE(-6), + FEEDER(5), + REVERSE(-5), IDLE(0); public double output; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 24fcc27..6aeb770 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -2,6 +2,7 @@ import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -10,6 +11,7 @@ import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; +import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; public class Shooter extends SubsystemBase { private final AnglerIO anglerIO; @@ -35,6 +37,9 @@ public void periodic() { Logger.processInputs("Shooter/Flywheel", flywheelInputs); Logger.recordOutput("Shooter/Mode", shooterMode); + // System.out.println("***********************"); + // System.out.println(FlywheelIOTalonFX.plotOutput); + // System.out.println("***********************"); } public void runShooterEnum(ShooterModes mode) { @@ -50,9 +55,9 @@ public Command runShooterCommand(ShooterModes mode) { } public Command stop() { - return run(() -> { + return new RunCommand(() -> { flywheelIO.setRPM(0); - }).withName("Intake.Stop"); + }, this).withName("Intake.Stop"); - } + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 3ada159..158914c 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -32,7 +32,7 @@ public static class ShooterFlywheelConstants { public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - public static final double p = 1; + public static final double p = 5; public static final double i = 0; public static final double d = 0; // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 4694fd0..6279a9a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -2,6 +2,8 @@ import static edu.wpi.first.units.Units.*; +import org.ejml.dense.block.VectorOps_DDRB; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.Follower; @@ -41,7 +43,7 @@ public FlywheelTalonFXConfig() { private StatusSignal followerAppliedVoltage; private StatusSignal followerFlywheelRPM; private StatusSignal followerAmps; - + public static VelocityVoltage plotOutput; // No clue stole from ModuleIO private final Debouncer flywheelConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); @@ -83,12 +85,15 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setRPM(double rpm) { - // VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - // leader.setControl(output); + VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); + leader.setControl(new VelocityVoltage(Units.RPM.of(rpm))); + plotOutput = output; // FIX RPM WHY NO WORK? rn its hardcoded voltage - if (rpm <= 0) { - leader.setVoltage(0); - } - leader.setVoltage(5); + // if (rpm == 0 || rpm < 0) { + // leader.setVoltage(0); + // } + // else { + // leader.setVoltage(5); + // } } } From 6cd61843845a8095040165577b330249413e61de Mon Sep 17 00:00:00 2001 From: AlbedoLinguine Date: Sun, 22 Feb 2026 18:42:35 -0600 Subject: [PATCH 06/21] added velocitydutycycle --- .../robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 6279a9a..0833eef 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -7,11 +7,13 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.math.filter.Debouncer; +import edu.wpi.first.units.Unit; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; @@ -86,7 +88,7 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setRPM(double rpm) { VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - leader.setControl(new VelocityVoltage(Units.RPM.of(rpm))); + leader.setControl(new VelocityDutyCycle(Units.RPM.of(rpm))); plotOutput = output; // FIX RPM WHY NO WORK? rn its hardcoded voltage // if (rpm == 0 || rpm < 0) { From ea9cc4f9d77b39c7c1ae773fccc54ee023fa1477 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Mon, 23 Feb 2026 12:15:23 -0600 Subject: [PATCH 07/21] stuff --- .../java/frc/robot/subsystems/feeder/FeederConstants.java | 2 +- .../frc/robot/subsystems/shooter/ShooterConstants.java | 2 +- .../subsystems/shooter/flywheel/FlywheelIOTalonFX.java | 8 ++++---- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index e52b249..141d1dc 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -35,7 +35,7 @@ public enum FeederState { // public static final double idleSpeed = 0.0; public enum FeederModes { - FEEDER(5), + FEEDER(12), REVERSE(-5), IDLE(0); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 158914c..c0931d7 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -32,7 +32,7 @@ public static class ShooterFlywheelConstants { public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - public static final double p = 5; + public static final double p = 3; public static final double i = 0; public static final double d = 0; // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 6279a9a..29f7205 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -85,15 +85,15 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setRPM(double rpm) { - VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - leader.setControl(new VelocityVoltage(Units.RPM.of(rpm))); - plotOutput = output; + VelocityVoltage output = config.velocityControl.withVelocity(Units.RotationsPerSecond.of(rpm / 60)); + leader.setControl(output); + // plotOutput = output; // FIX RPM WHY NO WORK? rn its hardcoded voltage // if (rpm == 0 || rpm < 0) { // leader.setVoltage(0); // } // else { - // leader.setVoltage(5); + // leader.setVoltage(5.5); // } } } From b638be8481778e0027e94ddbf68e87eba94ff829 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Thu, 26 Feb 2026 18:00:31 -0600 Subject: [PATCH 08/21] shooter needs fixing --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../subsystems/feeder/FeederConstants.java | 3 +- .../subsystems/feeder/FeederIOSparkFlex.java | 1 - .../shooter/flywheel/FlywheelIOTalonFX.java | 29 +++++++++++++++++-- vendordeps/AdvantageKit.json | 6 ++-- vendordeps/REVLib.json | 18 ++++++------ 6 files changed, 42 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 05d2fea..3002cf0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -111,7 +111,7 @@ public RobotContainer() { shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); - feeder = new Feeder(new FeederIOSparkFlex()); + feeder = new Feeder(new FeederIOSim()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -260,6 +260,7 @@ private void configureDriverBindings() { driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); driver.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + driver.b().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); // // Auto aim command example; code from AKit template. diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 141d1dc..6f7bb4c 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -25,7 +25,8 @@ public enum FeederState { public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - public static final Current stallLimit = Units.Amps.of(60); + // stall is current at 0 rpm, supply is current at runnign speed. Stator is current throguh "motor widning" whatever that means. + public static final Current stallLimit = Units.Amps.of(90); public static final Current supplyLimit = Units.Amps.of(80); public static final double maxForwardOutput = 1; public static final double maxReverseOutput = -1; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java index c9d6dac..841ab4b 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederIOSparkFlex.java @@ -19,7 +19,6 @@ public FeederSparkFlexConfig() { super("PivotSparkFlex", FeederConstants.id); configPIDGains(FeederConstants.p, FeederConstants.i, FeederConstants.d); configSmartCurrentLimit(FeederConstants.stallLimit, FeederConstants.supplyLimit); - configSmartStallCurrentLimit(FeederConstants.stallLimit); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 0a23f6f..a2e4386 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -18,6 +18,7 @@ import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; @@ -35,6 +36,7 @@ public FlywheelTalonFXConfig() { configPIDGains(ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); configGearRatio(ShooterFlywheelConstants.gearRatio); configMotorInverted(ShooterFlywheelConstants.inverted); + configFeedForwardGains(0.2, 0.12, 0, 0); } } @@ -46,6 +48,7 @@ public FlywheelTalonFXConfig() { private StatusSignal followerFlywheelRPM; private StatusSignal followerAmps; public static VelocityVoltage plotOutput; + public static double plotrps; // No clue stole from ModuleIO private final Debouncer flywheelConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); @@ -61,6 +64,7 @@ public FlywheelIOTalonFX() { followerFlywheelRPM = follower.getVelocity(); followerAmps = follower.getStatorCurrent(); + // config.talonConfig.Slot0.k; config.applyTalonConfig(leader); config.applyTalonConfig(follower); @@ -83,13 +87,32 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.followerAppliedVoltage = followerAppliedVoltage.getValueAsDouble(); inputs.followerRPM = followerFlywheelRPM.getValue().in(RPM); inputs.followerAmps = followerAmps.getValueAsDouble(); + + if (plotrps > 0 && plotOutput.Velocity > 0) { + SmartDashboard.putNumber("FlyhweelRPS", plotrps); + SmartDashboard.putNumber("FlyhweelOutputVelocity", plotOutput.Velocity); + } + } @Override public void setRPM(double rpm) { - VelocityVoltage output = config.velocityControl.withVelocity(Units.RPM.of(rpm)); - leader.setControl(new VelocityDutyCycle(Units.RPM.of(rpm))); - plotOutput = output; + AngularVelocity rps = Units.RotationsPerSecond.of(rpm / 60); + double rps2 = 40; + // VelocityVoltage output = config.velocityControl.withVelocity(rps); + VelocityVoltage velocityOuput = new VelocityVoltage(0).withVelocity(rps2); + leader.setControl(velocityOuput); + + plotOutput = velocityOuput; + plotrps = rps2; + + + + System.out.println("******************"); + System.out.println(rps2); + System.out.println("******************"); + System.out.println(velocityOuput); + System.out.println("******************"); // FIX RPM WHY NO WORK? rn its hardcoded voltage // if (rpm == 0 || rpm < 0) { // leader.setVoltage(0); diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 162ad66..868ae9d 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "26.0.0", + "version": "26.0.1", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2026", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-java", - "version": "26.0.0" + "version": "26.0.1" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit", "artifactId": "akit-wpilibio", - "version": "26.0.0", + "version": "26.0.1", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index d8ba7ce..a5af3e9 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2026.0.2", + "version": "2026.0.3", "frcYear": "2026", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2026.0.2" + "version": "2026.0.3" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.2", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -34,7 +34,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -49,7 +49,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -66,7 +66,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -83,7 +83,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false, @@ -100,7 +100,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "BackendDriver", "sharedLibrary": true, "skipInvalidPlatforms": true, @@ -116,7 +116,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "RevLibWpiBackendDriver", - "version": "2026.0.2", + "version": "2026.0.3", "libName": "REVLibWpi", "sharedLibrary": true, "skipInvalidPlatforms": true, From 3faefd7e5b78e6947b03d9b44216dcb26fe65b95 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Thu, 26 Feb 2026 18:11:03 -0600 Subject: [PATCH 09/21] 213 --- .../subsystems/shooter/flywheel/FlywheelIOTalonFX.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index a2e4386..6f62720 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -36,7 +36,7 @@ public FlywheelTalonFXConfig() { configPIDGains(ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); configGearRatio(ShooterFlywheelConstants.gearRatio); configMotorInverted(ShooterFlywheelConstants.inverted); - configFeedForwardGains(0.2, 0.12, 0, 0); + configFeedForwardGains(0.35, 0.12, 0, 0); } } @@ -98,13 +98,13 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setRPM(double rpm) { AngularVelocity rps = Units.RotationsPerSecond.of(rpm / 60); - double rps2 = 40; + AngularVelocity rps2 = RotationsPerSecond.of(4800 / 60); // VelocityVoltage output = config.velocityControl.withVelocity(rps); - VelocityVoltage velocityOuput = new VelocityVoltage(0).withVelocity(rps2); + VelocityVoltage velocityOuput = new VelocityVoltage(rps2).withSlot(0); leader.setControl(velocityOuput); plotOutput = velocityOuput; - plotrps = rps2; + // plotrps = rps2; From 56d63832b660a17fc6f2b71c74de01406aa63796 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Thu, 26 Feb 2026 19:50:38 -0600 Subject: [PATCH 10/21] uses default wpi pid instead of ctre setControl still needs calabration but deos run. --- src/main/java/frc/robot/RobotContainer.java | 10 +- .../subsystems/shooter/ShooterConstants.java | 27 ++++-- .../shooter/flywheel/FlywheelIOTalonFX.java | 92 +++++++++++++------ 3 files changed, 86 insertions(+), 43 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 3002cf0..a137a1d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -254,11 +254,11 @@ private void configureDriverBindings() { new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); - driver.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in - driver.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); - driver.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); - driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); - driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); + // driver.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in + // driver.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); + // driver.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); + // driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + // driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); driver.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); driver.b().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index c0931d7..4b64406 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.shooter; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import edu.wpi.first.units.Units; @@ -10,9 +12,9 @@ public class ShooterConstants { public enum ShooterModes { - SHOOT_FAR( Units.RPM.of(500), Units.Degree.of(30)), - SHOOT_CLOSE(Units.RPM.of(400), Units.Degree.of(60)), - IDLE( Units.RPM.of(0), Units.Degree.of(0.0)); + SHOOT_FAR(Units.RPM.of(5000), Units.Degree.of(30)), + SHOOT_CLOSE(Units.RPM.of(2500), Units.Degree.of(60)), + IDLE(Units.RPM.of(0), Units.Degree.of(0.0)); public AngularVelocity speed; public Angle angle; @@ -32,9 +34,18 @@ public static class ShooterFlywheelConstants { public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - public static final double p = 3; - public static final double i = 0; - public static final double d = 0; + // public static final double p = 0; + // public static final double i = 0; + // public static final double d = 0; + + public static final LoggedNetworkNumber testp = new LoggedNetworkNumber("Shooter/P", 0); + public static final LoggedNetworkNumber testi = new LoggedNetworkNumber("Shooter/I", 0); + public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("Shooter/D", 0); + public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("Shooter/kS", 0); + public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("Shooter/kV", 0); + + // public static final double kS = 0.5; + // public static final double kV = 0.14; //feedforward // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later @@ -44,7 +55,7 @@ public static class ShooterFlywheelConstants { public static final Current supplyLimit = Units.Amps.of(80); } - + public static class ShooterAnglerConstants { public static final boolean attached = true; public static final int id = 15; @@ -59,7 +70,7 @@ public static class ShooterAnglerConstants { // Add later public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; - + public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); // public static final double maxForwardOutput = 1; diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 6f62720..2ac7e69 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -6,12 +6,14 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.Unit; import edu.wpi.first.units.Units; @@ -20,23 +22,25 @@ import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; +import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; public class FlywheelIOTalonFX implements FlywheelIO { - private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANIVORE_CANBUS); + private final TalonFX follower = new TalonFX(ShooterFlywheelConstants.followerId, Constants.CANIVORE_CANBUS); private final TalonFX leader = new TalonFX(ShooterFlywheelConstants.leaderId, Constants.CANIVORE_CANBUS); + public final PIDController pid = new PIDController(ShooterFlywheelConstants.testp.get(), ShooterFlywheelConstants.testi.get(), ShooterFlywheelConstants.testd.get()); + public static class FlywheelTalonFXConfig extends CTREMechanism.Config { public FlywheelTalonFXConfig() { super("FlywheelTalonFX", Constants.CANIVORE_CANBUS); configNeutralBrakeMode(ShooterFlywheelConstants.breakType); configFeedbackSensorSource(ShooterFlywheelConstants.feedbackSensorCTRE); - configNeutralBrakeMode(ShooterFlywheelConstants.breakType); - configPIDGains(ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); + // configPIDGains(0, ShooterFlywheelConstants.p, ShooterFlywheelConstants.i, ShooterFlywheelConstants.d); configGearRatio(ShooterFlywheelConstants.gearRatio); configMotorInverted(ShooterFlywheelConstants.inverted); - configFeedForwardGains(0.35, 0.12, 0, 0); + configFeedForwardGains(0, 0.35, 0.12, 0, 0); } } @@ -50,33 +54,42 @@ public FlywheelTalonFXConfig() { public static VelocityVoltage plotOutput; public static double plotrps; // No clue stole from ModuleIO - private final Debouncer flywheelConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer flywheelConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); - private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); + private FlywheelTalonFXConfig config = new FlywheelTalonFXConfig(); public FlywheelIOTalonFX() { leaderAppliedVoltage = leader.getMotorVoltage(); leaderFlywheelRPM = leader.getVelocity(); leaderAmps = leader.getStatorCurrent(); - + followerAppliedVoltage = follower.getMotorVoltage(); followerFlywheelRPM = follower.getVelocity(); followerAmps = follower.getStatorCurrent(); + // TalonFXConfiguration config1 = new TalonFXConfiguration(); + // config1. // config.talonConfig.Slot0.k; config.applyTalonConfig(leader); config.applyTalonConfig(follower); - + // will need to config whether aligned or inverted later follower.setControl(new Follower(ShooterFlywheelConstants.leaderId, MotorAlignmentValue.Opposed)); - BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + BaseStatusSignal.setUpdateFrequencyForAll(50, leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, + followerAppliedVoltage, followerAmps, followerFlywheelRPM); } @Override public void updateInputs(FlywheelIOInputs inputs) { - var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); + + pid.setP(ShooterFlywheelConstants.testp.get()); + pid.setI(ShooterFlywheelConstants.testi.get()); + pid.setD(ShooterFlywheelConstants.testd.get()); + + + var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, + followerAppliedVoltage, followerAmps, followerFlywheelRPM); inputs.flywheelConnected = flywheelConnectedDebounce.calculate(flywheelStatus.isOK()); @@ -89,36 +102,55 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.followerAmps = followerAmps.getValueAsDouble(); if (plotrps > 0 && plotOutput.Velocity > 0) { - SmartDashboard.putNumber("FlyhweelRPS", plotrps); - SmartDashboard.putNumber("FlyhweelOutputVelocity", plotOutput.Velocity); + SmartDashboard.putNumber("FlyhweelRPS", plotrps); + SmartDashboard.putNumber("FlyhweelOutputVelocity", plotOutput.Velocity); } - + + SmartDashboard.putNumber("Flywheel RPM", leader.getVelocity().getValue().in(Units.RPM)); + } @Override - public void setRPM(double rpm) { - AngularVelocity rps = Units.RotationsPerSecond.of(rpm / 60); - AngularVelocity rps2 = RotationsPerSecond.of(4800 / 60); - // VelocityVoltage output = config.velocityControl.withVelocity(rps); - VelocityVoltage velocityOuput = new VelocityVoltage(rps2).withSlot(0); - leader.setControl(velocityOuput); - - plotOutput = velocityOuput; - // plotrps = rps2; + public void setRPM(double rpm){ + double rps = rpm / 60.0; + AngularVelocity currentRPS = leader.getVelocity().getValue(); + double pidOutput = pid.calculate(currentRPS.in(Units.RotationsPerSecond), rps); - + double voltage = pidOutput + ShooterFlywheelConstants.testkS.get() + ShooterFlywheelConstants.testkV.get() * rps; + + voltage = Math.max(Math.min(voltage, 12), -12); + + leader.setVoltage(voltage); System.out.println("******************"); - System.out.println(rps2); - System.out.println("******************"); - System.out.println(velocityOuput); + System.out.println(leader.getVelocity().getValue()); System.out.println("******************"); + + } + + public void setRPMbck(double rpm) { + // AngularVelcity rps = Units.RotationsPerSecond.of(rpm / 60); + // AngularVelocity rps2 = RotationsPerSecond.of(4800 / 60); + // // VelocityVoltage output = config.velocityControl.withVelocity(rps); + // VelocityVoltage velocityOuput = new + // VelocityVoltage(rps2).withSlot(0).withFeedForward(5); + // leader.setControl(velocityOuput); + + // plotOutput = velocityOuput; + // plotrps = rps2; + + + // System.out.println("******************"); + // System.out.println(rps2); + // System.out.println("******************"); + // System.out.println(velocityOuput); + // System.out.println("******************"); // FIX RPM WHY NO WORK? rn its hardcoded voltage // if (rpm == 0 || rpm < 0) { // leader.setVoltage(0); + // } else { + // leader.setVoltage(8); // } - // else { - // leader.setVoltage(5.5); - // } + } } From b79eddc40195afbc37e228c83bfb1efe554503c2 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Fri, 27 Feb 2026 19:07:22 -0600 Subject: [PATCH 11/21] added mode for commands logging and setZero for angler. --- .OutlineViewer/outlineviewer.json | 1 + src/main/java/frc/robot/RobotContainer.java | 23 ++-- .../frc/robot/subsystems/climber/Climber.java | 2 +- .../frc/robot/subsystems/feeder/Feeder.java | 2 +- .../frc/robot/subsystems/hopper/Carpet.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 17 ++- .../subsystems/shooter/ShooterConstants.java | 1 + .../subsystems/shooter/angler/AnglerIO.java | 7 +- .../shooter/angler/AnglerIOTalonFX.java | 5 + .../shooter/flywheel/FlywheelIOSim.java | 108 +++++++++++------- 10 files changed, 108 insertions(+), 60 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/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 05d2fea..c578fbb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -79,8 +79,7 @@ public class RobotContainer { private final Feeder feeder; // Controller - private final CommandXboxController driver = new CommandXboxController(0); - private final CommandXboxController operator = new CommandXboxController(1); + private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs private final LoggedDashboardChooser autoChooser; @@ -220,9 +219,9 @@ private void configureDriverBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -driver.getLeftY(), - () -> -driver.getLeftX(), - () -> -driver.getRightX())); + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX())); intake.setDefaultCommand(intake.stop()); @@ -245,7 +244,7 @@ private void configureDriverBindings() { // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed - driver + controller .y() .onTrue( Commands.runOnce( @@ -254,12 +253,12 @@ private void configureDriverBindings() { new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); - driver.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in - driver.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); - driver.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); - driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); - driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); - driver.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in + controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); + controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); + controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); + controller.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); // // Auto aim command example; code from AKit template. diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 3fbaae6..7a5c411 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -29,7 +29,7 @@ public void periodic() { public void runClimberEnum(ClimberModes climberMode) { this.mode = climberMode; - climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); + climberIO.setClimberPosition(mode.positionAngle.baseUnitMagnitude()); } public Command runCarpetCommand(ClimberModes climberModes) { diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 2339f4b..8f170bb 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -36,6 +36,6 @@ public void runFeederEnum(FeederModes feederMode) { } public Command runFeederCommand(FeederModes feederMode) { - return new RunCommand(() -> runFeederEnum(feederMode), this).withName("Feeder.runEnum"); + return new RunCommand(() -> runFeederEnum(feederMode), this).withName("Feeder.runEnum" + feederMode.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/hopper/Carpet.java b/src/main/java/frc/robot/subsystems/hopper/Carpet.java index f147957..8db22af 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Carpet.java +++ b/src/main/java/frc/robot/subsystems/hopper/Carpet.java @@ -35,7 +35,7 @@ public void runRollerEnum(CarpetModes carpetMode) { public Command runCarpetCommand(CarpetModes carpetMode) { return new RunCommand(() -> { this.runRollerEnum(carpetMode); - }, this).withName("Carpet.runCarpetEnum"); + }, this).withName("Carpet.runCarpetEnum" + carpetMode.toString()); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 6aeb770..3c3f4d1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,8 +4,11 @@ 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.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; @@ -60,4 +63,16 @@ public Command stop() { }, this).withName("Intake.Stop"); } -} + + public Command setZero() { + return new SequentialCommandGroup( + new RunCommand(() -> { + anglerIO.setVoltage(-2); + }, this).until( + () -> anglerInputs.currentAmps > ShooterAnglerConstants.homingCurrent.baseUnitMagnitude() + ).withTimeout(2), + new RunCommand( + () -> anglerIO.setPosition(0), this) + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 158914c..222c8bd 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -62,6 +62,7 @@ public static class ShooterAnglerConstants { public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); + public static final Current homingCurrent = Units.Amps.of(50); // public static final double maxForwardOutput = 1; // public static final double maxReverseOutput = 0.5; diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java index d732bc9..713446e 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -15,6 +15,9 @@ public static class AnglerIOInputs { public default void updateInputs(AnglerIOInputs inputs) {} /** Run the motor to the specified position. */ - public default void setPosition(double positionAngle) { - } + public default void setPosition(double positionAngle) {} + + /** Run the motor to the specified voltage. */ + public default void setVoltage(double voltage) {} + } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 71da7e0..e4ac499 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -67,4 +67,9 @@ public void setPosition(double positionAngle) { PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); talon.setControl(mm); } + + @Override + public void setVoltage(double voltage) { + talon.setVoltage(voltage); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index c692a8d..1f5974a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -1,54 +1,78 @@ package frc.robot.subsystems.shooter.flywheel; +import com.ctre.phoenix6.controls.VelocityVoltage; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.simulation.DCMotorSim; +import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class FlywheelIOSim implements FlywheelIO { - // private DCMotorSim flywheelMotorSim; - // private PIDController flywheelController = new PIDController(ROLLER_KP, 0, ROLLER_KD); - // private boolean flywheelClosedLoop = false; - // private double appliedVoltage = 0.0; - // private double rollerFFVolts = 0.0; - - // // From ModuleIOSim no clue tbh - // private static final double ROLLER_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation - // private static final double ROLLER_KV = 1.0 / Units.rotationsToRadians(1.0 / ROLLER_KV_ROT); - // private static final double ROLLER_KS = 0.0; - // private static final double ROLLER_KP = 1.0; - // private static final double ROLLER_KD = 0.0; - - - // public FlywheelIOSim() { - // this.flywheelMotorSim = new DCMotorSim( - // LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), .0004, 1.0), DCMotor.getKrakenX60(1)); + private FlywheelSim flywheelMotorSim; + private PIDController flywheelController = new PIDController(FLYWHEEL_KP, 0, FLYWHEEL_KD); + private boolean flywheelClosedLoop = false; + private double appliedVoltage = 0.0; + private double flywheelFFVolts = 0.0; + + // From ModuleIOSim no clue tbh + private static final double FLYWHEEL_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation + private static final double FLYWHEEL_KV = FLYWHEEL_KV_ROT / (2.0 * Math.PI); + private static final double FLYWHEEL_KS = 0.0; + private static final double FLYWHEEL_KP = .03; + private static final double FLYWHEEL_KD = 0.0; + + + public FlywheelIOSim() { + LinearSystem plant = LinearSystemId.createFlywheelSystem(DCMotor.getKrakenX60(2), 1.0, 0.004); + + this.flywheelMotorSim = new FlywheelSim( + plant, DCMotor.getKrakenX60(2) + ); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + if (flywheelClosedLoop) { + appliedVoltage = flywheelFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); + } else { + flywheelController.reset(); + } + + flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); + flywheelMotorSim.update(0.02); + + inputs.flywheelConnected = true; - // } - - // @Override - // public void updateInputs(FlywheelIOInputs inputs) { - // if (flywheelClosedLoop) { - // appliedVoltage = rollerFFVolts + flywheelController.calculate(flywheelMotorSim.getAngularVelocityRadPerSec()); - // } else { - // flywheelController.reset(); - // } - - // flywheelMotorSim.setInputVoltage(MathUtil.clamp(appliedVoltage, -12.0, 12.0)); - // flywheelMotorSim.update(0.02); - - // inputs.flywheelConnected = true; - // inputs.l = flywheelMotorSim.getAngularVelocityRadPerSec(); - // inputs.appliedVoltage = appliedVoltage; - // inputs.currentAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); - // } - - // @Override - // public void setRPM(double RPM) { - // flywheelClosedLoop = true; - // rollerFFVolts = ROLLER_KS * Math.signum(RPM) + ROLLER_KV * RPM; - // flywheelController.setSetpoint(RPM); - // } + inputs.leaderRPM = flywheelMotorSim.getAngularVelocityRPM(); + inputs.leaderAppliedVoltage = appliedVoltage; + inputs.leaderAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + + inputs.followerRPM = flywheelMotorSim.getAngularVelocityRPM(); + inputs.followerAppliedVoltage = appliedVoltage; + inputs.followerAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + } + + @Override + public void setRPM(double rpm) { + + if (rpm == 0) { + flywheelClosedLoop = false; + appliedVoltage = 0.0; + return; + } + + flywheelClosedLoop = true; + + double setpointRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(rpm); + + flywheelFFVolts = FLYWHEEL_KS * Math.signum(setpointRadPerSec) + + FLYWHEEL_KV * setpointRadPerSec; + + flywheelController.setSetpoint(setpointRadPerSec); + } } From 3d487d4da0eddc56048d16ef4af9c10ba06f2109 Mon Sep 17 00:00:00 2001 From: micw1417 Date: Fri, 27 Feb 2026 19:09:50 -0600 Subject: [PATCH 12/21] oop merged with conflict --- src/main/java/frc/robot/RobotContainer.java | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 576222a..c578fbb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -110,7 +110,7 @@ public RobotContainer() { shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); - feeder = new Feeder(new FeederIOSim()); + feeder = new Feeder(new FeederIOSparkFlex()); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -253,22 +253,12 @@ private void configureDriverBindings() { new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); -<<<<<<< HEAD controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); controller.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); -======= - // driver.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in - // driver.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); - // driver.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); - // driver.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); - // driver.x().onTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); - driver.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); - driver.b().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); ->>>>>>> 56d63832b660a17fc6f2b71c74de01406aa63796 // // Auto aim command example; code from AKit template. From 014766752bb7486d659ec50e2cd6aac0ac4d40e0 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Fri, 6 Mar 2026 14:31:02 -0600 Subject: [PATCH 13/21] Before most aa changes except for angler homing stuff --- .Glass/glass.json | 45 ++++++++++++++ .../pathplanner/autos/Example Auto.auto | 19 ------ .../pathplanner/autos/NearOutpostClimb.auto | 55 +++++++++++++++++ .../pathplanner/autos/NearOutpostShot.auto | 43 +++++++++++++ .../{Example Path.path => MidToClimbFar.path} | 38 ++++-------- .../pathplanner/paths/NearToOutpost.path | 61 +++++++++++++++++++ .../pathplanner/paths/OutpostToMid.path | 54 ++++++++++++++++ src/main/deploy/pathplanner/settings.json | 17 +++++- src/main/java/frc/robot/RobotContainer.java | 22 +++++-- .../frc/robot/commands/InhaleCommand.java | 21 +++++-- .../frc/robot/commands/ShootFuelCommand.java | 2 +- .../frc/robot/generated/TunerConstants.java | 24 ++++---- .../subsystems/feeder/FeederConstants.java | 2 +- .../subsystems/hopper/CarpetConstants.java | 2 +- .../subsystems/intake/IntakeConstants.java | 4 +- .../frc/robot/subsystems/shooter/Shooter.java | 38 ++++++++++-- .../subsystems/shooter/ShooterConstants.java | 43 +++++++------ .../subsystems/shooter/angler/AnglerIO.java | 1 + .../shooter/angler/AnglerIOTalonFX.java | 44 ++++++++++++- .../shooter/flywheel/FlywheelIO.java | 4 +- .../shooter/flywheel/FlywheelIOSim.java | 9 +-- .../shooter/flywheel/FlywheelIOTalonFX.java | 41 ++++++++----- 22 files changed, 470 insertions(+), 119 deletions(-) create mode 100644 .Glass/glass.json delete mode 100644 src/main/deploy/pathplanner/autos/Example Auto.auto create mode 100644 src/main/deploy/pathplanner/autos/NearOutpostClimb.auto create mode 100644 src/main/deploy/pathplanner/autos/NearOutpostShot.auto rename src/main/deploy/pathplanner/paths/{Example Path.path => MidToClimbFar.path} (57%) create mode 100644 src/main/deploy/pathplanner/paths/NearToOutpost.path create mode 100644 src/main/deploy/pathplanner/paths/OutpostToMid.path diff --git a/.Glass/glass.json b/.Glass/glass.json new file mode 100644 index 0000000..0d5ce82 --- /dev/null +++ b/.Glass/glass.json @@ -0,0 +1,45 @@ +{ + "NetworkTables": { + "Retained Values": { + "open": false + }, + "transitory": { + "AdvantageKit": { + "NetworkInputs": { + "Shooter": { + "open": true + }, + "Tuning": { + "Shooter": { + "open": true + }, + "open": true + } + }, + "open": true + }, + "Tuning": { + "Angler": { + "open": true + }, + "open": true + } + }, + "types": { + "/AdvantageKit/RealOutputs/Alerts": "Alerts", + "/AdvantageKit/RealOutputs/PathPlanner": "Alerts", + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Alerts": "Alerts", + "/SmartDashboard/Auto Choices": "String Chooser", + "/SmartDashboard/PathPlanner": "Alerts", + "/SmartDashboard/Scheduler": "Scheduler" + } + }, + "NetworkTables Info": { + "visible": true + }, + "NetworkTables Settings": { + "mode": "Client (NT4)", + "serverTeam": "5431" + } +} diff --git a/src/main/deploy/pathplanner/autos/Example Auto.auto b/src/main/deploy/pathplanner/autos/Example Auto.auto deleted file mode 100644 index 70b7ab2..0000000 --- a/src/main/deploy/pathplanner/autos/Example Auto.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Example Path" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearOutpostClimb.auto b/src/main/deploy/pathplanner/autos/NearOutpostClimb.auto new file mode 100644 index 0000000..41a206e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NearOutpostClimb.auto @@ -0,0 +1,55 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NearToOutpost" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "OutpostToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "path", + "data": { + "pathName": "MidToClimbFar" + } + }, + { + "type": "named", + "data": { + "name": "ClimbL1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NearOutpostShot.auto b/src/main/deploy/pathplanner/autos/NearOutpostShot.auto new file mode 100644 index 0000000..9a5c8b4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/NearOutpostShot.auto @@ -0,0 +1,43 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "NearToOutpost" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "OutpostToMid" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/MidToClimbFar.path similarity index 57% rename from src/main/deploy/pathplanner/paths/Example Path.path rename to src/main/deploy/pathplanner/paths/MidToClimbFar.path index 303dbb2..dfa4fdc 100644 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ b/src/main/deploy/pathplanner/paths/MidToClimbFar.path @@ -3,41 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.0, - "y": 7.0 + "x": 2.3453777777777782, + "y": 4.054488888888889 }, "prevControl": null, "nextControl": { - "x": 3.013282910175676, - "y": 6.5274984191074985 + "x": 2.5986948697796577, + "y": 4.073707657710675 }, "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 5.166769560390973, - "y": 5.0964860911203305 - }, - "prevControl": { - "x": 4.166769560390973, - "y": 6.0964860911203305 - }, - "nextControl": { - "x": 6.166769560390973, - "y": 4.0964860911203305 - }, - "isLocked": false, - "linkedName": null + "linkedName": "Mid" }, { "anchor": { - "x": 7.0, - "y": 1.0 + "x": 1.2929777777753984, + "y": 4.298099999996573 }, "prevControl": { - "x": 6.75, - "y": 2.5 + "x": 1.0456042819636244, + "y": 4.347729686023186 }, "nextControl": null, "isLocked": false, @@ -49,7 +33,7 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, + "maxVelocity": 1.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, @@ -58,7 +42,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0 + "rotation": -0.30476191052866086 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/NearToOutpost.path b/src/main/deploy/pathplanner/paths/NearToOutpost.path new file mode 100644 index 0000000..2e61868 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/NearToOutpost.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.767346647646219, + "y": 0.6903566333808839 + }, + "prevControl": null, + "nextControl": { + "x": 3.162229180205553, + "y": 0.690356633380884 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 0.4291726105563479, + "y": 0.6903566333808839 + }, + "prevControl": { + "x": 1.2172227385084622, + "y": 0.6903566333808838 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Outpost" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "shoot", + "waypointRelativePos": 0.5, + "endWaypointRelativePos": null, + "command": null + } + ], + "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": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OutpostToMid.path b/src/main/deploy/pathplanner/paths/OutpostToMid.path new file mode 100644 index 0000000..601c328 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OutpostToMid.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 0.4291726105563479, + "y": 0.6903566333808839 + }, + "prevControl": null, + "nextControl": { + "x": 1.429172610556348, + "y": 0.6903566333808842 + }, + "isLocked": false, + "linkedName": "Outpost" + }, + { + "anchor": { + "x": 2.3453777777777782, + "y": 4.054488888888889 + }, + "prevControl": { + "x": 1.3453777777777782, + "y": 4.054488888888889 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Mid" + } + ], + "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": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f981b63..134b2ba 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -8,14 +8,25 @@ "defaultMaxAccel": 3.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, "robotMass": 74.088, "robotMOI": 6.883, - "robotWheelbase": 0.546, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60FOC", "driveCurrentLimit": 60.0, - "wheelCOF": 1.2 -} + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index c578fbb..0cfb555 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -8,6 +8,8 @@ package frc.robot; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -107,7 +109,7 @@ public RobotContainer() { // new VisionIOLimelight(camera1Name, drive::getRotation)); intake = new Intake(new RollerIOSparkFlex(), new PivotIOSim()); - shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); + shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); feeder = new Feeder(new FeederIOSparkFlex()); @@ -202,7 +204,7 @@ public RobotContainer() { configureDriverBindings(); configureOperatorBindings(); - + registerCommands(); SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); @@ -253,13 +255,15 @@ private void configureDriverBindings() { new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), drive) .ignoringDisable(true)); - controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in - controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, false)); + controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in + controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true, false)); + controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); controller.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); - + controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") @@ -281,6 +285,10 @@ private void configureDriverBindings() { private void configureOperatorBindings() { // operator.a().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); // operator.b().whileTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + } + + private void registerCommands() { + NamedCommands.registerCommand("ShootClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -290,4 +298,8 @@ private void configureOperatorBindings() { public Command getAutonomousCommand() { return autoChooser.get(); } + + public void teleopInit() { + CommandScheduler.getInstance().schedule(shooter.homing()); + } } diff --git a/src/main/java/frc/robot/commands/InhaleCommand.java b/src/main/java/frc/robot/commands/InhaleCommand.java index d3d8a01..57a1d88 100644 --- a/src/main/java/frc/robot/commands/InhaleCommand.java +++ b/src/main/java/frc/robot/commands/InhaleCommand.java @@ -1,5 +1,6 @@ package frc.robot.commands; +import edu.wpi.first.epilogue.logging.NullBackend; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.subsystems.feeder.Feeder; @@ -13,14 +14,26 @@ public class InhaleCommand extends ParallelCommandGroup { - public InhaleCommand(Intake intake, Carpet carpet, Feeder feeder, boolean isInhaling) { + public InhaleCommand(Intake intake, Carpet carpet, Feeder feeder, boolean useFeeder, boolean isInhaling) { + + if (isInhaling) { + if(!useFeeder){ addCommands( intake.runIntakeCommand(IntakeMode.INTAKE), - carpet.runCarpetCommand(CarpetModes.INTAKE), - feeder.runFeederCommand(FeederModes.FEEDER) + carpet.runCarpetCommand(CarpetModes.INTAKE) ); - } else { + } + else { + addCommands( + intake.runIntakeCommand(IntakeMode.INTAKE), + carpet.runCarpetCommand(CarpetModes.INTAKE), + feeder.runFeederCommand(FeederModes.FEEDER)); + } + } + + + else { addCommands( intake.runIntakeCommand(IntakeMode.OUTTAKE), carpet.runCarpetCommand(CarpetModes.OUTTAKE), diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java index 770145f..76f9ac5 100644 --- a/src/main/java/frc/robot/commands/ShootFuelCommand.java +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -18,7 +18,7 @@ public ShootFuelCommand(Intake intake, Carpet carpet, Feeder feeder, Shooter sho feeder.runFeederCommand(FeederModes.REVERSE), new WaitCommand(0.5)), new ParallelCommandGroup( - new InhaleCommand(intake, carpet, feeder, true).withName("ShootFuelCommand.Inhale")), + new InhaleCommand(intake, carpet, feeder,true, true).withName("ShootFuelCommand.Inhale")), shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withName("ShootFuelCommand.Shoot") ); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 949bf95..c555ef3 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -125,9 +125,9 @@ public class TunerConstants { // Front Left - private static final int kFrontLeftDriveMotorId = 1; - private static final int kFrontLeftSteerMotorId = 2; - private static final int kFrontLeftEncoderId = 9; + private static final int kFrontLeftDriveMotorId = 11; + private static final int kFrontLeftSteerMotorId = 12; + private static final int kFrontLeftEncoderId = 13; private static final Angle kFrontLeftEncoderOffset = Rotations.of(-0.30029296875); private static final boolean kFrontLeftSteerMotorInverted = true; private static final boolean kFrontLeftEncoderInverted = false; @@ -136,9 +136,9 @@ public class TunerConstants { private static final Distance kFrontLeftYPos = Inches.of(11.5); // Front Right - private static final int kFrontRightDriveMotorId = 4; - private static final int kFrontRightSteerMotorId = 3; - private static final int kFrontRightEncoderId = 10; + private static final int kFrontRightDriveMotorId = 41; + private static final int kFrontRightSteerMotorId = 42; + private static final int kFrontRightEncoderId = 43; private static final Angle kFrontRightEncoderOffset = Rotations.of(0.320068359375); private static final boolean kFrontRightSteerMotorInverted = true; private static final boolean kFrontRightEncoderInverted = false; @@ -147,9 +147,9 @@ public class TunerConstants { private static final Distance kFrontRightYPos = Inches.of(-11.5); // Back Left - private static final int kBackLeftDriveMotorId = 7; - private static final int kBackLeftSteerMotorId = 8; - private static final int kBackLeftEncoderId = 12; + private static final int kBackLeftDriveMotorId = 21; + private static final int kBackLeftSteerMotorId = 22; + private static final int kBackLeftEncoderId = 23; private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.181884765625); private static final boolean kBackLeftSteerMotorInverted = true; private static final boolean kBackLeftEncoderInverted = false; @@ -158,9 +158,9 @@ public class TunerConstants { private static final Distance kBackLeftYPos = Inches.of(11.5); // Back Right - private static final int kBackRightDriveMotorId = 5; - private static final int kBackRightSteerMotorId = 6; - private static final int kBackRightEncoderId = 11; + private static final int kBackRightDriveMotorId = 31; + private static final int kBackRightSteerMotorId = 32; + private static final int kBackRightEncoderId = 33; private static final Angle kBackRightEncoderOffset = Rotations.of(-0.4580078125); private static final boolean kBackRightSteerMotorInverted = true; private static final boolean kBackRightEncoderInverted = false; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 6f7bb4c..7539adf 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -15,7 +15,7 @@ public enum FeederState { public static final boolean attached = true; - public static final int id = 16; + public static final int id = 24; public static final double p = 1; public static final double i = 0; diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index 317b6db..7dbd5c5 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -25,7 +25,7 @@ public enum CarpetModes { public static final class CarpetRollerConstants { public static final boolean attached = true; - public static final int id = 17; + public static final int id = 44; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index df5da4b..abbf2af 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -31,7 +31,7 @@ public static final class IntakeRollerConstants { public static final boolean attached = true; - public static final int id = 19; + public static final int id = 46; PIDConstants pidConstants = new PIDConstants(1, 0, 0); public static final double p = 1; @@ -60,7 +60,7 @@ public static final class IntakeRollerConstants { public static final class IntakePivotConstants { public static final boolean attached = true; - public static final int id = 18; + public static final int id = 14; public static final double p = 1; public static final double i = 0; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 3c3f4d1..8b4e1f9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,10 +1,16 @@ package frc.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.RPM; + import org.littletonrobotics.junction.Logger; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; 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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -15,6 +21,7 @@ import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; +import lombok.Getter; public class Shooter extends SubsystemBase { private final AnglerIO anglerIO; @@ -25,6 +32,8 @@ public class Shooter extends SubsystemBase { private ShooterModes shooterMode; + @Getter private boolean zeroed = false; + public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { this.anglerIO = anglerIO; this.flywheelIO = flywheelIO; @@ -47,10 +56,17 @@ public void periodic() { public void runShooterEnum(ShooterModes mode) { this.shooterMode = mode; - flywheelIO.setRPM(mode.speed.baseUnitMagnitude()); + flywheelIO.setRPM(mode.speed); anglerIO.setPosition(mode.angle.magnitude()); } + public Command runAngler(ShooterModes mode) { + return new RunCommand(() -> { + // this.runShooterEnum(mode); + anglerIO.setPosition(mode.angle.magnitude()); + }, this).withName("Shooter.runAngler" + mode.toString()); + } + public Command runShooterCommand(ShooterModes mode) { return new RunCommand(() -> { this.runShooterEnum(mode); @@ -59,20 +75,30 @@ public Command runShooterCommand(ShooterModes mode) { public Command stop() { return new RunCommand(() -> { - flywheelIO.setRPM(0); + flywheelIO.setRPM(AngularVelocity.ofBaseUnits(0, RPM)); + anglerIO.setVoltage(0); }, this).withName("Intake.Stop"); } - public Command setZero() { + public InstantCommand setZero() { + return new InstantCommand(() -> anglerIO.setZero(), this); + } + + public Command homing() { return new SequentialCommandGroup( new RunCommand(() -> { + zeroed = false; anglerIO.setVoltage(-2); }, this).until( () -> anglerInputs.currentAmps > ShooterAnglerConstants.homingCurrent.baseUnitMagnitude() ).withTimeout(2), - new RunCommand( - () -> anglerIO.setPosition(0), this) - ); + new ParallelCommandGroup( + new RunCommand(() -> { + zeroed = true; + anglerIO.setVoltage(0); + }), null), + setZero() + ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index ff94eea..af39f60 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -12,9 +12,9 @@ public class ShooterConstants { public enum ShooterModes { - SHOOT_FAR(Units.RPM.of(5000), Units.Degree.of(30)), - SHOOT_CLOSE(Units.RPM.of(2500), Units.Degree.of(60)), - IDLE(Units.RPM.of(0), Units.Degree.of(0.0)); + SHOOT_FAR(Units.RPM.of(5000), Units.Radians.of(.6)), + SHOOT_CLOSE(Units.RPM.of(2500), Units.Radians.of(.5)), + IDLE(Units.RPM.of(0), Units.Degree.of(-.15)); public AngularVelocity speed; public Angle angle; @@ -27,29 +27,29 @@ public enum ShooterModes { public static class ShooterFlywheelConstants { public static final boolean attached = true; - public static final int leaderId = 13; - public static final int followerId = 14; + public static final int leaderId = 51; + public static final int followerId = 52; public static final boolean inverted = false; public static final boolean breakType = false; public static final double gearRatio = 1 / 1; - // public static final double p = 0; - // public static final double i = 0; - // public static final double d = 0; + public static final double p = 0.002000; + public static final double i = 0; + public static final double d = 0; - public static final LoggedNetworkNumber testp = new LoggedNetworkNumber("Shooter/P", 0); - public static final LoggedNetworkNumber testi = new LoggedNetworkNumber("Shooter/I", 0); - public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("Shooter/D", 0); - public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("Shooter/kS", 0); - public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("Shooter/kV", 0); + public static LoggedNetworkNumber testp = new LoggedNetworkNumber("/Tuning/Shooter/P", 002000); + public static final LoggedNetworkNumber testi = new LoggedNetworkNumber("/Tuning/Shooter/I", 0); + public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("/Tuning/Shooter/D", 0); + public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0); + public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0); - // public static final double kS = 0.5; - // public static final double kV = 0.14; //feedforward + public static final double kS = 0; + public static final double kV = 0.001000; //feedforward // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later - public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.RotorSensor; public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); @@ -58,7 +58,7 @@ public static class ShooterFlywheelConstants { public static class ShooterAnglerConstants { public static final boolean attached = true; - public static final int id = 15; + public static final int id = 53; public static final boolean inverted = false; public static final boolean breakType = false; public static final double gearRatio = 1 / 1; @@ -77,6 +77,15 @@ public static class ShooterAnglerConstants { // public static final double maxForwardOutput = 1; // public static final double maxReverseOutput = 0.5; + + public static LoggedNetworkNumber anglerP = new LoggedNetworkNumber("/Tuning/Angler/P", .002000); + public static final LoggedNetworkNumber anglerI = new LoggedNetworkNumber("/Tuning/Angler/I", 0); + public static final LoggedNetworkNumber anglerD = new LoggedNetworkNumber("/Tuning/Angler/D", 0); + public static final LoggedNetworkNumber anglerkS = new LoggedNetworkNumber("/Tuning/Angler/kS", 0); + public static final LoggedNetworkNumber anglerkV = new LoggedNetworkNumber("/Tuning/Angler/kV", 0); + + public static final boolean tunePID = true; + public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); public static final Angle maxFowardRotation = Units.Rotation.of(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java index 713446e..8925839 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIO.java @@ -20,4 +20,5 @@ public default void setPosition(double positionAngle) {} /** Run the motor to the specified voltage. */ public default void setVoltage(double voltage) {} + public default void setZero() {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index e4ac499..be7ffbf 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -2,13 +2,18 @@ import static edu.wpi.first.units.Units.*; +import org.littletonrobotics.junction.Logger; + import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; +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.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; @@ -18,6 +23,8 @@ public class AnglerIOTalonFX implements AnglerIO { private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANIVORE_CANBUS); + public final PIDController pid = new PIDController(ShooterAnglerConstants.anglerP.get(), ShooterAnglerConstants.anglerI.get(), ShooterAnglerConstants.anglerD.get()); + public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { super("AnglerTalonFX", Constants.CANIVORE_CANBUS); @@ -53,6 +60,13 @@ public AnglerIOTalonFX() { @Override public void updateInputs(AnglerIOInputs inputs) { + + if (ShooterAnglerConstants.tunePID) { + pid.setP(ShooterAnglerConstants.anglerP.get()); + pid.setI(ShooterAnglerConstants.anglerI.get()); + pid.setD(ShooterAnglerConstants.anglerD.get()); + } + var anglerStatus = BaseStatusSignal.refreshAll(appliedVoltage, currentAmps, pivotPosition); inputs.anglerConnected = anglerConnectedDebounce.calculate(anglerStatus.isOK()); @@ -64,12 +78,38 @@ public void updateInputs(AnglerIOInputs inputs) { @Override public void setPosition(double positionAngle) { - PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); - talon.setControl(mm); + // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); + // talon.setControl(mm); + + Logger.recordOutput("/ShooterAngler/DesiredAngle", positionAngle); + Logger.recordOutput("/ShooterAngler/Voltage", talon.getMotorVoltage().getValueAsDouble()); + Angle currentAngle = talon.getPosition().getValue(); + double pidOutput = pid.calculate(currentAngle.in(Units.Radian), positionAngle); + + double voltage = pidOutput + ShooterAnglerConstants.anglerkS.get() + ShooterAnglerConstants.anglerkV.get() * positionAngle; + + voltage = Math.max(Math.min(voltage, 12), -12); + + System.out.println(voltage); + talon.setVoltage( ShooterAnglerConstants.anglerkV.get()); + + // if(positionAngle > 0){ + // talon.setVoltage(voltage); + // } + // else { + // talon.set(0); + // } + } @Override public void setVoltage(double voltage) { talon.setVoltage(voltage); } + + @Override + public void setZero() { + talon.setPosition(0); + + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java index 31cee28..028dcaa 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -2,6 +2,8 @@ import org.littletonrobotics.junction.AutoLog; +import edu.wpi.first.units.measure.AngularVelocity; + public interface FlywheelIO { @AutoLog public static class FlywheelIOInputs { @@ -19,5 +21,5 @@ public static class FlywheelIOInputs { public default void updateInputs(FlywheelIOInputs inputs) {} /** Run the motor to the specified rotation per minute. */ - public default void setRPM(double rpm) {} + public default void setRPM(AngularVelocity rpm) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 1f5974a..1668dde 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -8,7 +8,8 @@ import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; @@ -58,9 +59,9 @@ public void updateInputs(FlywheelIOInputs inputs) { } @Override - public void setRPM(double rpm) { + public void setRPM(AngularVelocity rpm) { - if (rpm == 0) { + if (rpm.in(Units.RPM) == 0) { flywheelClosedLoop = false; appliedVoltage = 0.0; return; @@ -68,7 +69,7 @@ public void setRPM(double rpm) { flywheelClosedLoop = true; - double setpointRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(rpm); + double setpointRadPerSec = rpm.in(Units.RadiansPerSecond); flywheelFFVolts = FLYWHEEL_KS * Math.signum(setpointRadPerSec) + FLYWHEEL_KV * setpointRadPerSec; diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 2ac7e69..6600b20 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.*; import org.ejml.dense.block.VectorOps_DDRB; +import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; @@ -86,7 +87,7 @@ public void updateInputs(FlywheelIOInputs inputs) { pid.setP(ShooterFlywheelConstants.testp.get()); pid.setI(ShooterFlywheelConstants.testi.get()); pid.setD(ShooterFlywheelConstants.testd.get()); - + var flywheelStatus = BaseStatusSignal.refreshAll(leaderAppliedVoltage, leaderAmps, leaderFlywheelRPM, followerAppliedVoltage, followerAmps, followerFlywheelRPM); @@ -111,21 +112,33 @@ public void updateInputs(FlywheelIOInputs inputs) { } @Override - public void setRPM(double rpm){ - double rps = rpm / 60.0; - AngularVelocity currentRPS = leader.getVelocity().getValue(); - double pidOutput = pid.calculate(currentRPS.in(Units.RotationsPerSecond), rps); + public void setRPM(AngularVelocity rpm){ + Logger.recordOutput("/Shooter/DesiredRPM", rpm); + Logger.recordOutput("/Shooter/Voltage", leader.getMotorVoltage().getValueAsDouble()); + AngularVelocity currentRPM = leader.getVelocity().getValue(); + double pidOutput = pid.calculate(currentRPM.in(Units.RPM), rpm.in(Units.RPM)); - double voltage = pidOutput + ShooterFlywheelConstants.testkS.get() + ShooterFlywheelConstants.testkV.get() * rps; + double voltage = pidOutput + ShooterFlywheelConstants.kS + ShooterFlywheelConstants.testkV.get() * rpm.in(Units.RPM); voltage = Math.max(Math.min(voltage, 12), -12); leader.setVoltage(voltage); - System.out.println("******************"); - System.out.println(leader.getVelocity().getValue()); - System.out.println("******************"); + if(rpm.in(Units.RotationsPerSecond) > 0){ + leader.setVoltage(voltage); + } + else { + leader.set(0); + } + // System.out.println("******************"); + // System.out.println(ShooterFlywheelConstants.testp.getAsDouble()); + // System.out.println("******************"); + // if (rpm == 0 || rpm < 0) { + // leader.setVoltage(0); + // } else { + // leader.setVoltage(5); + // } } public void setRPMbck(double rpm) { @@ -146,11 +159,11 @@ public void setRPMbck(double rpm) { // System.out.println(velocityOuput); // System.out.println("******************"); // FIX RPM WHY NO WORK? rn its hardcoded voltage - // if (rpm == 0 || rpm < 0) { - // leader.setVoltage(0); - // } else { - // leader.setVoltage(8); - // } + if (rpm == 0 || rpm < 0) { + leader.setVoltage(0); + } else { + leader.setVoltage(5); + } } } From 93fc5f756b40c41436db60b8630b2b722e3ebc7c Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Fri, 6 Mar 2026 16:38:30 -0600 Subject: [PATCH 14/21] aa stuff --- src/main/java/frc/robot/Robot.java | 2 ++ src/main/java/frc/robot/RobotContainer.java | 10 ++++--- .../frc/robot/generated/TunerConstants.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 23 ++++++++++----- .../subsystems/shooter/ShooterConstants.java | 13 +++++---- .../shooter/angler/AnglerIOTalonFX.java | 28 +++++++++++++++---- .../shooter/flywheel/FlywheelIOTalonFX.java | 2 +- 7 files changed, 56 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 935ea9d..74d890b 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -122,6 +122,8 @@ public void teleopInit() { if (autonomousCommand != null) { autonomousCommand.cancel(); } + + robotContainer.teleopInit(); } /** This function is called periodically during operator control. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0cfb555..9e570b5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -262,8 +262,8 @@ private void configureDriverBindings() { controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); controller.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); - controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); - + // controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + controller.b().whileTrue(shooter.tune()); // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") @@ -300,6 +300,8 @@ public Command getAutonomousCommand() { } public void teleopInit() { - CommandScheduler.getInstance().schedule(shooter.homing()); - } + if (!shooter.isZeroed()) { + CommandScheduler.getInstance().schedule(shooter.homing()); + } + } } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index c555ef3..3b724f4 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -86,7 +86,7 @@ public class TunerConstants { private static final boolean kInvertLeftSide = false; private static final boolean kInvertRightSide = true; - private static final int kPigeonId = 30; + private static final int kPigeonId = 2; // These are only used for simulation private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01); diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 8b4e1f9..9703b30 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,6 +1,8 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.Rotations; import org.littletonrobotics.junction.Logger; @@ -15,6 +17,7 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; @@ -49,6 +52,7 @@ public void periodic() { Logger.processInputs("Shooter/Flywheel", flywheelInputs); Logger.recordOutput("Shooter/Mode", shooterMode); + Logger.recordOutput("Shooter/Zeroed", zeroed); // System.out.println("***********************"); // System.out.println(FlywheelIOTalonFX.plotOutput); // System.out.println("***********************"); @@ -84,21 +88,26 @@ public Command stop() { public InstantCommand setZero() { return new InstantCommand(() -> anglerIO.setZero(), this); } + + public Command tune() { + return new RunCommand(() -> { + flywheelIO.setRPM(AngularVelocity.ofRelativeUnits(ShooterFlywheelConstants.tuneDesiredSpeed.get(), RPM)); + anglerIO.setPosition(ShooterAnglerConstants.tuneDesiredPosition.get()); + }, this); + } public Command homing() { return new SequentialCommandGroup( new RunCommand(() -> { zeroed = false; - anglerIO.setVoltage(-2); + anglerIO.setVoltage(-1); }, this).until( () -> anglerInputs.currentAmps > ShooterAnglerConstants.homingCurrent.baseUnitMagnitude() ).withTimeout(2), - new ParallelCommandGroup( - new RunCommand(() -> { - zeroed = true; - anglerIO.setVoltage(0); - }), null), - setZero() + new RunCommand(() -> { + zeroed = true; + anglerIO.setZero(); + }) ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index af39f60..80e40e5 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -12,9 +12,9 @@ public class ShooterConstants { public enum ShooterModes { - SHOOT_FAR(Units.RPM.of(5000), Units.Radians.of(.6)), - SHOOT_CLOSE(Units.RPM.of(2500), Units.Radians.of(.5)), - IDLE(Units.RPM.of(0), Units.Degree.of(-.15)); + SHOOT_FAR(Units.RPM.of(0), Units.Rotations.of(.75)), + SHOOT_CLOSE(Units.RPM.of(0), Units.Rotations.of(.1)), + IDLE(Units.RPM.of(0), Units.Degree.of(0)); public AngularVelocity speed; public Angle angle; @@ -43,6 +43,8 @@ public static class ShooterFlywheelConstants { public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("/Tuning/Shooter/D", 0); public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0); public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0); + public static final LoggedNetworkNumber tuneDesiredSpeed = new LoggedNetworkNumber("/Tuning/Shooter/desiredSpeed", 0); + public static final double kS = 0; public static final double kV = 0.001000; //feedforward @@ -69,11 +71,11 @@ public static class ShooterAnglerConstants { // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later - public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.FusedCANcoder; + public static final FeedbackSensorSourceValue feedbackSensorCTRE = FeedbackSensorSourceValue.RotorSensor; public static final Current stallLimit = Units.Amps.of(60); public static final Current supplyLimit = Units.Amps.of(80); - public static final Current homingCurrent = Units.Amps.of(50); + public static final Current homingCurrent = Units.Amps.of(35); // public static final double maxForwardOutput = 1; // public static final double maxReverseOutput = 0.5; @@ -83,6 +85,7 @@ public static class ShooterAnglerConstants { public static final LoggedNetworkNumber anglerD = new LoggedNetworkNumber("/Tuning/Angler/D", 0); public static final LoggedNetworkNumber anglerkS = new LoggedNetworkNumber("/Tuning/Angler/kS", 0); public static final LoggedNetworkNumber anglerkV = new LoggedNetworkNumber("/Tuning/Angler/kV", 0); + public static final LoggedNetworkNumber tuneDesiredPosition = new LoggedNetworkNumber("/Tuning/Angler/desiredPosition", 0); public static final boolean tunePID = true; diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index be7ffbf..fdc2ed3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -6,6 +6,7 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.controls.NeutralOut; import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; @@ -84,14 +85,28 @@ public void setPosition(double positionAngle) { Logger.recordOutput("/ShooterAngler/DesiredAngle", positionAngle); Logger.recordOutput("/ShooterAngler/Voltage", talon.getMotorVoltage().getValueAsDouble()); Angle currentAngle = talon.getPosition().getValue(); - double pidOutput = pid.calculate(currentAngle.in(Units.Radian), positionAngle); + double angleDifference = positionAngle - currentAngle.in(Radians); + double voltage = 0; + if (angleDifference > 0.05) { + voltage = ShooterAnglerConstants.anglerP.get(); + } else if (angleDifference < -0.05) { + voltage = -ShooterAnglerConstants.anglerD.get(); + } else { + voltage = ShooterAnglerConstants.anglerkS.get(); + } + // if (currentAngle.in(Rotations) > positionAngle) { + + // } + // double pidOutput = pid.calculate(currentAngle.in(Units.Rotations), positionAngle); - double voltage = pidOutput + ShooterAnglerConstants.anglerkS.get() + ShooterAnglerConstants.anglerkV.get() * positionAngle; + // double voltage = pidOutput + ShooterAnglerConstants.anglerkS.get() + ShooterAnglerConstants.anglerkV.get() * positionAngle; voltage = Math.max(Math.min(voltage, 12), -12); - System.out.println(voltage); - talon.setVoltage( ShooterAnglerConstants.anglerkV.get()); + // System.out.println(voltage); + // talon.setVoltage( ShooterAnglerConstants.anglerkV.get()); + + talon.setVoltage(voltage); // if(positionAngle > 0){ // talon.setVoltage(voltage); @@ -109,7 +124,8 @@ public void setVoltage(double voltage) { @Override public void setZero() { - talon.setPosition(0); - + talon.setControl(new NeutralOut()); + talon.setPosition(0.0); + talon.getPosition().waitForUpdate(0.1); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 6600b20..29cc1d8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -118,7 +118,7 @@ public void setRPM(AngularVelocity rpm){ AngularVelocity currentRPM = leader.getVelocity().getValue(); double pidOutput = pid.calculate(currentRPM.in(Units.RPM), rpm.in(Units.RPM)); - double voltage = pidOutput + ShooterFlywheelConstants.kS + ShooterFlywheelConstants.testkV.get() * rpm.in(Units.RPM); + double voltage = pidOutput + ShooterFlywheelConstants.testkS.get() + ShooterFlywheelConstants.testkV.get() * rpm.in(Units.RPM); voltage = Math.max(Math.min(voltage, 12), -12); From 35b773abcce4ea0e04f6aebe6fe03fffe7e302cd Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Fri, 6 Mar 2026 20:26:49 -0600 Subject: [PATCH 15/21] angler working --- src/main/java/frc/robot/RobotContainer.java | 25 ++++++---- .../frc/robot/commands/ShootFuelCommand.java | 6 +-- .../subsystems/intake/IntakeConstants.java | 11 ++-- .../intake/pivot/PivotIOSparkFlex.java | 38 ++++++++------ .../intake/pivot/PivotIOTalonFX.java | 3 +- .../subsystems/shooter/ShooterConstants.java | 32 ++++++------ .../shooter/angler/AnglerIOTalonFX.java | 50 ++++++++++++------- .../shooter/flywheel/FlywheelIOTalonFX.java | 1 + 8 files changed, 100 insertions(+), 66 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9e570b5..05762b3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,6 +18,7 @@ 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.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; import frc.robot.commands.InhaleCommand; @@ -232,15 +233,15 @@ private void configureDriverBindings() { shooter.setDefaultCommand(shooter.stop()); // shooter.setDefaultCommand(); // // Lock to 0° when A button is held - // driver - // .a() - // .whileTrue( - // DriveCommands.joystickDriveAtAngle( - // drive,\[] + controller + .a() + .whileTrue( + DriveCommands.joystickDriveAtAngle( + drive, - // () -> -driver.getLeftY(), - // () -> -driver.getLeftX(), - // () -> Rotation2d.kZero)); + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> Rotation2d.kZero)); // // Switch to X pattern when X button is pressed // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); @@ -260,10 +261,12 @@ private void configureDriverBindings() { controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); - controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); - controller.y().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + // controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); + controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + controller.povUp().whileTrue(shooter.runShooterCommand(ShooterModes.REVERSE)); + controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); // controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); - controller.b().whileTrue(shooter.tune()); + controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java index 76f9ac5..a9c5724 100644 --- a/src/main/java/frc/robot/commands/ShootFuelCommand.java +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -14,9 +14,9 @@ public class ShootFuelCommand extends SequentialCommandGroup { public ShootFuelCommand(Intake intake, Carpet carpet, Feeder feeder, Shooter shooter) { addCommands( - new ParallelRaceGroup( - feeder.runFeederCommand(FeederModes.REVERSE), - new WaitCommand(0.5)), + // new ParallelRaceGroup( + // feeder.runFeederCommand(FeederModes.REVERSE), + // new WaitCommand(0.5)), new ParallelCommandGroup( new InhaleCommand(intake, carpet, feeder,true, true).withName("ShootFuelCommand.Inhale")), shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withName("ShootFuelCommand.Shoot") diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index abbf2af..9ac7759 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -1,5 +1,7 @@ package frc.robot.subsystems.intake; +import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; + import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; import com.pathplanner.lib.config.PIDConstants; import com.revrobotics.spark.FeedbackSensor; @@ -62,9 +64,11 @@ public static final class IntakePivotConstants { public static final int id = 14; - public static final double p = 1; - public static final double i = 0; - public static final double d = 0; + public static LoggedNetworkNumber p = new LoggedNetworkNumber("/Tuning/Shooter/P", 0.003); + public static final LoggedNetworkNumber i = new LoggedNetworkNumber("/Tuning/Shooter/I", 0); + public static final LoggedNetworkNumber d = new LoggedNetworkNumber("/Tuning/Shooter/D", 0.00002); + public static final LoggedNetworkNumber kS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0.375); + public static final LoggedNetworkNumber kV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0.0021); public static final double maxIAccum = 0.2; public static final double gearRatio = 1 / 1; @@ -83,5 +87,6 @@ public static final class IntakePivotConstants { public static final Current stallLimit = Units.Amps.of(80); public static final Current supplyLimit = Units.Amps.of(60); + } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java index f9f60e4..23a40ad 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOSparkFlex.java @@ -10,35 +10,43 @@ import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; +import edu.wpi.first.math.controller.PIDController; import frc.robot.subsystems.intake.IntakeConstants.IntakePivotConstants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.team5431.titan.core.subsystem.REVMechanism; public class PivotIOSparkFlex implements PivotIO { private final SparkFlex sparkFlex = new SparkFlex(IntakePivotConstants.id, MotorType.kBrushless); private final RelativeEncoder encoder = sparkFlex.getEncoder(); + public final PIDController pid = new PIDController(ShooterFlywheelConstants.testp.get(), ShooterFlywheelConstants.testi.get(), ShooterFlywheelConstants.testd.get()); + public static class PivotSparkFlexConfig extends REVMechanism.Config { public PivotSparkFlexConfig() { - super("PivotSparkFlex", IntakePivotConstants.id); - configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); - configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); - // configGear(IntakePivotConstants.gearRatio); - // configGravity(IntakePivotConstants.gravityType); - configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); - configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); - configReverseSoftLimit( - IntakePivotConstants.maxReverseRotation, IntakePivotConstants.useRMaxRotation); - configForwardSoftLimit( - IntakePivotConstants.maxFowardRotation, IntakePivotConstants.useFMaxRotation); + super("PivotSparkFlex", IntakePivotConstants.id); + configFeedbackSensorSource(IntakePivotConstants.feedbackSensorREV); + // configGear(IntakePivotConstants.gearRatio); + // configGravity(IntakePivotConstants.gravityType); + configSmartCurrentLimit(IntakePivotConstants.stallLimit, IntakePivotConstants.supplyLimit); + configSmartStallCurrentLimit(IntakePivotConstants.stallLimit); + configReverseSoftLimit( + IntakePivotConstants.maxReverseRotation, IntakePivotConstants.useRMaxRotation); + configForwardSoftLimit( + IntakePivotConstants.maxFowardRotation, IntakePivotConstants.useFMaxRotation); } - } + } public PivotIOSparkFlex() { - sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + sparkFlex.configure(new PivotSparkFlexConfig().sparkConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); } @Override public void updateInputs(PivotIOInputs inputs) { + pid.setP(ShooterFlywheelConstants.testp.get()); + pid.setI(ShooterFlywheelConstants.testi.get()); + pid.setD(ShooterFlywheelConstants.testd.get()); + ifOk(sparkFlex, encoder::getPosition, (value) -> inputs.positionAngle = value); ifOk(sparkFlex, sparkFlex::getBusVoltage, (value) -> inputs.appliedVoltage = value); ifOk(sparkFlex, sparkFlex::getOutputCurrent, (value) -> inputs.currentAmps = value); @@ -52,6 +60,6 @@ public void setPivotVoltage(double voltage) { @Override public void setPosition(double positionAngle) { sparkFlex.getClosedLoopController().setSetpoint((positionAngle), ControlType.kPosition, - ClosedLoopSlot.kSlot0); - } + ClosedLoopSlot.kSlot0); + } } diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java index 1d37025..6e1d44b 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIOTalonFX.java @@ -1,4 +1,4 @@ -package frc.robot.subsystems.intake.pivot; + package frc.robot.subsystems.intake.pivot; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; @@ -22,7 +22,6 @@ public class PivotIOTalonFX implements PivotIO { public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { super("PivotTalonFX", Constants.RIO_CANBUS); - configPIDGains(IntakePivotConstants.p, IntakePivotConstants.i, IntakePivotConstants.d); configNeutralBrakeMode(IntakePivotConstants.breakType); configFeedbackSensorSource(IntakePivotConstants.feedbackSensorCTRE); configGearRatio(IntakePivotConstants.gearRatio); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 80e40e5..27006dc 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -4,6 +4,7 @@ import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; @@ -12,9 +13,10 @@ public class ShooterConstants { public enum ShooterModes { - SHOOT_FAR(Units.RPM.of(0), Units.Rotations.of(.75)), - SHOOT_CLOSE(Units.RPM.of(0), Units.Rotations.of(.1)), - IDLE(Units.RPM.of(0), Units.Degree.of(0)); + SHOOT_FAR(Units.RPM.of(3500), Units.Rotations.of(.75)), + SHOOT_CLOSE(Units.RPM.of(2500), Units.Rotations.of(.1)), + IDLE(Units.RPM.of(0), Units.Degree.of(0)), + REVERSE(Units.RPM.of(-500), Units.Degree.of(0)); public AngularVelocity speed; public Angle angle; @@ -38,16 +40,16 @@ public static class ShooterFlywheelConstants { public static final double i = 0; public static final double d = 0; - public static LoggedNetworkNumber testp = new LoggedNetworkNumber("/Tuning/Shooter/P", 002000); + public static LoggedNetworkNumber testp = new LoggedNetworkNumber("/Tuning/Shooter/P", 0.003); public static final LoggedNetworkNumber testi = new LoggedNetworkNumber("/Tuning/Shooter/I", 0); - public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("/Tuning/Shooter/D", 0); - public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0); - public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0); - public static final LoggedNetworkNumber tuneDesiredSpeed = new LoggedNetworkNumber("/Tuning/Shooter/desiredSpeed", 0); - + public static final LoggedNetworkNumber testd = new LoggedNetworkNumber("/Tuning/Shooter/D", 0.00002); + public static final LoggedNetworkNumber testkS = new LoggedNetworkNumber("/Tuning/Shooter/kS", 0.375); + public static final LoggedNetworkNumber testkV = new LoggedNetworkNumber("/Tuning/Shooter/kV", 0.0021); + public static final LoggedNetworkNumber tuneDesiredSpeed = new LoggedNetworkNumber("/Tuning/Shooter/desiredSpeed", + 0); public static final double kS = 0; - public static final double kV = 0.001000; //feedforward + public static final double kV = 0.001000; // feedforward // public static final double maxIAccum = 2 * i; //CTRE Doesn't have one? Might // Add later @@ -62,7 +64,7 @@ public static class ShooterAnglerConstants { public static final boolean attached = true; public static final int id = 53; public static final boolean inverted = false; - public static final boolean breakType = false; + public static final boolean breakType = true; public static final double gearRatio = 1 / 1; public static final double p = 1; @@ -79,15 +81,17 @@ public static class ShooterAnglerConstants { // public static final double maxForwardOutput = 1; // public static final double maxReverseOutput = 0.5; - public static LoggedNetworkNumber anglerP = new LoggedNetworkNumber("/Tuning/Angler/P", .002000); public static final LoggedNetworkNumber anglerI = new LoggedNetworkNumber("/Tuning/Angler/I", 0); public static final LoggedNetworkNumber anglerD = new LoggedNetworkNumber("/Tuning/Angler/D", 0); public static final LoggedNetworkNumber anglerkS = new LoggedNetworkNumber("/Tuning/Angler/kS", 0); public static final LoggedNetworkNumber anglerkV = new LoggedNetworkNumber("/Tuning/Angler/kV", 0); - public static final LoggedNetworkNumber tuneDesiredPosition = new LoggedNetworkNumber("/Tuning/Angler/desiredPosition", 0); + public static final LoggedNetworkNumber tuneDesiredPosition = new LoggedNetworkNumber( + "/Tuning/Angler/desiredPosition", 0); + + public static final boolean tunePID = true; - public static final boolean tunePID = true; + public static final BangBangController bangBangController = new BangBangController(); public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); public static final Angle maxFowardRotation = Units.Rotation.of(2); diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index fdc2ed3..72ba2f8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.units.Units; @@ -24,7 +25,8 @@ public class AnglerIOTalonFX implements AnglerIO { private final TalonFX talon = new TalonFX(ShooterAnglerConstants.id, Constants.CANIVORE_CANBUS); - public final PIDController pid = new PIDController(ShooterAnglerConstants.anglerP.get(), ShooterAnglerConstants.anglerI.get(), ShooterAnglerConstants.anglerD.get()); + public final PIDController pid = new PIDController(ShooterAnglerConstants.anglerP.get(), + ShooterAnglerConstants.anglerI.get(), ShooterAnglerConstants.anglerD.get()); public static class PivotTalonFXConfig extends CTREMechanism.Config { public PivotTalonFXConfig() { @@ -38,14 +40,12 @@ public PivotTalonFXConfig() { } } - private StatusSignal appliedVoltage; private StatusSignal pivotPosition; private StatusSignal currentAmps; // No clue stole from ModuleIO - private final Debouncer anglerConnectedDebounce = - new Debouncer(0.5, Debouncer.DebounceType.kFalling); + private final Debouncer anglerConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); private PivotTalonFXConfig config = new PivotTalonFXConfig(); @@ -78,28 +78,42 @@ public void updateInputs(AnglerIOInputs inputs) { } @Override - public void setPosition(double positionAngle) { + public void setPosition(double angleSetpoint) { // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); - // talon.setControl(mm); + // talon.setControl(mm); - Logger.recordOutput("/ShooterAngler/DesiredAngle", positionAngle); + Logger.recordOutput("/ShooterAngler/DesiredAngle", angleSetpoint); Logger.recordOutput("/ShooterAngler/Voltage", talon.getMotorVoltage().getValueAsDouble()); - Angle currentAngle = talon.getPosition().getValue(); - double angleDifference = positionAngle - currentAngle.in(Radians); + double currentAngle = talon.getPosition().getValueAsDouble(); double voltage = 0; - if (angleDifference > 0.05) { - voltage = ShooterAnglerConstants.anglerP.get(); - } else if (angleDifference < -0.05) { - voltage = -ShooterAnglerConstants.anglerD.get(); + + ShooterAnglerConstants.bangBangController.setTolerance(0.1); + + if (angleSetpoint > currentAngle) { + voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * 1; } else { - voltage = ShooterAnglerConstants.anglerkS.get(); + voltage = -ShooterAnglerConstants.bangBangController.calculate(-currentAngle, -angleSetpoint) * 1; } + + if(ShooterAnglerConstants.bangBangController.atSetpoint()){ + voltage = 0; + } + // if (angleDi][\fference > 0.05) { + // voltage = ShooterAnglerConstants.anglerP.get(); + // } else if (angleDifference < -0.05) { + // voltage = -ShooterAnglerConstants.anglerD.get(); + // } else { + // voltage = ShooterAnglerConstants.anglerkS.get(); + // } + // if (currentAngle.in(Rotations) > positionAngle) { // } - // double pidOutput = pid.calculate(currentAngle.in(Units.Rotations), positionAngle); + // double pidOutput = pid.calculate(currentAngle.in(Units.Rotations), + // positionAngle); - // double voltage = pidOutput + ShooterAnglerConstants.anglerkS.get() + ShooterAnglerConstants.anglerkV.get() * positionAngle; + // double voltage = pidOutput + ShooterAnglerConstants.anglerkS.get() + + // ShooterAnglerConstants.anglerkV.get() * positionAngle; voltage = Math.max(Math.min(voltage, 12), -12); @@ -112,7 +126,7 @@ public void setPosition(double positionAngle) { // talon.setVoltage(voltage); // } // else { - // talon.set(0); + // talon.set(0); // } } @@ -126,6 +140,6 @@ public void setVoltage(double voltage) { public void setZero() { talon.setControl(new NeutralOut()); talon.setPosition(0.0); - talon.getPosition().waitForUpdate(0.1); + talon.getPosition().waitForUpdate(0.1); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 29cc1d8..9eef4db 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -110,6 +110,7 @@ public void updateInputs(FlywheelIOInputs inputs) { SmartDashboard.putNumber("Flywheel RPM", leader.getVelocity().getValue().in(Units.RPM)); } + @Override public void setRPM(AngularVelocity rpm){ From 0e525f7c337ba30652e898974fd57d92a6755d23 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Sat, 7 Mar 2026 08:44:10 -0600 Subject: [PATCH 16/21] every subsystem primatively programmed --- .../deploy/apriltags/andymark/2026-hub.json | 296 +++++++++ .../deploy/apriltags/andymark/2026-none.json | 7 + .../apriltags/andymark/2026-official.json | 584 ++++++++++++++++++ .../apriltags/andymark/2026-outpost.json | 0 .../deploy/apriltags/andymark/2026-tower.json | 80 +++ src/main/java/frc/robot/FieldConstants.java | 384 ++++++++++++ src/main/java/frc/robot/RobotContainer.java | 16 +- .../frc/robot/subsystems/intake/Intake.java | 6 + .../subsystems/intake/pivot/PivotIO.java | 2 +- 9 files changed, 1371 insertions(+), 4 deletions(-) create mode 100644 src/main/deploy/apriltags/andymark/2026-hub.json create mode 100644 src/main/deploy/apriltags/andymark/2026-none.json create mode 100644 src/main/deploy/apriltags/andymark/2026-official.json create mode 100644 src/main/deploy/apriltags/andymark/2026-outpost.json create mode 100644 src/main/deploy/apriltags/andymark/2026-tower.json create mode 100644 src/main/java/frc/robot/FieldConstants.java diff --git a/src/main/deploy/apriltags/andymark/2026-hub.json b/src/main/deploy/apriltags/andymark/2026-hub.json new file mode 100644 index 0000000..1e7c79c --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-hub.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-none.json b/src/main/deploy/apriltags/andymark/2026-none.json new file mode 100644 index 0000000..a558e3b --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-none.json @@ -0,0 +1,7 @@ +{ + "tags": [], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-official.json b/src/main/deploy/apriltags/andymark/2026-official.json new file mode 100644 index 0000000..2b732e2 --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-official.json @@ -0,0 +1,584 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 11.863959, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 11.9013986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 11.2978438, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 11.9013986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 11.863959, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": 11.9388636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": 12.2569986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 12.5051566, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 12.5051566, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 12.2569986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.9388636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 16.499332, + "y": 7.391907999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 16.499332, + "y": 6.960107999999999, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 17, + "pose": { + "translation": { + "x": 4.6490636, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 18, + "pose": { + "translation": { + "x": 4.6115986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 19, + "pose": { + "translation": { + "x": 5.2151534, + "y": 3.6657534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 20, + "pose": { + "translation": { + "x": 5.2151534, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 21, + "pose": { + "translation": { + "x": 4.6115986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 22, + "pose": { + "translation": { + "x": 4.6490636, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 23, + "pose": { + "translation": { + "x": 4.574159, + "y": 7.411491399999999, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 24, + "pose": { + "translation": { + "x": 4.2559986, + "y": 4.6247558, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 0.7071067811865476, + "X": 0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 25, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.3769534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 26, + "pose": { + "translation": { + "x": 4.007866, + "y": 4.0213534, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 27, + "pose": { + "translation": { + "x": 4.2559986, + "y": 3.417951, + "z": 1.12395 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 28, + "pose": { + "translation": { + "x": 4.574159, + "y": 0.6312154, + "z": 0.889 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 29, + "pose": { + "translation": { + "x": 0.0136906, + "y": 0.6507734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 30, + "pose": { + "translation": { + "x": 0.0136906, + "y": 1.0825734, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ No newline at end of file diff --git a/src/main/deploy/apriltags/andymark/2026-outpost.json b/src/main/deploy/apriltags/andymark/2026-outpost.json new file mode 100644 index 0000000..e69de29 diff --git a/src/main/deploy/apriltags/andymark/2026-tower.json b/src/main/deploy/apriltags/andymark/2026-tower.json new file mode 100644 index 0000000..37c7f24 --- /dev/null +++ b/src/main/deploy/apriltags/andymark/2026-tower.json @@ -0,0 +1,80 @@ +{ + "tags": [ + { + "ID": 15, + "pose": { + "translation": { + "x": 16.4989764, + "y": 4.3124882, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 16.4989764, + "y": 3.8806881999999994, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 31, + "pose": { + "translation": { + "x": 0.0140462, + "y": 3.7301932, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 32, + "pose": { + "translation": { + "x": 0.0140462, + "y": 4.1619931999999995, + "z": 0.55245 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + } + ], + "field": { + "length": 16.518, + "width": 8.043 + } +} \ 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..b5fa919 --- /dev/null +++ b/src/main/java/frc/robot/FieldConstants.java @@ -0,0 +1,384 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +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; +import lombok.Getter; +import lombok.RequiredArgsConstructor; + +/** + * Contains information for location of field element and other useful reference points. + * + *

NOTE: All constants are defined relative to the field coordinate system, and from the + * perspective of the blue alliance station + */ +public class FieldConstants { + public static final FieldType fieldType = FieldType.ANDYMARK; + + // AprilTag related constants + public static final int aprilTagCount = AprilTagLayoutType.OFFICIAL.getLayout().getTags().size(); + public static final double aprilTagWidth = Units.inchesToMeters(6.5); + public static final AprilTagLayoutType defaultAprilTagType = AprilTagLayoutType.OFFICIAL; + + // Field dimensions + public static final double fieldLength = AprilTagLayoutType.OFFICIAL.getLayout().getFieldLength(); + public static final double fieldWidth = AprilTagLayoutType.OFFICIAL.getLayout().getFieldWidth(); + + // Fuel dimensions + public static final double fuelDiameter = Units.inchesToMeters(5.91); + + /** + * Officially defined and relevant vertical lines found on the field (defined by X-axis offset) + */ + public static class LinesVertical { + public static final double center = fieldLength / 2.0; + public static final double starting = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX(); + public static final double allianceZone = starting; + public static final double hubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + Hub.width / 2.0; + public static final double neutralZoneNear = center - Units.inchesToMeters(120); + public static final double neutralZoneFar = center + Units.inchesToMeters(120); + public static final double oppHubCenter = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + Hub.width / 2.0; + public static final double oppAllianceZone = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(10).get().getX(); + } + + /** + * Officially defined and relevant horizontal lines found on the field (defined by Y-axis offset) + * + *

NOTE: The field element start and end are always left to right from the perspective of the + * alliance station + */ + public static class LinesHorizontal { + + public static final double center = fieldWidth / 2.0; + + // Right of hub + public static final double rightBumpStart = Hub.nearRightCorner.getY(); + public static final double rightBumpEnd = rightBumpStart - RightBump.width; + public static final double rightBumpMiddle = (rightBumpStart + rightBumpEnd) / 2.0; + public static final double rightTrenchOpenStart = rightBumpEnd - Units.inchesToMeters(12.0); + public static final double rightTrenchOpenEnd = 0; + + // Left of hub + public static final double leftBumpEnd = Hub.nearLeftCorner.getY(); + public static final double leftBumpStart = leftBumpEnd + LeftBump.width; + public static final double leftBumpMiddle = (leftBumpStart + leftBumpEnd) / 2.0; + public static final double leftTrenchOpenEnd = leftBumpStart + Units.inchesToMeters(12.0); + public static final double leftTrenchOpenStart = fieldWidth; + } + + /** Hub related constants */ + public static class Hub { + + // Dimensions + public static final double width = Units.inchesToMeters(47.0); + public static final double height = + Units.inchesToMeters(72.0); // includes the catcher at the top + public static final double innerWidth = Units.inchesToMeters(41.7); + public static final double innerHeight = Units.inchesToMeters(56.5); + + // Relevant reference points on alliance side + public static final Translation3d topCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation3d innerCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().getX() + width / 2.0, + fieldWidth / 2.0, + innerHeight); + + public static final Translation2d nearLeftCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(topCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d farLeftCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d farRightCorner = + new Translation2d(topCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Relevant reference points on the opposite side + public static final Translation3d oppTopCenterPoint = + new Translation3d( + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(4).get().getX() + width / 2.0, + fieldWidth / 2.0, + height); + public static final Translation2d oppNearLeftCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppNearRightCorner = + new Translation2d(oppTopCenterPoint.getX() - width / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d oppFarLeftCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d oppFarRightCorner = + new Translation2d(oppTopCenterPoint.getX() + width / 2.0, fieldWidth / 2.0 - width / 2.0); + + // Hub faces + public static final Pose2d nearFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(26).get().toPose2d(); + public static final Pose2d farFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(20).get().toPose2d(); + public static final Pose2d rightFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(18).get().toPose2d(); + public static final Pose2d leftFace = + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(21).get().toPose2d(); + } + + /** Left Bump related constants */ + public static class LeftBump { + + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Right Bump related constants */ + public static class RightBump { + // Dimensions + public static final double width = Units.inchesToMeters(73.0); + public static final double height = Units.inchesToMeters(6.513); + public static final double depth = Units.inchesToMeters(44.4); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d nearRightCorner = Hub.nearLeftCorner; + public static final Translation2d farLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d farRightCorner = Hub.farLeftCorner; + + // Relevant reference points on opposing side + public static final Translation2d oppNearLeftCorner = + new Translation2d(LinesVertical.hubCenter + width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppNearRightCorner = Hub.oppNearLeftCorner; + public static final Translation2d oppFarLeftCorner = + new Translation2d(LinesVertical.hubCenter - width / 2, Units.inchesToMeters(255)); + public static final Translation2d oppFarRightCorner = Hub.oppFarLeftCorner; + } + + /** Left Trench related constants */ + public static class LeftTrench { + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, fieldWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, fieldWidth - openingWidth, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, fieldWidth - openingWidth, openingHeight); + } + + public static class RightTrench { + + // Dimensions + public static final double width = Units.inchesToMeters(65.65); + public static final double depth = Units.inchesToMeters(47.0); + public static final double height = Units.inchesToMeters(40.25); + public static final double openingWidth = Units.inchesToMeters(50.34); + public static final double openingHeight = Units.inchesToMeters(22.25); + + // Relevant reference points on alliance side + public static final Translation3d openingTopLeft = + new Translation3d(LinesVertical.hubCenter, openingWidth, openingHeight); + public static final Translation3d openingTopRight = + new Translation3d(LinesVertical.hubCenter, 0, openingHeight); + + // Relevant reference points on opposing side + public static final Translation3d oppOpeningTopLeft = + new Translation3d(LinesVertical.oppHubCenter, openingWidth, openingHeight); + public static final Translation3d oppOpeningTopRight = + new Translation3d(LinesVertical.oppHubCenter, 0, openingHeight); + } + + /** Tower related constants */ + public static class Tower { + // Dimensions + 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); + + public static final double uprightHeight = Units.inchesToMeters(72.1); + + // Rung heights from the floor + public static final double lowRungHeight = Units.inchesToMeters(27.0); + public static final double midRungHeight = Units.inchesToMeters(45.0); + public static final double highRungHeight = Units.inchesToMeters(63.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d( + frontFaceX, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()); + public static final Translation2d leftUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d rightUpright = + new Translation2d( + frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(31).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + + // Relevant reference points on opposing side + public static final Translation2d oppCenterPoint = + new Translation2d( + fieldLength - frontFaceX, + AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()); + public static final Translation2d oppLeftUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + + innerOpeningWidth / 2 + + Units.inchesToMeters(0.75)); + public static final Translation2d oppRightUpright = + new Translation2d( + fieldLength - frontFaceX, + (AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(15).get().getY()) + - innerOpeningWidth / 2 + - Units.inchesToMeters(0.75)); + } + + public static class Depot { + // Dimensions + public static final double width = Units.inchesToMeters(42.0); + public static final double depth = Units.inchesToMeters(27.0); + public static final double height = Units.inchesToMeters(1.125); + public static final double distanceFromCenterY = Units.inchesToMeters(75.93); + + // Relevant reference points on alliance side + public static final Translation3d depotCenter = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY, height); + public static final Translation3d leftCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY + (width / 2), height); + public static final Translation3d rightCorner = + new Translation3d(depth, (fieldWidth / 2) + distanceFromCenterY - (width / 2), height); + } + + public static class Outpost { + // Dimensions + public static final double width = Units.inchesToMeters(31.8); + public static final double openingDistanceFromFloor = Units.inchesToMeters(28.1); + public static final double height = Units.inchesToMeters(7.0); + + // Relevant reference points on alliance side + public static final Translation2d centerPoint = + new Translation2d(0, AprilTagLayoutType.OFFICIAL.getLayout().getTagPose(29).get().getY()); + } + + public static class FuelPool { + // Dimensions + public static final double width = Units.inchesToMeters(181.9); + public static final double depth = Units.inchesToMeters(71.9); + + // Relevant reference points on alliance side + public static final Translation2d nearLeftCorner = + new Translation2d(fieldLength / 2.0 - depth / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d nearRightCorner = + new Translation2d(fieldLength / 2.0 - depth / 2.0, fieldWidth / 2.0 - width / 2.0); + public static final Translation2d leftCenter = + new Translation2d(fieldLength / 2.0, fieldWidth / 2.0 + width / 2.0); + public static final Translation2d rightCenter = + new Translation2d(fieldLength / 2.0, fieldWidth / 2.0 - width / 2.0); + } + + @RequiredArgsConstructor + public enum FieldType { + HQ("welded"), + ANDYMARK("andymark"), + WELDED("welded"); + + @Getter private final String jsonFolder; + } + + public enum AprilTagLayoutType { + OFFICIAL("2026-official"), + NONE("2026-none"), + HUB("2026-hub"), + OUTPOST("2026-outpost"), + TOWER("2026-tower"); + + private final String name; + private volatile AprilTagFieldLayout layout; + private volatile String layoutString; + + AprilTagLayoutType(String name) { + this.name = name; + } + + public AprilTagFieldLayout getLayout() { + if (layout == null) { + synchronized (this) { + if (layout == null) { + try { + Path p = + Path.of( + Filesystem.getDeployDirectory().getPath(), + "apriltags", + fieldType.getJsonFolder(), + name + ".json"); + layout = new AprilTagFieldLayout(p); + layoutString = new ObjectMapper().writeValueAsString(layout); + } catch (IOException e) { + throw new RuntimeException(e); + } + } + } + } + return layout; + } + + public String getLayoutString() { + if (layoutString == null) { + getLayout(); + } + return layoutString; + } + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 05762b3..6f3547c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -61,6 +61,9 @@ import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.flywheel.FlywheelIOSim; import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; +import frc.robot.subsystems.vision.Vision; +import frc.robot.subsystems.vision.VisionIO; +import frc.robot.subsystems.vision.VisionIOLimelight; import frc.team5431.titan.core.joysticks.CommandXboxController; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; @@ -80,6 +83,7 @@ public class RobotContainer { private final Carpet carpet; private final Climber climber; private final Feeder feeder; + private final Vision vision; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -109,11 +113,12 @@ public RobotContainer() { // new VisionIOLimelight(camera0Name, drive::getRotation), // new VisionIOLimelight(camera1Name, drive::getRotation)); - intake = new Intake(new RollerIOSparkFlex(), new PivotIOSim()); + intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); feeder = new Feeder(new FeederIOSparkFlex()); + vision = new Vision(drive::addVisionMeasurement, new VisionIOLimelight("limelight", drive::getRotation)); // vision = // new Vision( // demoDrive::addVisionMeasurement, @@ -163,6 +168,7 @@ public RobotContainer() { climber = new Climber(new ClimberIOSim()); feeder = new Feeder(new FeederIOSim()); + vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}); break; default: // Replayed robot, disable IO implementations @@ -180,6 +186,7 @@ public RobotContainer() { carpet = new Carpet(new CarpetIO() {}); climber = new Climber(new ClimberIO() {}); feeder = new Feeder(new FeederIO() {}); + vision = new Vision(drive::addVisionMeasurement, new VisionIO() {}); break; } @@ -263,10 +270,13 @@ private void configureDriverBindings() { controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); // controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); - controller.povUp().whileTrue(shooter.runShooterCommand(ShooterModes.REVERSE)); + controller.povLeft().whileTrue(shooter.runShooterCommand(ShooterModes.REVERSE)); controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); // controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); - controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // controller.b().whileTrue(intake.runPivotVoltageCommand(-1)); + // controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5)); + controller.povDown().whileTrue(intake.runPivotVoltageCommand(1)); //positive means down + controller.povUp().whileTrue(intake.runPivotVoltageCommand(-1)); // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index cd512ce..c8b6056 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -5,7 +5,9 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; import frc.robot.subsystems.intake.pivot.PivotIO; import frc.robot.subsystems.intake.pivot.PivotIOInputsAutoLogged; @@ -51,6 +53,10 @@ public Command runIntakeCommand(IntakeMode intakeMode) { }, this).withName("Intake.runIntakeCommand" + intakeMode.toString()); } + public Command runPivotVoltageCommand(double voltage){ + return new StartEndCommand(()-> pivotIO.setPivotVoltage(voltage), ()-> pivotIO.setPivotVoltage(0), this); + } + public Command stop() { // return new RunCommand(() -> { // this.pivotIO.setPivotVoltage(0); diff --git a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java index d9d6c6b..4eda308 100644 --- a/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java +++ b/src/main/java/frc/robot/subsystems/intake/pivot/PivotIO.java @@ -6,7 +6,7 @@ public interface PivotIO { @AutoLog public static class PivotIOInputs { - public boolean pivotConnected = false; + public boolean pivotConnected = true; public double appliedVoltage = 0.0; public double positionAngle = 0.0; public double currentAmps = 0.0; From b1af38b2833cdf10874bd3d2175e31baa6b18c27 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Sun, 8 Mar 2026 09:14:27 -0500 Subject: [PATCH 17/21] pre space city comp day 1 commit. Auto shoot and align implemented --- .../deploy/pathplanner/autos/MidDepot.auto | 50 +++++++++ .../deploy/pathplanner/autos/MidSimple.auto | 50 +++++++++ .../deploy/pathplanner/autos/TopDepot.auto | 50 +++++++++ .../paths/MidToDepotToTopMidShoot.path | 106 ++++++++++++++++++ .../deploy/pathplanner/paths/MidToTower.path | 61 ++++++++++ .../pathplanner/paths/OutpostToMid.path | 4 +- .../paths/TopTrenchToDepotToTopMidShoot.path | 102 +++++++++++++++++ src/main/deploy/pathplanner/settings.json | 19 +++- src/main/java/frc/robot/Robot.java | 8 +- src/main/java/frc/robot/RobotContainer.java | 96 ++++++++++++---- .../frc/robot/commands/AutoShootCommand.java | 72 ++++++++++++ .../frc/robot/commands/DriveCommands.java | 11 +- .../frc/robot/commands/ShootFuelCommand.java | 3 + .../java/frc/robot/commands/UnjamCommand.java | 16 +++ .../frc/robot/subsystems/drive/Drive.java | 11 ++ .../frc/robot/subsystems/shooter/Shooter.java | 22 ++++ .../subsystems/shooter/ShooterConstants.java | 2 + .../robot/subsystems/shooter/ShooterMath.java | 24 ++++ .../shooter/angler/AnglerIOTalonFX.java | 2 +- .../shooter/flywheel/FlywheelIO.java | 2 + .../shooter/flywheel/FlywheelIOTalonFX.java | 26 +---- .../java/frc/robot/util/AllianceFlipUtil.java | 66 +++++++++++ src/main/java/frc/robot/util/Bounds.java | 28 +++++ 23 files changed, 772 insertions(+), 59 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/MidDepot.auto create mode 100644 src/main/deploy/pathplanner/autos/MidSimple.auto create mode 100644 src/main/deploy/pathplanner/autos/TopDepot.auto create mode 100644 src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path create mode 100644 src/main/deploy/pathplanner/paths/MidToTower.path create mode 100644 src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path create mode 100644 src/main/java/frc/robot/commands/AutoShootCommand.java create mode 100644 src/main/java/frc/robot/commands/UnjamCommand.java create mode 100644 src/main/java/frc/robot/subsystems/shooter/ShooterMath.java create mode 100644 src/main/java/frc/robot/util/AllianceFlipUtil.java create mode 100644 src/main/java/frc/robot/util/Bounds.java diff --git a/src/main/deploy/pathplanner/autos/MidDepot.auto b/src/main/deploy/pathplanner/autos/MidDepot.auto new file mode 100644 index 0000000..9aaa1dd --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidDepot.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MidToDepotToTopMidShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AutoAlign" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/MidSimple.auto b/src/main/deploy/pathplanner/autos/MidSimple.auto new file mode 100644 index 0000000..4f6732e --- /dev/null +++ b/src/main/deploy/pathplanner/autos/MidSimple.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "MidToTower" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AutoAlign" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/TopDepot.auto b/src/main/deploy/pathplanner/autos/TopDepot.auto new file mode 100644 index 0000000..b7755c4 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TopDepot.auto @@ -0,0 +1,50 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TopTrenchToDepotToTopMidShoot" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 5.0 + } + }, + { + "type": "named", + "data": { + "name": "AutoAlign" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AutoShoot" + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path b/src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path new file mode 100644 index 0000000..0d56d34 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MidToDepotToTopMidShoot.path @@ -0,0 +1,106 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5473894436519258, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 2.2923395149786017, + "y": 5.775249643366619 + }, + "isLocked": false, + "linkedName": "MidStart" + }, + { + "anchor": { + "x": 0.533, + "y": 5.051 + }, + "prevControl": { + "x": 0.533, + "y": 4.768482149020954 + }, + "nextControl": { + "x": 0.533, + "y": 5.333517850979046 + }, + "isLocked": false, + "linkedName": "DepotBottom" + }, + { + "anchor": { + "x": 0.533, + "y": 6.5 + }, + "prevControl": { + "x": 0.533, + "y": 5.599291374986944 + }, + "nextControl": { + "x": 0.533, + "y": 7.400708625013055 + }, + "isLocked": false, + "linkedName": "DepotTop" + }, + { + "anchor": { + "x": 2.3958487874465044, + "y": 5.684679029957205 + }, + "prevControl": { + "x": 2.2077046137697844, + "y": 5.849305181924334 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopMidShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.4, + "rotationDegrees": -16.06402391526286 + }, + { + "waypointRelativePos": 1.0, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.16377171215882175, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.23197740263973 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidToTower.path b/src/main/deploy/pathplanner/paths/MidToTower.path new file mode 100644 index 0000000..755c387 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/MidToTower.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.5473894436519258, + "y": 4.0 + }, + "prevControl": null, + "nextControl": { + "x": 3.0139148641319236, + "y": 4.0 + }, + "isLocked": false, + "linkedName": "MidStart" + }, + { + "anchor": { + "x": 1.8912410841654772, + "y": 3.795634807417975 + }, + "prevControl": { + "x": 2.3379528555486515, + "y": 3.795634807417975 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TowerShoot" + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.3349088453747421, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OutpostToMid.path b/src/main/deploy/pathplanner/paths/OutpostToMid.path index 601c328..7fa2eab 100644 --- a/src/main/deploy/pathplanner/paths/OutpostToMid.path +++ b/src/main/deploy/pathplanner/paths/OutpostToMid.path @@ -35,8 +35,8 @@ "globalConstraints": { "maxVelocity": 3.0, "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, "nominalVoltage": 12.0, "unlimited": false }, diff --git a/src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path b/src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path new file mode 100644 index 0000000..bdeb460 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TopTrenchToDepotToTopMidShoot.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.4075222222222226, + "y": 5.545388888888888 + }, + "prevControl": null, + "nextControl": { + "x": 2.939788888888889, + "y": 4.814555555555555 + }, + "isLocked": false, + "linkedName": "TopTrenchStart" + }, + { + "anchor": { + "x": 0.533, + "y": 5.051 + }, + "prevControl": { + "x": 0.533, + "y": 4.762340810322713 + }, + "nextControl": { + "x": 0.533, + "y": 5.339659189677287 + }, + "isLocked": false, + "linkedName": "DepotBottom" + }, + { + "anchor": { + "x": 0.533, + "y": 6.5 + }, + "prevControl": { + "x": 0.533, + "y": 6.096986380542398 + }, + "nextControl": { + "x": 0.533, + "y": 6.903013619457602 + }, + "isLocked": false, + "linkedName": "DepotTop" + }, + { + "anchor": { + "x": 2.3958487874465044, + "y": 5.684679029957205 + }, + "prevControl": { + "x": 1.3958487874465044, + "y": 5.684679029957205 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "TopMidShoot" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.6597510373443868, + "rotationDegrees": -20.0 + }, + { + "waypointRelativePos": 1, + "rotationDegrees": 90.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "deployIntake", + "waypointRelativePos": 0.36029776674937175, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 91.23197740263973 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": -51.170175095029535 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 134b2ba..810f1e7 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -2,14 +2,18 @@ "robotWidth": 0.9, "robotLength": 0.9, "holonomicMode": true, - "pathFolders": [], - "autoFolders": [], + "pathFolders": [ + "AA" + ], + "autoFolders": [ + "AA" + ], "defaultMaxVel": 3.0, "defaultMaxAccel": 3.0, - "defaultMaxAngVel": 540.0, - "defaultMaxAngAccel": 720.0, + "defaultMaxAngVel": 270.0, + "defaultMaxAngAccel": 360.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 60.0, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, @@ -28,5 +32,8 @@ "brModuleY": -0.273, "bumperOffsetX": 0.0, "bumperOffsetY": 0.0, - "robotFeatures": [] + "robotFeatures": [ + "{\"name\":\"Rectangle\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.6,\"y\":0.0},\"size\":{\"width\":0.9,\"length\":0.3},\"borderRadius\":0.05,\"strokeWidth\":0.02,\"filled\":false}}", + "{\"name\":\"Line\",\"type\":\"line\",\"data\":{\"start\":{\"x\":-0.3,\"y\":0.206375},\"end\":{\"x\":-0.15,\"y\":0.206},\"strokeWidth\":0.05}}" + ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 74d890b..802cf41 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import edu.wpi.first.wpilibj2.command.Commands; + import org.littletonrobotics.junction.LogFileUtil; import org.littletonrobotics.junction.LoggedRobot; import org.littletonrobotics.junction.Logger; @@ -104,7 +106,11 @@ public void autonomousInit() { // schedule the autonomous command (example) if (autonomousCommand != null) { - CommandScheduler.getInstance().schedule(autonomousCommand); + CommandScheduler.getInstance().schedule( + Commands.sequence( + robotContainer.getHomingCommand(), + autonomousCommand.asProxy() + )); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6f3547c..73c332e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,6 +12,8 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -20,9 +22,11 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.commands.AutoShootCommand; import frc.robot.commands.DriveCommands; import frc.robot.commands.InhaleCommand; import frc.robot.commands.ShootFuelCommand; +import frc.robot.commands.UnjamCommand; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberIO; @@ -64,8 +68,14 @@ import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOLimelight; +import frc.robot.util.AllianceFlipUtil; import frc.team5431.titan.core.joysticks.CommandXboxController; +import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; + +import java.util.Set; + import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -91,6 +101,8 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; + + /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { switch (Constants.currentMode) { @@ -240,43 +252,55 @@ private void configureDriverBindings() { shooter.setDefaultCommand(shooter.stop()); // shooter.setDefaultCommand(); // // Lock to 0° when A button is held - controller - .a() - .whileTrue( - DriveCommands.joystickDriveAtAngle( - drive, + // controller + // .a() + // .whileTrue( + // DriveCommands.joystickDriveAtAngle( + // drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> Rotation2d.kZero)); + // () -> -controller.getLeftY(), + // () -> -controller.getLeftX(), + // () -> Rotation2d.kZero)); // // Switch to X pattern when X button is pressed // driver.x().onTrue(Commands.runOnce(drive::stopWithX, drive)); // Reset gyro to 0° when B button is pressed - controller - .y() - .onTrue( - Commands.runOnce( - () -> - drive.setPose( - new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), - drive) - .ignoringDisable(true)); + // controller + // .y() + // .onTrue( + // Commands.runOnce( + // () -> + // drive.setPose( + // new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), + // drive) + // .ignoringDisable(true)); + controller.y().whileTrue( + DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement) + ); + controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); + controller.a().whileTrue(Commands.defer(() -> { + return new AutoShootCommand(intake, carpet, feeder, shooter, drive); + }, Set.of(intake, carpet, feeder, shooter))); + controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true, false)); - - controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUT_IDLE)); + controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + // controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); - controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); - controller.povLeft().whileTrue(shooter.runShooterCommand(ShooterModes.REVERSE)); - controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); // controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); // controller.b().whileTrue(intake.runPivotVoltageCommand(-1)); // controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5)); + controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); controller.povDown().whileTrue(intake.runPivotVoltageCommand(1)); //positive means down - controller.povUp().whileTrue(intake.runPivotVoltageCommand(-1)); + + controller.start().whileTrue(new UnjamCommand(feeder, shooter)); + + + // upclimb is left dpad, downclimb is right dpad + // // Auto aim command example; code from AKit template. // @SuppressWarnings("resource") @@ -302,6 +326,16 @@ private void configureOperatorBindings() { private void registerCommands() { NamedCommands.registerCommand("ShootClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + + // NamedCommands.registerCommand("AutoShoot", ); + NamedCommands.registerCommand("AutoShoot", Commands.defer( + () -> {return new AutoShootCommand(intake, carpet, feeder, shooter, drive);}, Set.of(intake, carpet, feeder, shooter))); + + NamedCommands.registerCommand("AutoAlign", DriveCommands.joystickDriveAtAngle(drive, () -> 0, () -> 0, this::getAngleToGameElement)); + + NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); + + NamedCommands.registerCommand("deployIntake", intake.runIntakeCommand(IntakeMode.OUT_IDLE).withTimeout(2)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -317,4 +351,20 @@ public void teleopInit() { CommandScheduler.getInstance().schedule(shooter.homing()); } } + + public Rotation2d getAngleToGameElement() { + Translation2d hubPose = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); + Translation2d hubPoseAdj = AllianceFlipUtil.apply(hubPose); + + Pose2d robotPose = drive.getPose(); + Transform2d shooterTransform = new Transform2d(Inches.of(-10.824), Inches.of(8.125), Rotation2d.kZero); + Pose2d shooterPose = robotPose.transformBy(shooterTransform); + + Translation2d diff = hubPoseAdj.minus(shooterPose.getTranslation()); + return diff.getAngle(); + } + + public Command getHomingCommand() { + return shooter.homing(); + } } diff --git a/src/main/java/frc/robot/commands/AutoShootCommand.java b/src/main/java/frc/robot/commands/AutoShootCommand.java new file mode 100644 index 0000000..7bf2562 --- /dev/null +++ b/src/main/java/frc/robot/commands/AutoShootCommand.java @@ -0,0 +1,72 @@ +package frc.robot.commands; + +import java.util.function.Supplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.hopper.Carpet; +import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; +import frc.robot.subsystems.intake.Intake; +import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.shooter.ShooterMath; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; +import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; + +public class AutoShootCommand extends Command { + + private final Intake intake; + private final Carpet carpet; + private final Feeder feeder; + private final Shooter shooter; + private final Supplier distToHub; + private double cyclesAtTarget; + private boolean speedAchieved; + + public AutoShootCommand(Intake intake, Carpet carpet, Feeder feeder, Shooter shooter, Drive drive) { + // addCommands( + // // new ParallelRaceGroup( + // // feeder.runFeederCommand(FeederModes.REVERSE), + // // new WaitCommand(0.5)), + // new ParallelCommandGroup( + // new InhaleCommand(intake, carpet, feeder,true, true).withName("ShootFuelCommand.Inhale")), + // shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withName("ShootFuelCommand.Shoot") + // ); + + addRequirements(intake, carpet, feeder, shooter); + + this.intake = intake; + this.carpet = carpet; + this.feeder = feeder; + this.shooter = shooter; + distToHub = drive::distFromHub; + } + + @Override + public void execute() { + double desiredRPM = ShooterMath.calculateSpeed(distToHub.get()); + double desiredPos = ShooterMath.calculateHoodPosition(distToHub.get()); + + shooter.runShooterCustom(desiredRPM, desiredPos); + + if (!MathUtil.isNear(desiredPos, shooter.getPosition(), ShooterAnglerConstants.tolerance)) { + cyclesAtTarget = 0; + } else { + cyclesAtTarget++; + } + + if (MathUtil.isNear(desiredRPM, shooter.getSpeed(), desiredRPM * 0.05)) { + speedAchieved = true; + } + + if (cyclesAtTarget > 5 && speedAchieved) { + intake.runIntakeEnum(IntakeMode.INTAKE); + carpet.runRollerEnum(CarpetModes.INTAKE); + feeder.runFeederEnum(FeederModes.FEEDER); + } + } +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index a749f60..76221a4 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -30,11 +30,13 @@ import java.util.function.DoubleSupplier; import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + public class DriveCommands { private static final double DEADBAND = 0.1; - private static final double ANGLE_KP = 5.0; - private static final double ANGLE_KD = 0.4; - private static final double ANGLE_MAX_VELOCITY = 8.0; + private static final double ANGLE_KP = 20.0; + private static final double ANGLE_KD = 0.5; + private static final double ANGLE_MAX_VELOCITY = 15.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; private static final double FF_START_DELAY = 2.0; // Secs private static final double FF_RAMP_RATE = 0.1; // Volts/Sec @@ -128,6 +130,9 @@ public static Command joystickDriveAtAngle( angleController.calculate( drive.getRotation().getRadians(), rotationSupplier.get().getRadians()); + Logger.recordOutput("RotationTuning/curr", drive.getRotation().getRadians()); + Logger.recordOutput("RotationTuning/desired", rotationSupplier.get().getRadians()); + // Convert to field relative speeds & send command ChassisSpeeds speeds = new ChassisSpeeds( diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java index a9c5724..45aadaf 100644 --- a/src/main/java/frc/robot/commands/ShootFuelCommand.java +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -1,9 +1,12 @@ package frc.robot.commands; +import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederConstants.FeederModes; import frc.robot.subsystems.hopper.Carpet; diff --git a/src/main/java/frc/robot/commands/UnjamCommand.java b/src/main/java/frc/robot/commands/UnjamCommand.java new file mode 100644 index 0000000..7188640 --- /dev/null +++ b/src/main/java/frc/robot/commands/UnjamCommand.java @@ -0,0 +1,16 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.shooter.Shooter; + +public class UnjamCommand extends ParallelCommandGroup { + public UnjamCommand(Feeder feeder, Shooter shooter){ + addCommands( + feeder.runFeederCommand(FeederModes.REVERSE), + shooter.runShooterVoltageCommand(-4) + ); + addRequirements(feeder, shooter); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 3205414..0625701 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -41,7 +41,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; +import frc.robot.FieldConstants; import frc.robot.generated.TunerConstants; +import frc.robot.util.AllianceFlipUtil; import frc.robot.util.LocalADStarAK; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -154,6 +156,7 @@ public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); Logger.processInputs("Drive/Gyro", gyroInputs); + Logger.recordOutput("Drive/HubDistance", distFromHub()); for (var module : modules) { module.periodic(); } @@ -356,4 +359,12 @@ public static Translation2d[] getModuleTranslations() { new Translation2d(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY) }; } + + public double distFromHub() { + Translation2d hub = FieldConstants.Hub.topCenterPoint.toTranslation2d().minus(this.getPose().getTranslation()); + Translation2d hubAdjusted = AllianceFlipUtil.apply(hub); + Translation2d translateDiff = hubAdjusted.minus(this.getPose().getTranslation()); + return Math.sqrt(Math.pow(translateDiff.getX(), 2) + Math.pow(translateDiff.getY(), 2)); + } + } diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9703b30..0aff649 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -4,6 +4,8 @@ import static edu.wpi.first.units.Units.Radians; import static edu.wpi.first.units.Units.Rotations; +import java.util.function.Supplier; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.units.Units; @@ -64,6 +66,11 @@ public void runShooterEnum(ShooterModes mode) { anglerIO.setPosition(mode.angle.magnitude()); } + public void runShooterCustom(double rpm, double position) { + flywheelIO.setRPM(Units.RPM.of(rpm)); + anglerIO.setPosition(position); + } + public Command runAngler(ShooterModes mode) { return new RunCommand(() -> { // this.runShooterEnum(mode); @@ -77,6 +84,12 @@ public Command runShooterCommand(ShooterModes mode) { }, this).withName("Shooter.runShooterEnum" + mode.toString()); } + public Command runShooterVoltageCommand(double voltage){ + return new RunCommand(()-> { + flywheelIO.setVoltage(voltage); + }); + } + public Command stop() { return new RunCommand(() -> { flywheelIO.setRPM(AngularVelocity.ofBaseUnits(0, RPM)); @@ -110,4 +123,13 @@ public Command homing() { }) ); } + + public double getPosition() { + return anglerInputs.positionAngle; + } + + public double getSpeed() { + return flywheelInputs.leaderRPM; + } + } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 27006dc..dabe170 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -96,5 +96,7 @@ public static class ShooterAnglerConstants { public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); public static final Angle maxFowardRotation = Units.Rotation.of(2); + public static final double tolerance = 0.1; + } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java b/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java new file mode 100644 index 0000000..add5842 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java @@ -0,0 +1,24 @@ +package frc.robot.subsystems.shooter; + +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; + +public class ShooterMath { + + private static InterpolatingDoubleTreeMap hoodMap = new InterpolatingDoubleTreeMap(); + private static InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); + + static { + + } + + public static double calculateHoodPosition(double distanceToHub) { + return hoodMap.get(distanceToHub); + } + + public static double calculateSpeed(double distanceToHub) { + return speedMap.get(distanceToHub); + } + + + +} diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 72ba2f8..683d842 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -87,7 +87,7 @@ public void setPosition(double angleSetpoint) { double currentAngle = talon.getPosition().getValueAsDouble(); double voltage = 0; - ShooterAnglerConstants.bangBangController.setTolerance(0.1); + ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tolerance); if (angleSetpoint > currentAngle) { voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * 1; diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java index 028dcaa..8c39887 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -22,4 +22,6 @@ public default void updateInputs(FlywheelIOInputs inputs) {} /** Run the motor to the specified rotation per minute. */ public default void setRPM(AngularVelocity rpm) {} + + public default void setVoltage(double voltage) {} } diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 9eef4db..bcce3a4 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -142,29 +142,9 @@ public void setRPM(AngularVelocity rpm){ // } } - public void setRPMbck(double rpm) { - // AngularVelcity rps = Units.RotationsPerSecond.of(rpm / 60); - // AngularVelocity rps2 = RotationsPerSecond.of(4800 / 60); - // // VelocityVoltage output = config.velocityControl.withVelocity(rps); - // VelocityVoltage velocityOuput = new - // VelocityVoltage(rps2).withSlot(0).withFeedForward(5); - // leader.setControl(velocityOuput); - - // plotOutput = velocityOuput; - // plotrps = rps2; - - - // System.out.println("******************"); - // System.out.println(rps2); - // System.out.println("******************"); - // System.out.println(velocityOuput); - // System.out.println("******************"); - // FIX RPM WHY NO WORK? rn its hardcoded voltage - if (rpm == 0 || rpm < 0) { - leader.setVoltage(0); - } else { - leader.setVoltage(5); - } + @Override + public void setVoltage(double voltage) { + leader.setVoltage(voltage); } } diff --git a/src/main/java/frc/robot/util/AllianceFlipUtil.java b/src/main/java/frc/robot/util/AllianceFlipUtil.java new file mode 100644 index 0000000..4214ff8 --- /dev/null +++ b/src/main/java/frc/robot/util/AllianceFlipUtil.java @@ -0,0 +1,66 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.geometry.*; +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.FieldConstants; + +public class AllianceFlipUtil { + public static double applyX(double x) { + return shouldFlip() ? FieldConstants.fieldLength - x : x; + } + + public static double applyY(double y) { + return shouldFlip() ? FieldConstants.fieldWidth - y : y; + } + + public static Translation2d apply(Translation2d translation) { + return new Translation2d(applyX(translation.getX()), applyY(translation.getY())); + } + + public static Rotation2d apply(Rotation2d rotation) { + return shouldFlip() ? rotation.rotateBy(Rotation2d.kPi) : rotation; + } + + public static Pose2d apply(Pose2d pose) { + return shouldFlip() + ? new Pose2d(apply(pose.getTranslation()), apply(pose.getRotation())) + : pose; + } + + public static Translation3d apply(Translation3d translation) { + return new Translation3d( + applyX(translation.getX()), applyY(translation.getY()), translation.getZ()); + } + + public static Rotation3d apply(Rotation3d rotation) { + return shouldFlip() ? rotation.rotateBy(new Rotation3d(0.0, 0.0, Math.PI)) : rotation; + } + + public static Pose3d apply(Pose3d pose) { + return new Pose3d(apply(pose.getTranslation()), apply(pose.getRotation())); + } + + public static Bounds apply(Bounds bounds) { + if (shouldFlip()) { + return new Bounds( + applyX(bounds.maxX()), + applyX(bounds.minX()), + applyY(bounds.maxY()), + applyY(bounds.minY())); + } else { + return bounds; + } + } + + public static boolean shouldFlip() { + return DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Bounds.java b/src/main/java/frc/robot/util/Bounds.java new file mode 100644 index 0000000..c9bd54d --- /dev/null +++ b/src/main/java/frc/robot/util/Bounds.java @@ -0,0 +1,28 @@ +// Copyright (c) 2025-2026 Littleton Robotics +// http://github.com/Mechanical-Advantage +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file at +// the root directory of this project. + +package frc.robot.util; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Translation2d; + +public record Bounds(double minX, double maxX, double minY, double maxY) { + /** Whether the translation is contained within the bounds. */ + public boolean contains(Translation2d translation) { + return translation.getX() >= minX() + && translation.getX() <= maxX() + && translation.getY() >= minY() + && translation.getY() <= maxY(); + } + + /** Clamps the translation to the bounds. */ + public Translation2d clamp(Translation2d translation) { + return new Translation2d( + MathUtil.clamp(translation.getX(), minX(), maxX()), + MathUtil.clamp(translation.getY(), minY(), maxY())); + } +} \ No newline at end of file From 07b97d9a0e9557ec72bd714c97f7d2bbd770ac25 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Sun, 8 Mar 2026 15:57:47 -0500 Subject: [PATCH 18/21] removed unused imports + Fixed Swerve PID, new button bindings, combined shooter and feeder, added pathplanner commands & paths --- .../deploy/pathplanner/autos/MidSimple.auto | 31 ++------- .../deploy/pathplanner/autos/StepBack.auto | 38 +++++++++++ .../deploy/pathplanner/paths/MidToTower.path | 7 +- .../pathplanner/paths/TakingStepBack.path | 54 ++++++++++++++++ src/main/java/frc/robot/RobotContainer.java | 64 ++++++++++++------- .../frc/robot/commands/AutoShootCommand.java | 2 - .../frc/robot/commands/DriveCommands.java | 2 +- .../frc/robot/commands/InhaleCommand.java | 20 +----- .../frc/robot/commands/ShootFuelCommand.java | 23 +++---- .../frc/robot/generated/TunerConstants.java | 4 +- .../frc/robot/subsystems/drive/Drive.java | 2 +- .../frc/robot/subsystems/feeder/Feeder.java | 2 - .../subsystems/feeder/FeederConstants.java | 2 - .../frc/robot/subsystems/hopper/Carpet.java | 1 - .../subsystems/hopper/CarpetConstants.java | 4 +- .../subsystems/hopper/CarpetIOSparkFlex.java | 1 - .../frc/robot/subsystems/intake/Intake.java | 1 - .../subsystems/intake/IntakeConstants.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 12 +--- .../subsystems/shooter/ShooterConstants.java | 3 +- .../robot/subsystems/shooter/ShooterMath.java | 4 -- .../shooter/angler/AnglerIOTalonFX.java | 5 +- .../shooter/flywheel/FlywheelIOSim.java | 3 - .../shooter/flywheel/FlywheelIOTalonFX.java | 5 -- .../frc/robot/subsystems/vision/Vision.java | 7 +- 25 files changed, 176 insertions(+), 123 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/StepBack.auto create mode 100644 src/main/deploy/pathplanner/paths/TakingStepBack.path diff --git a/src/main/deploy/pathplanner/autos/MidSimple.auto b/src/main/deploy/pathplanner/autos/MidSimple.auto index 4f6732e..2701938 100644 --- a/src/main/deploy/pathplanner/autos/MidSimple.auto +++ b/src/main/deploy/pathplanner/autos/MidSimple.auto @@ -5,40 +5,21 @@ "data": { "commands": [ { - "type": "path", - "data": { - "pathName": "MidToTower" - } - }, - { - "type": "wait", + "type": "named", "data": { - "waitTime": 2.0 + "name": "ShootClose" } }, { - "type": "deadline", + "type": "path", "data": { - "commands": [ - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "named", - "data": { - "name": "AutoAlign" - } - } - ] + "pathName": "MidToTower" } }, { - "type": "named", + "type": "wait", "data": { - "name": "AutoShoot" + "waitTime": 2.0 } } ] diff --git a/src/main/deploy/pathplanner/autos/StepBack.auto b/src/main/deploy/pathplanner/autos/StepBack.auto new file mode 100644 index 0000000..b6523e1 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/StepBack.auto @@ -0,0 +1,38 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TakingStepBack" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "deployIntake" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/MidToTower.path b/src/main/deploy/pathplanner/paths/MidToTower.path index 755c387..741f28a 100644 --- a/src/main/deploy/pathplanner/paths/MidToTower.path +++ b/src/main/deploy/pathplanner/paths/MidToTower.path @@ -36,7 +36,12 @@ "name": "deployIntake", "waypointRelativePos": 0.3349088453747421, "endWaypointRelativePos": null, - "command": null + "command": { + "type": "named", + "data": { + "name": null + } + } } ], "globalConstraints": { diff --git a/src/main/deploy/pathplanner/paths/TakingStepBack.path b/src/main/deploy/pathplanner/paths/TakingStepBack.path new file mode 100644 index 0000000..c9404be --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TakingStepBack.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.6219, + "y": 4.015511111111111 + }, + "prevControl": null, + "nextControl": { + "x": 4.086013259619497, + "y": 4.010688804781543 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.7156666666666665, + "y": 4.025255555555555 + }, + "prevControl": { + "x": 2.417432091811763, + "y": 4.036442124686695 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 270.0, + "maxAngularAcceleration": 360.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": "AA", + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 73c332e..fc607a1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,12 +15,12 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.XboxController; 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; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoShootCommand; import frc.robot.commands.DriveCommands; @@ -31,7 +31,6 @@ import frc.robot.subsystems.climber.Climber; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOSim; -import frc.robot.subsystems.climber.ClimberIOSparkFlex; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -56,7 +55,6 @@ import frc.robot.subsystems.intake.roller.RollerIO; import frc.robot.subsystems.intake.roller.RollerIOSim; import frc.robot.subsystems.intake.roller.RollerIOSparkFlex; -import frc.robot.subsystems.intake.roller.RollerIOTalonFX; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; import frc.robot.subsystems.shooter.angler.AnglerIO; @@ -71,7 +69,6 @@ import frc.robot.util.AllianceFlipUtil; import frc.team5431.titan.core.joysticks.CommandXboxController; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; import java.util.Set; @@ -202,6 +199,7 @@ public RobotContainer() { break; } + registerCommands(); // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser()); @@ -224,10 +222,9 @@ public RobotContainer() { configureDriverBindings(); configureOperatorBindings(); - registerCommands(); SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); - + RobotController.setBrownoutVoltage(6); } /** @@ -241,8 +238,8 @@ private void configureDriverBindings() { drive.setDefaultCommand( DriveCommands.joystickDrive( drive, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), + () -> -controller.getLeftY() * 0.7, + () -> -controller.getLeftX() * 0.7, () -> -controller.getRightX())); @@ -278,26 +275,39 @@ private void configureDriverBindings() { controller.y().whileTrue( DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement) ); - controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); - // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); - controller.a().whileTrue(Commands.defer(() -> { - return new AutoShootCommand(intake, carpet, feeder, shooter, drive); - }, Set.of(intake, carpet, feeder, shooter))); - - controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in - controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, feeder, true, false)); - controller.rightBumper().onTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); - controller.leftBumper().onTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); + //TODO: ready to test + // controller.y().whileTrue( + // new ParallelCommandGroup( + // DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement), + // new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE) + // ) + // ); + + controller.x().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE)); + controller.a().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_FAR)); + // controller.a().whileTrue(Commands.defer(() -> { + // return new AutoShootCommand(intake, carpet, feeder, shooter, drive); + // }, Set.of(intake, carpet, feeder, shooter))); + // controller.b().onTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // controller.y().onTrue(shooter.runAngler(ShooterModes.IDLE)); + controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in + controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, false)); + controller.rightBumper().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); + controller.leftBumper().whileTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); // controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); // controller.b().whileTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); // controller.b().whileTrue(intake.runPivotVoltageCommand(-1)); - // controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5)); + // controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5) + // controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); - controller.povDown().whileTrue(intake.runPivotVoltageCommand(1)); //positive means down + controller.povDown().whileTrue(intake.runPivotVoltageCommand(3)); //positive means down controller.start().whileTrue(new UnjamCommand(feeder, shooter)); + // controller.x().whileTrue(shooter.tune()); + // upclimb is left dpad, downclimb is right dpad @@ -325,7 +335,17 @@ private void configureOperatorBindings() { } private void registerCommands() { - NamedCommands.registerCommand("ShootClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); + NamedCommands.registerCommand("ShootClose", Commands.parallel( + new InhaleCommand(intake, carpet, true), + new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE) + ).withTimeout(10)); + // NamedCommands.registerCommand("DeployIntake"); + // NamedCommands.registerCommand("ShootCloseJer", ); + NamedCommands.registerCommand("ShootFar", Commands.parallel( + new InhaleCommand(intake, carpet, true), + shooter.runShooterCommand(ShooterModes.SHOOT_FAR) + ).withTimeout(10)); + // NamedCommands.registerCommand("AutoShoot", ); NamedCommands.registerCommand("AutoShoot", Commands.defer( @@ -335,7 +355,7 @@ private void registerCommands() { NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); - NamedCommands.registerCommand("deployIntake", intake.runIntakeCommand(IntakeMode.OUT_IDLE).withTimeout(2)); + NamedCommands.registerCommand("deployIntake", intake.runIntakeCommand(IntakeMode.OUT_IDLE).withTimeout(0.5)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/commands/AutoShootCommand.java b/src/main/java/frc/robot/commands/AutoShootCommand.java index 7bf2562..acf52ae 100644 --- a/src/main/java/frc/robot/commands/AutoShootCommand.java +++ b/src/main/java/frc/robot/commands/AutoShootCommand.java @@ -4,7 +4,6 @@ import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederConstants.FeederModes; @@ -15,7 +14,6 @@ import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterMath; import frc.robot.subsystems.shooter.ShooterConstants.ShooterAnglerConstants; -import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; public class AutoShootCommand extends Command { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 76221a4..1611ab6 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -34,7 +34,7 @@ public class DriveCommands { private static final double DEADBAND = 0.1; - private static final double ANGLE_KP = 20.0; + private static final double ANGLE_KP = 5.0; private static final double ANGLE_KD = 0.5; private static final double ANGLE_MAX_VELOCITY = 15.0; private static final double ANGLE_MAX_ACCELERATION = 20.0; diff --git a/src/main/java/frc/robot/commands/InhaleCommand.java b/src/main/java/frc/robot/commands/InhaleCommand.java index 57a1d88..b7a7551 100644 --- a/src/main/java/frc/robot/commands/InhaleCommand.java +++ b/src/main/java/frc/robot/commands/InhaleCommand.java @@ -1,10 +1,6 @@ package frc.robot.commands; -import edu.wpi.first.epilogue.logging.NullBackend; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import frc.robot.subsystems.feeder.Feeder; -import frc.robot.subsystems.feeder.FeederConstants.FeederModes; import frc.robot.subsystems.hopper.Carpet; import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; import frc.robot.subsystems.intake.Intake; @@ -14,33 +10,23 @@ public class InhaleCommand extends ParallelCommandGroup { - public InhaleCommand(Intake intake, Carpet carpet, Feeder feeder, boolean useFeeder, boolean isInhaling) { + public InhaleCommand(Intake intake, Carpet carpet, boolean isInhaling) { if (isInhaling) { - if(!useFeeder){ addCommands( intake.runIntakeCommand(IntakeMode.INTAKE), carpet.runCarpetCommand(CarpetModes.INTAKE) ); - } - else { - addCommands( - intake.runIntakeCommand(IntakeMode.INTAKE), - carpet.runCarpetCommand(CarpetModes.INTAKE), - feeder.runFeederCommand(FeederModes.FEEDER)); - } } - else { addCommands( intake.runIntakeCommand(IntakeMode.OUTTAKE), - carpet.runCarpetCommand(CarpetModes.OUTTAKE), - feeder.runFeederCommand(FeederModes.REVERSE) + carpet.runCarpetCommand(CarpetModes.OUTTAKE) ); } - addRequirements(intake, carpet, feeder); + addRequirements(intake, carpet); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java index 45aadaf..610e125 100644 --- a/src/main/java/frc/robot/commands/ShootFuelCommand.java +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -1,30 +1,25 @@ package frc.robot.commands; -import java.util.function.Supplier; + import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.feeder.Feeder; import frc.robot.subsystems.feeder.FeederConstants.FeederModes; -import frc.robot.subsystems.hopper.Carpet; -import frc.robot.subsystems.intake.Intake; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterConstants.ShooterModes; public class ShootFuelCommand extends SequentialCommandGroup { - public ShootFuelCommand(Intake intake, Carpet carpet, Feeder feeder, Shooter shooter) { + public ShootFuelCommand(Feeder feeder, Shooter shooter, ShooterModes shooterModes) { addCommands( - // new ParallelRaceGroup( - // feeder.runFeederCommand(FeederModes.REVERSE), - // new WaitCommand(0.5)), + // shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot")).until(() -> MathUtil.isNear(shooterModes.speed.magnitude(), shooter.getSpeed(), shooterModes.speed.magnitude() * 0.05), new ParallelCommandGroup( - new InhaleCommand(intake, carpet, feeder,true, true).withName("ShootFuelCommand.Inhale")), - shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withName("ShootFuelCommand.Shoot") - ); + feeder.runFeederCommand(FeederModes.FEEDER).withName("FeederCommand.Feed"), + shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot") + )); - addRequirements(intake, carpet, feeder, shooter); + addRequirements(feeder, shooter); } + + //TODO: once shooter hits rpm run feeder } diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index 3b724f4..eb82e03 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -29,8 +29,8 @@ public class TunerConstants { // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput private static final Slot0Configs driveGains = new Slot0Configs() - .withKP(0.1).withKI(0).withKD(0) - .withKS(0).withKV(0.124); + .withKP(1).withKI(0).withKD(0) + .withKS(0).withKV(0.65); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 0625701..33408d6 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -63,7 +63,7 @@ public class Drive extends SubsystemBase { Math.hypot(TunerConstants.BackRight.LocationX, TunerConstants.BackRight.LocationY))); // PathPlanner config constants - private static final double ROBOT_MASS_KG = 74.088; + private static final double ROBOT_MASS_KG = 60; private static final double ROBOT_MOI = 6.883; private static final double WHEEL_COF = 1.2; private static final RobotConfig PP_CONFIG = diff --git a/src/main/java/frc/robot/subsystems/feeder/Feeder.java b/src/main/java/frc/robot/subsystems/feeder/Feeder.java index 8f170bb..eaf34a5 100644 --- a/src/main/java/frc/robot/subsystems/feeder/Feeder.java +++ b/src/main/java/frc/robot/subsystems/feeder/Feeder.java @@ -2,12 +2,10 @@ import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.feeder.FeederConstants.FeederModes; -import frc.robot.subsystems.feeder.FeederConstants.FeederState; import lombok.Getter; import lombok.Setter; diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 7539adf..a9ad82e 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.feeder; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; - import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.Current; diff --git a/src/main/java/frc/robot/subsystems/hopper/Carpet.java b/src/main/java/frc/robot/subsystems/hopper/Carpet.java index 8db22af..775c27f 100644 --- a/src/main/java/frc/robot/subsystems/hopper/Carpet.java +++ b/src/main/java/frc/robot/subsystems/hopper/Carpet.java @@ -6,7 +6,6 @@ import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.subsystems.hopper.CarpetConstants.CarpetModes; -import frc.robot.subsystems.hopper.CarpetIO.CarpetIOInputs; public class Carpet extends SubsystemBase { private final CarpetIO carpetIO; diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index 7dbd5c5..71145a7 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -12,8 +12,8 @@ public class CarpetConstants { public enum CarpetModes { IDLE(Units.Volts.of(0.0)), - INTAKE(Units.Volts.of(5.4)), - OUTTAKE(Units.Volts.of(-2.8)); + INTAKE(Units.Volts.of(7.4)), + OUTTAKE(Units.Volts.of(-4.8)); public Voltage voltage; diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java index d47b5a6..d264649 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetIOSparkFlex.java @@ -1,6 +1,5 @@ package frc.robot.subsystems.hopper; -import com.ctre.phoenix6.sim.TalonFXSimState.MotorType; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.ResetMode; diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java index c8b6056..b4414d1 100644 --- a/src/main/java/frc/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc/robot/subsystems/intake/Intake.java @@ -5,7 +5,6 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.robot.subsystems.intake.IntakeConstants.IntakeMode; diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 9ac7759..11d8978 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -17,7 +17,7 @@ public enum IntakeMode { STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), - INTAKE(Units.Volts.of(-5.4), Units.Degrees.of(0.0)), + INTAKE(Units.Volts.of(-4), Units.Degrees.of(0.0)), OUTTAKE(Units.Volts.of(2.8), Units.Degrees.of(180.0)); public Voltage voltage; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 0aff649..9f3d67d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,20 +1,13 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RPM; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.Rotations; - -import java.util.function.Supplier; - import org.littletonrobotics.junction.Logger; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; -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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -25,7 +18,6 @@ import frc.robot.subsystems.shooter.angler.AnglerIOInputsAutoLogged; import frc.robot.subsystems.shooter.flywheel.FlywheelIO; import frc.robot.subsystems.shooter.flywheel.FlywheelIOInputsAutoLogged; -import frc.robot.subsystems.shooter.flywheel.FlywheelIOTalonFX; import lombok.Getter; public class Shooter extends SubsystemBase { @@ -78,7 +70,7 @@ public Command runAngler(ShooterModes mode) { }, this).withName("Shooter.runAngler" + mode.toString()); } - public Command runShooterCommand(ShooterModes mode) { + public Command runShooterCommand(ShooterModes mode) { return new RunCommand(() -> { this.runShooterEnum(mode); }, this).withName("Shooter.runShooterEnum" + mode.toString()); @@ -117,7 +109,7 @@ public Command homing() { }, this).until( () -> anglerInputs.currentAmps > ShooterAnglerConstants.homingCurrent.baseUnitMagnitude() ).withTimeout(2), - new RunCommand(() -> { + Commands.runOnce(() -> { zeroed = true; anglerIO.setZero(); }) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index dabe170..8328df3 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -97,6 +97,7 @@ public static class ShooterAnglerConstants { public static final Angle maxFowardRotation = Units.Rotation.of(2); public static final double tolerance = 0.1; - + public static LoggedNetworkNumber tunableTolerance = new LoggedNetworkNumber("/Tuning/Angler/tolerance", .1); + public static LoggedNetworkNumber tunableBangPerentage = new LoggedNetworkNumber("/Tuning/Angler/bangPercentage", 1); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java b/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java index add5842..e2f7a3b 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterMath.java @@ -7,10 +7,6 @@ public class ShooterMath { private static InterpolatingDoubleTreeMap hoodMap = new InterpolatingDoubleTreeMap(); private static InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); - static { - - } - public static double calculateHoodPosition(double distanceToHub) { return hoodMap.get(distanceToHub); } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 683d842..5062ae8 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -7,15 +7,11 @@ import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.controls.NeutralOut; -import com.ctre.phoenix6.controls.PositionVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; -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.Current; import edu.wpi.first.units.measure.Voltage; import frc.robot.Constants; @@ -88,6 +84,7 @@ public void setPosition(double angleSetpoint) { double voltage = 0; ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tolerance); + // ShooterAnglerConstants.bangBangController.setTolerance(ShooterAnglerConstants.tunableTolerance.getAsDouble()); if (angleSetpoint > currentAngle) { voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * 1; diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 1668dde..046b000 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -1,7 +1,5 @@ package frc.robot.subsystems.shooter.flywheel; -import com.ctre.phoenix6.controls.VelocityVoltage; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.numbers.N1; @@ -10,7 +8,6 @@ import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; import edu.wpi.first.wpilibj.simulation.FlywheelSim; public class FlywheelIOSim implements FlywheelIO { diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index bcce3a4..71789f0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -2,28 +2,23 @@ import static edu.wpi.first.units.Units.*; -import org.ejml.dense.block.VectorOps_DDRB; import org.littletonrobotics.junction.Logger; import com.ctre.phoenix6.BaseStatusSignal; import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.MotorAlignmentValue; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.units.Unit; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; -import frc.robot.subsystems.shooter.ShooterConstants; import frc.robot.subsystems.shooter.ShooterConstants.ShooterFlywheelConstants; import frc.team5431.titan.core.subsystem.CTREMechanism; diff --git a/src/main/java/frc/robot/subsystems/vision/Vision.java b/src/main/java/frc/robot/subsystems/vision/Vision.java index 9241469..7b7c162 100644 --- a/src/main/java/frc/robot/subsystems/vision/Vision.java +++ b/src/main/java/frc/robot/subsystems/vision/Vision.java @@ -19,6 +19,7 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.subsystems.vision.VisionIO.PoseObservation; import frc.robot.subsystems.vision.VisionIO.PoseObservationType; import java.util.LinkedList; import java.util.List; @@ -91,8 +92,9 @@ public void periodic() { } // Loop over pose observations - for (var observation : inputs[cameraIndex].poseObservations) { + for (PoseObservation observation : inputs[cameraIndex].poseObservations) { // Check whether to reject pose + boolean rejectPose = observation.tagCount() == 0 // Must have at least one tag || (observation.tagCount() == 1 @@ -106,6 +108,9 @@ public void periodic() { || observation.pose().getY() < 0.0 || observation.pose().getY() > aprilTagLayout.getFieldWidth(); + if (PoseObservationType.MEGATAG_2.equals(observation.type())) { + rejectPose = true; + } // Add pose to log robotPoses.add(observation.pose()); if (rejectPose) { From b4aeb5519c3d349928cab97a59fe076a74e8ec80 Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Sun, 8 Mar 2026 19:24:52 -0500 Subject: [PATCH 19/21] End of space ctiy day 1. Auton step back and auto shoot works (limited) --- .../deploy/pathplanner/autos/StepBack.auto | 38 ------------------- .../pathplanner/autos/StepBackShoot.auto | 37 ++++++++++++++++++ .../pathplanner/paths/TakingStepBack.path | 8 ++-- src/main/java/frc/robot/RobotContainer.java | 25 ++++++------ .../subsystems/hopper/CarpetConstants.java | 2 +- 5 files changed, 56 insertions(+), 54 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/StepBack.auto create mode 100644 src/main/deploy/pathplanner/autos/StepBackShoot.auto diff --git a/src/main/deploy/pathplanner/autos/StepBack.auto b/src/main/deploy/pathplanner/autos/StepBack.auto deleted file mode 100644 index b6523e1..0000000 --- a/src/main/deploy/pathplanner/autos/StepBack.auto +++ /dev/null @@ -1,38 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "TakingStepBack" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "deployIntake" - } - }, - { - "type": "named", - "data": { - "name": "ShootClose" - } - } - ] - } - } - ] - } - }, - "resetOdom": true, - "folder": "AA", - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/StepBackShoot.auto b/src/main/deploy/pathplanner/autos/StepBackShoot.auto new file mode 100644 index 0000000..0facea8 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/StepBackShoot.auto @@ -0,0 +1,37 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TakingStepBack" + } + }, + { + "type": "named", + "data": { + "name": "deployIntake" + } + }, + { + "type": "named", + "data": { + "name": "ShootClose" + } + }, + { + "type": "named", + "data": { + "name": null + } + } + ] + } + }, + "resetOdom": true, + "folder": "AA", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TakingStepBack.path b/src/main/deploy/pathplanner/paths/TakingStepBack.path index c9404be..a4d1400 100644 --- a/src/main/deploy/pathplanner/paths/TakingStepBack.path +++ b/src/main/deploy/pathplanner/paths/TakingStepBack.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 2.7156666666666665, - "y": 4.025255555555555 + "x": 2.6028673323823104, + "y": 4.028530670470756 }, "prevControl": { - "x": 2.417432091811763, - "y": 4.036442124686695 + "x": 2.304632757527407, + "y": 4.039717239601896 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index fc607a1..502466a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -21,6 +21,7 @@ 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.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AutoShootCommand; import frc.robot.commands.DriveCommands; @@ -272,16 +273,16 @@ private void configureDriverBindings() { // new Pose2d(drive.getPose().getTranslation(), Rotation2d.kZero)), // drive) // .ignoringDisable(true)); - controller.y().whileTrue( - DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement) - ); - //TODO: ready to test - // controller.y().whileTrue( - // new ParallelCommandGroup( - // DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement), - // new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE) - // ) + // controller.y().whileTrue( + // DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement) // ); + //TODO: ready to test + controller.y().whileTrue( + new ParallelCommandGroup( + DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement), + new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE) + ) + ); controller.x().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE)); controller.a().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_FAR)); @@ -303,7 +304,7 @@ private void configureDriverBindings() { // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); controller.povDown().whileTrue(intake.runPivotVoltageCommand(3)); //positive means down - + // controller.povRight().whileTrue(shooter.tune()); controller.start().whileTrue(new UnjamCommand(feeder, shooter)); // controller.x().whileTrue(shooter.tune()); @@ -355,7 +356,9 @@ private void registerCommands() { NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); - NamedCommands.registerCommand("deployIntake", intake.runIntakeCommand(IntakeMode.OUT_IDLE).withTimeout(0.5)); + NamedCommands.registerCommand("deployIntake", intake.runPivotVoltageCommand(3).withTimeout(0.5)); + + NamedCommands.registerCommand("RevShooterClose", shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE).withTimeout(0.5)); } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index 71145a7..5fb900f 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -12,7 +12,7 @@ public class CarpetConstants { public enum CarpetModes { IDLE(Units.Volts.of(0.0)), - INTAKE(Units.Volts.of(7.4)), + INTAKE(Units.Volts.of(7.4)), //5.4 OUTTAKE(Units.Volts.of(-4.8)); public Voltage voltage; From 58da56f94ab267fe5edec97d210a8c7c6abf0f6b Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Mon, 9 Mar 2026 12:08:20 -0500 Subject: [PATCH 20/21] Pre Payoff spcae city day 2. Shoot auto cool, shooter auto trying attempts --- .../pathplanner/autos/StepBackShoot.auto | 6 ---- .../pathplanner/paths/TakingStepBack.path | 10 +++--- src/main/java/frc/robot/RobotContainer.java | 30 ++++++++++------- .../frc/robot/commands/ShootFuelCommand.java | 6 ++-- .../robot/commands/ShootFuelCommandAuto.java | 27 +++++++++++++++ .../frc/robot/subsystems/drive/Drive.java | 2 +- .../subsystems/feeder/FeederConstants.java | 6 ++-- .../subsystems/intake/IntakeConstants.java | 6 ++-- .../frc/robot/subsystems/shooter/Shooter.java | 33 +++++++++++++++++++ .../subsystems/shooter/ShooterConstants.java | 4 ++- .../shooter/angler/AnglerIOTalonFX.java | 5 +++ .../shooter/flywheel/FlywheelIO.java | 2 ++ .../shooter/flywheel/FlywheelIOSim.java | 3 ++ .../shooter/flywheel/FlywheelIOTalonFX.java | 7 +++- 14 files changed, 113 insertions(+), 34 deletions(-) create mode 100644 src/main/java/frc/robot/commands/ShootFuelCommandAuto.java diff --git a/src/main/deploy/pathplanner/autos/StepBackShoot.auto b/src/main/deploy/pathplanner/autos/StepBackShoot.auto index 0facea8..3e74f4b 100644 --- a/src/main/deploy/pathplanner/autos/StepBackShoot.auto +++ b/src/main/deploy/pathplanner/autos/StepBackShoot.auto @@ -21,12 +21,6 @@ "data": { "name": "ShootClose" } - }, - { - "type": "named", - "data": { - "name": null - } } ] } diff --git a/src/main/deploy/pathplanner/paths/TakingStepBack.path b/src/main/deploy/pathplanner/paths/TakingStepBack.path index a4d1400..891c3c8 100644 --- a/src/main/deploy/pathplanner/paths/TakingStepBack.path +++ b/src/main/deploy/pathplanner/paths/TakingStepBack.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 3.6219, + "x": 4.1, "y": 4.015511111111111 }, "prevControl": null, "nextControl": { - "x": 4.086013259619497, - "y": 4.010688804781543 + "x": 4.596725420737161, + "y": 4.0100318002396955 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 4.028530670470756 }, "prevControl": { - "x": 2.304632757527407, - "y": 4.039717239601896 + "x": 2.3528831255653007, + "y": 4.031340713543461 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 502466a..457a4b7 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -27,6 +27,7 @@ import frc.robot.commands.DriveCommands; import frc.robot.commands.InhaleCommand; import frc.robot.commands.ShootFuelCommand; +import frc.robot.commands.ShootFuelCommandAuto; import frc.robot.commands.UnjamCommand; import frc.robot.generated.TunerConstants; import frc.robot.subsystems.climber.Climber; @@ -74,6 +75,7 @@ import java.util.Set; +import org.littletonrobotics.junction.Logger; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -124,7 +126,7 @@ public RobotContainer() { // new VisionIOLimelight(camera1Name, drive::getRotation)); intake = new Intake(new RollerIOSparkFlex(), new PivotIOSparkFlex()); - shooter = new Shooter(new AnglerIOTalonFX(), new FlywheelIOTalonFX()); + shooter = new Shooter(new AnglerIOSim(), new FlywheelIOTalonFX()); carpet = new Carpet(new CarpetIOSparkFlex()); climber = new Climber(new ClimberIOSim()); feeder = new Feeder(new FeederIOSparkFlex()); @@ -222,7 +224,6 @@ public RobotContainer() { "Drive SysId (Dynamic Reverse)", drive.sysIdDynamic(SysIdRoutine.Direction.kReverse)); configureDriverBindings(); - configureOperatorBindings(); SmartDashboard.putData("Scheduler", CommandScheduler.getInstance()); RobotController.setBrownoutVoltage(6); @@ -279,8 +280,8 @@ private void configureDriverBindings() { //TODO: ready to test controller.y().whileTrue( new ParallelCommandGroup( - DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), this::getAngleToGameElement), - new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE) + DriveCommands.joystickDriveAtAngle(drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> getTranslationToGameElement().getAngle()), + new ShootFuelCommandAuto(feeder, shooter, () -> getTranslationToGameElement().getNorm()) ) ); @@ -302,9 +303,17 @@ private void configureDriverBindings() { // controller.povUp().whileTrue(intake.runPivotVoltageCommand(-5) // controller.x().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_CLOSE)); // controller.a().whileTrue(shooter.runShooterCommand(ShooterModes.SHOOT_FAR)); + + controller.povUp().whileTrue(intake.runPivotVoltageCommand(-3)); controller.povDown().whileTrue(intake.runPivotVoltageCommand(3)); //positive means down + + + // controller.povRight().onTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); + // controller.povLeft().onTrue(shooter.runAngler(ShooterModes.IDLE)); // controller.povRight().whileTrue(shooter.tune()); + // controller.rightBumper().whileTrue(feeder.runFeederCommand(FeederModes.FEEDER)); + controller.start().whileTrue(new UnjamCommand(feeder, shooter)); // controller.x().whileTrue(shooter.tune()); @@ -330,11 +339,6 @@ private void configureDriverBindings() { // drive)); } - private void configureOperatorBindings() { - // operator.a().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); - // operator.b().whileTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); - } - private void registerCommands() { NamedCommands.registerCommand("ShootClose", Commands.parallel( new InhaleCommand(intake, carpet, true), @@ -352,7 +356,7 @@ private void registerCommands() { NamedCommands.registerCommand("AutoShoot", Commands.defer( () -> {return new AutoShootCommand(intake, carpet, feeder, shooter, drive);}, Set.of(intake, carpet, feeder, shooter))); - NamedCommands.registerCommand("AutoAlign", DriveCommands.joystickDriveAtAngle(drive, () -> 0, () -> 0, this::getAngleToGameElement)); + NamedCommands.registerCommand("AutoAlign", DriveCommands.joystickDriveAtAngle(drive, () -> 0, () -> 0, () -> getTranslationToGameElement().getAngle())); NamedCommands.registerCommand("Intake", intake.runIntakeCommand(IntakeMode.INTAKE)); @@ -375,7 +379,7 @@ public void teleopInit() { } } - public Rotation2d getAngleToGameElement() { + public Translation2d getTranslationToGameElement() { Translation2d hubPose = FieldConstants.Hub.innerCenterPoint.toTranslation2d(); Translation2d hubPoseAdj = AllianceFlipUtil.apply(hubPose); @@ -384,7 +388,9 @@ public Rotation2d getAngleToGameElement() { Pose2d shooterPose = robotPose.transformBy(shooterTransform); Translation2d diff = hubPoseAdj.minus(shooterPose.getTranslation()); - return diff.getAngle(); + Logger.recordOutput("/Measurements/DistToHUb", diff.getNorm()); + Logger.recordOutput("/Measurements/AngleToHub", diff.getAngle()); + return diff; } public Command getHomingCommand() { diff --git a/src/main/java/frc/robot/commands/ShootFuelCommand.java b/src/main/java/frc/robot/commands/ShootFuelCommand.java index 610e125..84a9fd5 100644 --- a/src/main/java/frc/robot/commands/ShootFuelCommand.java +++ b/src/main/java/frc/robot/commands/ShootFuelCommand.java @@ -2,6 +2,7 @@ +import edu.wpi.first.math.MathUtil; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc.robot.subsystems.feeder.Feeder; @@ -12,11 +13,12 @@ public class ShootFuelCommand extends SequentialCommandGroup { public ShootFuelCommand(Feeder feeder, Shooter shooter, ShooterModes shooterModes) { addCommands( - // shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot")).until(() -> MathUtil.isNear(shooterModes.speed.magnitude(), shooter.getSpeed(), shooterModes.speed.magnitude() * 0.05), + shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot"). + until(() -> MathUtil.isNear(shooterModes.speed.magnitude(), shooter.getSpeed(), shooterModes.speed.magnitude() * 0.10)).withTimeout(2), new ParallelCommandGroup( feeder.runFeederCommand(FeederModes.FEEDER).withName("FeederCommand.Feed"), shooter.runShooterCommand(shooterModes).withName("ShootFuelCommand.Shoot") - )); + )); addRequirements(feeder, shooter); } diff --git a/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java b/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java new file mode 100644 index 0000000..1f6c5ee --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + + + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.feeder.Feeder; +import frc.robot.subsystems.feeder.FeederConstants.FeederModes; +import frc.robot.subsystems.shooter.Shooter; + +public class ShootFuelCommandAuto extends SequentialCommandGroup { + public ShootFuelCommandAuto(Feeder feeder, Shooter shooter, DoubleSupplier dist) { + addCommands( + shooter.runShootAuto(dist.getAsDouble()).withName("ShootFuelCommand.ShootAuto"). + until(shooter.atSetpoint()).withTimeout(2), + new ParallelCommandGroup( + feeder.runFeederCommand(FeederModes.FEEDER).withName("FeederCommand.Feed"), + shooter.runShootAuto(dist.getAsDouble()).withName("ShootFuelCommand.ShootAuto") + )); + + addRequirements(feeder, shooter); + } + + //TODO: once shooter hits rpm run feeder +} diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 33408d6..e99feb4 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -364,7 +364,7 @@ public double distFromHub() { Translation2d hub = FieldConstants.Hub.topCenterPoint.toTranslation2d().minus(this.getPose().getTranslation()); Translation2d hubAdjusted = AllianceFlipUtil.apply(hub); Translation2d translateDiff = hubAdjusted.minus(this.getPose().getTranslation()); - return Math.sqrt(Math.pow(translateDiff.getX(), 2) + Math.pow(translateDiff.getY(), 2)); + return translateDiff.getNorm(); } } diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index a9ad82e..88ae65f 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -24,8 +24,8 @@ public enum FeederState { public static final double gearRatio = 1 / 1; // stall is current at 0 rpm, supply is current at runnign speed. Stator is current throguh "motor widning" whatever that means. - public static final Current stallLimit = Units.Amps.of(90); - public static final Current supplyLimit = Units.Amps.of(80); + public static final Current stallLimit = Units.Amps.of(80); + public static final Current supplyLimit = Units.Amps.of(60); public static final double maxForwardOutput = 1; public static final double maxReverseOutput = -1; @@ -34,7 +34,7 @@ public enum FeederState { // public static final double idleSpeed = 0.0; public enum FeederModes { - FEEDER(12), + FEEDER(10), REVERSE(-5), IDLE(0); diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 11d8978..f1b6a75 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -17,7 +17,7 @@ public enum IntakeMode { STOW(Units.Volts.of(0.0), Units.Degrees.of(0.0)), OUT_IDLE(Units.Volts.of(0.0), Units.Degrees.of(180.0)), - INTAKE(Units.Volts.of(-4), Units.Degrees.of(0.0)), + INTAKE(Units.Volts.of(-3), Units.Degrees.of(0.0)), OUTTAKE(Units.Volts.of(2.8), Units.Degrees.of(180.0)); public Voltage voltage; @@ -55,8 +55,8 @@ public static final class IntakeRollerConstants { public static final Angle maxReverseRotation = Units.Rotation.of(-0.1); public static final Angle maxFowardRotation = Units.Rotation.of(5); - public static final Current stallLimit = Units.Amps.of(80); - public static final Current supplyLimit = Units.Amps.of(60); + public static final Current stallLimit = Units.Amps.of(70); + public static final Current supplyLimit = Units.Amps.of(50); } public static final class IntakePivotConstants { diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index 9f3d67d..ea81b79 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -1,8 +1,13 @@ package frc.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RPM; + +import java.util.function.BooleanSupplier; + import org.littletonrobotics.junction.Logger; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; @@ -21,6 +26,9 @@ import lombok.Getter; public class Shooter extends SubsystemBase { + /* + * + */ private final AnglerIO anglerIO; private final FlywheelIO flywheelIO; @@ -28,6 +36,7 @@ public class Shooter extends SubsystemBase { private final FlywheelIOInputsAutoLogged flywheelInputs = new FlywheelIOInputsAutoLogged(); private ShooterModes shooterMode; + private static InterpolatingDoubleTreeMap speedMap = new InterpolatingDoubleTreeMap(); @Getter private boolean zeroed = false; @@ -35,6 +44,8 @@ public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { this.anglerIO = anglerIO; this.flywheelIO = flywheelIO; this.shooterMode = ShooterModes.IDLE; + speedMap.put(10.1, 2500.0); + speedMap.put(10.8, 3000.0); } @Override @@ -63,6 +74,28 @@ public void runShooterCustom(double rpm, double position) { anglerIO.setPosition(position); } + // public Command runShootAuto(double dist) { + // return new RunCommand(() -> flywheelIO.setRPM(Units.RPM.of(speedMap.get(dist))), this); + // } + + public Command runShootAuto(double dist) { + return new RunCommand(() -> runShootAutotest(dist), this); + } + + public void runShootAutotest(double dist) { + System.out.println("((((((((((()))))))))))"); + System.out.println(dist); + System.out.println(speedMap.get(dist)); + System.out.println(speedMap.get(10.1)); + System.out.println(speedMap.get(10.8)); + System.out.println("((((((((((()))))))))))"); + flywheelIO.setRPM(Units.RPM.of(speedMap.get(dist))); + } + + public BooleanSupplier atSetpoint() { + return () -> MathUtil.isNear(flywheelInputs.setpointRPM, flywheelInputs.leaderRPM, flywheelInputs.setpointRPM * 0.10); + } + public Command runAngler(ShooterModes mode) { return new RunCommand(() -> { // this.runShooterEnum(mode); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java index 8328df3..f616643 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterConstants.java @@ -98,6 +98,8 @@ public static class ShooterAnglerConstants { public static final double tolerance = 0.1; public static LoggedNetworkNumber tunableTolerance = new LoggedNetworkNumber("/Tuning/Angler/tolerance", .1); - public static LoggedNetworkNumber tunableBangPerentage = new LoggedNetworkNumber("/Tuning/Angler/bangPercentage", 1); + public static LoggedNetworkNumber bangBangForwardVoltage = new LoggedNetworkNumber("/Tuning/Angler/bangBangForwardVoltage", 1); + public static LoggedNetworkNumber bangBangReversedVoltage = new LoggedNetworkNumber("/Tuning/Angler/bangBangReversedVoltage", 1); + public static LoggedNetworkNumber bangBangAngle = new LoggedNetworkNumber("/Tuning/Angler/bangBangAngle", 1); } } diff --git a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java index 5062ae8..f16bef0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/angler/AnglerIOTalonFX.java @@ -78,6 +78,9 @@ public void setPosition(double angleSetpoint) { // PositionVoltage mm = config.positionVoltage.withPosition(positionAngle); // talon.setControl(mm); + // far is 0.75, near is 1 + // angleSetpoint = ShooterAnglerConstants.bangBangAngle.getAsDouble(); + Logger.recordOutput("/ShooterAngler/DesiredAngle", angleSetpoint); Logger.recordOutput("/ShooterAngler/Voltage", talon.getMotorVoltage().getValueAsDouble()); double currentAngle = talon.getPosition().getValueAsDouble(); @@ -88,8 +91,10 @@ public void setPosition(double angleSetpoint) { if (angleSetpoint > currentAngle) { voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * 1; + // voltage = ShooterAnglerConstants.bangBangController.calculate(currentAngle, angleSetpoint) * ShooterAnglerConstants.bangBangForwardVoltage.getAsDouble(); } else { voltage = -ShooterAnglerConstants.bangBangController.calculate(-currentAngle, -angleSetpoint) * 1; + // voltage = -ShooterAnglerConstants.bangBangController.calculate(-currentAngle, -angleSetpoint) * ShooterAnglerConstants.bangBangReversedVoltage.getAsDouble(); } if(ShooterAnglerConstants.bangBangController.atSetpoint()){ diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java index 8c39887..852782d 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIO.java @@ -15,6 +15,8 @@ public static class FlywheelIOInputs { public double followerAppliedVoltage = 0.0; public double followerRPM = 0.0; public double followerAmps = 0.0; + + public double setpointRPM = 0.0; } /** Updates the set of loggable inputs. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java index 046b000..5837592 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOSim.java @@ -16,6 +16,7 @@ public class FlywheelIOSim implements FlywheelIO { private boolean flywheelClosedLoop = false; private double appliedVoltage = 0.0; private double flywheelFFVolts = 0.0; + private double flywheelSetpoint = 0.0; // From ModuleIOSim no clue tbh private static final double FLYWHEEL_KV_ROT = 0.91035; // Same units as TunerConstants: (volt * secs) / rotation @@ -53,10 +54,12 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.followerRPM = flywheelMotorSim.getAngularVelocityRPM(); inputs.followerAppliedVoltage = appliedVoltage; inputs.followerAmps = Math.abs(flywheelMotorSim.getCurrentDrawAmps()); + inputs.setpointRPM = flywheelSetpoint; } @Override public void setRPM(AngularVelocity rpm) { + flywheelSetpoint = rpm.magnitude(); if (rpm.in(Units.RPM) == 0) { flywheelClosedLoop = false; diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 71789f0..85e7bba 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -49,6 +49,8 @@ public FlywheelTalonFXConfig() { private StatusSignal followerAmps; public static VelocityVoltage plotOutput; public static double plotrps; + + public double setpointRPM = 0.0; // No clue stole from ModuleIO private final Debouncer flywheelConnectedDebounce = new Debouncer(0.5, Debouncer.DebounceType.kFalling); @@ -97,6 +99,7 @@ public void updateInputs(FlywheelIOInputs inputs) { inputs.followerRPM = followerFlywheelRPM.getValue().in(RPM); inputs.followerAmps = followerAmps.getValueAsDouble(); + inputs.setpointRPM = setpointRPM; if (plotrps > 0 && plotOutput.Velocity > 0) { SmartDashboard.putNumber("FlyhweelRPS", plotrps); SmartDashboard.putNumber("FlyhweelOutputVelocity", plotOutput.Velocity); @@ -109,7 +112,8 @@ public void updateInputs(FlywheelIOInputs inputs) { @Override public void setRPM(AngularVelocity rpm){ - Logger.recordOutput("/Shooter/DesiredRPM", rpm); + setpointRPM = rpm.in(Units.RPM); + Logger.recordOutput("/Shooter/Voltage", leader.getMotorVoltage().getValueAsDouble()); AngularVelocity currentRPM = leader.getVelocity().getValue(); double pidOutput = pid.calculate(currentRPM.in(Units.RPM), rpm.in(Units.RPM)); @@ -127,6 +131,7 @@ public void setRPM(AngularVelocity rpm){ leader.set(0); } + // System.out.println("******************"); // System.out.println(ShooterFlywheelConstants.testp.getAsDouble()); // System.out.println("******************"); From 58b379f825c0440fff3329fc8a746c93ab848c1a Mon Sep 17 00:00:00 2001 From: FRC 5431 Programming Date: Tue, 10 Mar 2026 12:06:47 -0500 Subject: [PATCH 21/21] End of spcae city; Shoot auto and lower current --- src/main/java/frc/robot/RobotContainer.java | 9 ++++---- .../robot/commands/ShootFuelCommandAuto.java | 4 ++-- .../frc/robot/generated/TunerConstants.java | 7 +++++- .../subsystems/feeder/FeederConstants.java | 6 ++--- .../subsystems/hopper/CarpetConstants.java | 2 +- .../frc/robot/subsystems/shooter/Shooter.java | 22 ++++++++++++------- .../shooter/flywheel/FlywheelIOTalonFX.java | 2 ++ 7 files changed, 33 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 457a4b7..2467ced 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -286,7 +286,8 @@ private void configureDriverBindings() { ); controller.x().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_CLOSE)); - controller.a().whileTrue(new ShootFuelCommand(feeder, shooter, ShooterModes.SHOOT_FAR)); + controller.a().whileTrue(new ShootFuelCommandAuto(feeder, shooter, () -> getTranslationToGameElement().getNorm())); + // controller.a().whileTrue(Commands.defer(() -> { // return new AutoShootCommand(intake, carpet, feeder, shooter, drive); // }, Set.of(intake, carpet, feeder, shooter))); @@ -294,7 +295,7 @@ private void configureDriverBindings() { // controller.y().onTrue(shooter.runAngler(ShooterModes.IDLE)); controller.rightTrigger().whileTrue(new InhaleCommand(intake, carpet, true)); // TODO: run magic carpet, also when pivot is out, doesn't run if pivot is in controller.leftTrigger().whileTrue(new InhaleCommand(intake, carpet, false)); - controller.rightBumper().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); + // controller.rightBumper().whileTrue(intake.runIntakeCommand(IntakeMode.INTAKE)); controller.leftBumper().whileTrue(intake.runIntakeCommand(IntakeMode.OUTTAKE)); // controller.x().whileTrue(new ShootFuelCommand(intake, carpet, feeder, shooter)); @@ -311,8 +312,8 @@ private void configureDriverBindings() { // controller.povRight().onTrue(shooter.runAngler(ShooterModes.SHOOT_FAR)); // controller.povLeft().onTrue(shooter.runAngler(ShooterModes.IDLE)); - // controller.povRight().whileTrue(shooter.tune()); - // controller.rightBumper().whileTrue(feeder.runFeederCommand(FeederModes.FEEDER)); + controller.povRight().whileTrue(shooter.tune()); + controller.rightBumper().whileTrue(feeder.runFeederCommand(FeederModes.FEEDER)); controller.start().whileTrue(new UnjamCommand(feeder, shooter)); diff --git a/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java b/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java index 1f6c5ee..ae7aba1 100644 --- a/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java +++ b/src/main/java/frc/robot/commands/ShootFuelCommandAuto.java @@ -13,11 +13,11 @@ public class ShootFuelCommandAuto extends SequentialCommandGroup { public ShootFuelCommandAuto(Feeder feeder, Shooter shooter, DoubleSupplier dist) { addCommands( - shooter.runShootAuto(dist.getAsDouble()).withName("ShootFuelCommand.ShootAuto"). + shooter.runShootAuto(dist).withName("ShootFuelCommand.ShootAuto"). until(shooter.atSetpoint()).withTimeout(2), new ParallelCommandGroup( feeder.runFeederCommand(FeederModes.FEEDER).withName("FeederCommand.Feed"), - shooter.runShootAuto(dist.getAsDouble()).withName("ShootFuelCommand.ShootAuto") + shooter.runShootAuto(dist).withName("ShootFuelCommand.ShootAuto") )); addRequirements(feeder, shooter); diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java index eb82e03..c6a6879 100644 --- a/src/main/java/frc/robot/generated/TunerConstants.java +++ b/src/main/java/frc/robot/generated/TunerConstants.java @@ -54,7 +54,12 @@ public class TunerConstants { // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null. // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation. - private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration(); + private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withSupplyCurrentLimit(Amps.of(65)) + .withStatorCurrentLimit(Amps.of(90)) + ); private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java index 88ae65f..a9ad82e 100644 --- a/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java +++ b/src/main/java/frc/robot/subsystems/feeder/FeederConstants.java @@ -24,8 +24,8 @@ public enum FeederState { public static final double gearRatio = 1 / 1; // stall is current at 0 rpm, supply is current at runnign speed. Stator is current throguh "motor widning" whatever that means. - public static final Current stallLimit = Units.Amps.of(80); - public static final Current supplyLimit = Units.Amps.of(60); + public static final Current stallLimit = Units.Amps.of(90); + public static final Current supplyLimit = Units.Amps.of(80); public static final double maxForwardOutput = 1; public static final double maxReverseOutput = -1; @@ -34,7 +34,7 @@ public enum FeederState { // public static final double idleSpeed = 0.0; public enum FeederModes { - FEEDER(10), + FEEDER(12), REVERSE(-5), IDLE(0); diff --git a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java index 5fb900f..fb7e640 100644 --- a/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java +++ b/src/main/java/frc/robot/subsystems/hopper/CarpetConstants.java @@ -12,7 +12,7 @@ public class CarpetConstants { public enum CarpetModes { IDLE(Units.Volts.of(0.0)), - INTAKE(Units.Volts.of(7.4)), //5.4 + INTAKE(Units.Volts.of(5.4)), //5.4 OUTTAKE(Units.Volts.of(-4.8)); public Voltage voltage; diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index ea81b79..3a1c4ee 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -3,6 +3,7 @@ import static edu.wpi.first.units.Units.RPM; import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; import org.littletonrobotics.junction.Logger; @@ -44,8 +45,13 @@ public Shooter(AnglerIO anglerIO, FlywheelIO flywheelIO) { this.anglerIO = anglerIO; this.flywheelIO = flywheelIO; this.shooterMode = ShooterModes.IDLE; - speedMap.put(10.1, 2500.0); - speedMap.put(10.8, 3000.0); + speedMap.put(1.9, 2000.0); + speedMap.put(2.75, 2500.0); + speedMap.put(2.9, 2600.0); + speedMap.put(3.1, 2700.0); + speedMap.put(3.3, 2800.0); + speedMap.put(3.5, 3000.0); + speedMap.put(3.8, 3500.0); } @Override @@ -74,14 +80,14 @@ public void runShooterCustom(double rpm, double position) { anglerIO.setPosition(position); } - // public Command runShootAuto(double dist) { - // return new RunCommand(() -> flywheelIO.setRPM(Units.RPM.of(speedMap.get(dist))), this); - // } - - public Command runShootAuto(double dist) { - return new RunCommand(() -> runShootAutotest(dist), this); + public Command runShootAuto(DoubleSupplier dist) { + return new RunCommand(() -> flywheelIO.setRPM(Units.RPM.of(speedMap.get(dist.getAsDouble()))), this); } + // public Command runShootAuto(DoubleSupplier dist) { + // return new RunCommand(() -> runShootAutotest(dist.getAsDouble()), this); + // } + public void runShootAutotest(double dist) { System.out.println("((((((((((()))))))))))"); System.out.println(dist); diff --git a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java index 85e7bba..1de8ae0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/shooter/flywheel/FlywheelIOTalonFX.java @@ -37,6 +37,8 @@ public FlywheelTalonFXConfig() { configGearRatio(ShooterFlywheelConstants.gearRatio); configMotorInverted(ShooterFlywheelConstants.inverted); configFeedForwardGains(0, 0.35, 0.12, 0, 0); + configSupplyCurrentLimit(ShooterFlywheelConstants.supplyLimit); + configStatorCurrentLimit(Amps.of(100)); } }