From 51d2b80ea0ff88911192799340745aaa9338a413 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky Date: Mon, 9 Feb 2026 17:18:54 -0800 Subject: [PATCH] Robot should be ready --- .../java/frc4388/robot/RobotContainer.java | 53 ++++++++++++------ src/main/java/frc4388/robot/RobotMap.java | 23 ++++---- .../robot/constants/BuildConstants.java | 10 ++-- .../frc4388/robot/constants/Constants.java | 5 +- .../robot/subsystems/intake/Intake.java | 21 ++++--- .../subsystems/intake/IntakeConstants.java | 24 +++++--- .../robot/subsystems/intake/IntakeIO.java | 1 + .../robot/subsystems/intake/IntakeReal.java | 4 ++ .../robot/subsystems/shooter/Shooter.java | 24 ++++---- .../subsystems/shooter/ShooterConstants.java | 40 +++++++++----- .../robot/subsystems/shooter/ShooterIO.java | 9 ++- .../robot/subsystems/shooter/ShooterReal.java | 55 +++++++++++++++---- .../swerve/SwerveDriveConstants.java | 4 +- .../frc4388/utility/status/FaultReporter.java | 5 +- 14 files changed, 183 insertions(+), 95 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 70c894c..b322e41 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,7 +34,9 @@ import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; +import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; +import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -98,13 +100,13 @@ public class RobotContainer { // new InstantCommand(() -> System.out.println(m_robotLED.getMode())) // ); - private Command RobotShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) - ); + // private Command RobotShoot = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Active), m_robotShooter) + // ); - private Command RobotIntake = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) - ); + // private Command RobotIntake = new SequentialCommandGroup( + // new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Down), m_robotIntake) + // ); public RobotContainer() { @@ -147,23 +149,23 @@ public class RobotContainer { private void configureButtonBindings() { - new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) // .onTrue(new RotTo45(m_robotSwerveDrive)); - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(RobotShoot) - .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); + // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) + // .onTrue(RobotShoot) + // .onFalse(new InstantCommand(() -> m_robotShooter.setMode(Shooter.ShooterMode.Resting), m_robotShooter)); - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) - .onTrue(RobotIntake) - .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); + // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.8 && !operatorManualMode) + // .onTrue(RobotIntake) + // .onFalse(new InstantCommand(() -> m_robotIntake.setMode(Intake.IntakeMode.Up), m_robotShooter)); - new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + // .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); @@ -172,12 +174,27 @@ public class RobotContainer { .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Extended);}, m_robotIntake)); + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); + + // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> { + m_robotIntake.io.updateGains(); + m_robotShooter.io.updateGains(); + })); // IF the driver is holding the aim button, aim the robot towards the hub new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index ea8f489..3cc013a 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,8 @@ import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.swerve.SwerveDrivetrain; + +import frc4388.robot.constants.Constants; //import frc4388.robot.constants.Constants.ElevatorConstants; import frc4388.robot.constants.Constants.SimConstants; import frc4388.robot.subsystems.intake.IntakeConstants; @@ -82,22 +84,21 @@ public class RobotMap { swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); // Configure Shooter 22,23,24 - - // TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id); - // TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id); - // TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id); + TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.RIO_CANBUS); + TalonFX shooter2 = new TalonFX(ShooterConstants.SHOOTER2_ID.id, Constants.RIO_CANBUS); + TalonFX indexer = new TalonFX(ShooterConstants.INDEXER_ID.id, Constants.RIO_CANBUS); //Configure Intake 20,21 - TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id); - TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id); + TalonFX arm = new TalonFX(IntakeConstants.ARM_ID.id, Constants.RIO_CANBUS); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.RIO_CANBUS); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); // DigitalInput IRIntakeBeam = new DigitalInput(ElevatorConstants.INTAKE_LIMIT_SWITCH); - shooterIO = new ShooterIO() {}; - // shooterIO = new ShooterReal(shooter1, shooter2, indexer); + // shooterIO = new ShooterIO() {}; + shooterIO = new ShooterReal(shooter1, shooter2, indexer); intakeIO = new IntakeReal(arm, roller); @@ -105,9 +106,9 @@ public class RobotMap { FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); - // FaultTalonFX.addDevice(shooter1, "Shooter1"); - // FaultTalonFX.addDevice(shooter2, "Shooter2"); - // FaultTalonFX.addDevice(indexer, "Indexer"); + FaultTalonFX.addDevice(shooter1, "Shooter1"); + FaultTalonFX.addDevice(shooter2, "Shooter2"); + FaultTalonFX.addDevice(indexer, "Indexer"); FaultTalonFX.addDevice(arm, "Arm"); FaultTalonFX.addDevice(roller, "Roller"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 0a393b8..c3642e0 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 35; - public static final String GIT_SHA = "684e18ddcc5438f14f05c5902506d14cc4cbab19"; - public static final String GIT_DATE = "2026-02-07 14:52:21 MST"; + public static final int GIT_REVISION = 36; + public static final String GIT_SHA = "d90bddac0fff1c8f1bb6dd9a6b4a862c822a005a"; + public static final String GIT_DATE = "2026-02-09 17:03:48 MST"; public static final String GIT_BRANCH = "arm-test"; - public static final String BUILD_DATE = "2026-02-09 16:43:56 MST"; - public static final long BUILD_UNIX_TIME = 1770680636562L; + public static final String BUILD_DATE = "2026-02-09 18:14:36 MST"; + public static final long BUILD_UNIX_TIME = 1770686076395L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 490aefd..4c461a8 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -7,6 +7,8 @@ package frc4388.robot.constants; +import com.ctre.phoenix6.CANBus; + import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; @@ -31,7 +33,8 @@ import frc4388.utility.structs.LEDPatterns; * constants are needed, to reduce verbosity. */ public final class Constants { - public static final String CANBUS_NAME = "rio"; + public static final CANBus RIO_CANBUS = CANBus.roboRIO(); + public static final CANBus CANIVORE_CANBUS = new CANBus("drivetrain"); // public static final class LiDARConstants { // public static final int REEF_LIDAR_DIO_CHANNEL = 7; diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 4e87f34..0b212d7 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -1,6 +1,9 @@ package frc4388.robot.subsystems.intake; import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; import java.util.function.Supplier; @@ -12,7 +15,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.status.FaultReporter; public class Intake extends SubsystemBase { - IntakeIO io; + public IntakeIO io; IntakeStateAutoLogged state = new IntakeStateAutoLogged(); Supplier m_swervePoseSupplier; @@ -26,19 +29,19 @@ public class Intake extends SubsystemBase { } public enum IntakeMode { - Up, - Down, + Extended, + Retracted, } public void setMode(IntakeMode mode) { switch (mode) { - case Up: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_UPPER); - io.setRollerVelocity(state, IntakeConstants.ROLLER_STOP); + case Extended: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_EXTENDED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(IntakeConstants.ROLLER_ACTIVE.get())); break; - case Down: - io.setArmAngle(state, IntakeConstants.ARM_LIMIT_LOWER); - io.setRollerVelocity(state, IntakeConstants.ROLLER_MAX_VELOCITY); + case Retracted: + io.setArmAngle(state, Rotations.of(IntakeConstants.ARM_LIMIT_RETRACTED.get())); + io.setRollerVelocity(state, RotationsPerSecond.of(0)); break; } } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index 77570c1..9d1fc48 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -12,6 +12,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; public class IntakeConstants { @@ -33,21 +34,28 @@ public class IntakeConstants { //when testing the negative output of 10% made the robot put the intake up - public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); - public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); - public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); - public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); + // public static final Angle ARM_LIMIT_LOWER = Degrees.of(90); + // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); + + public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 0.25); + + public static final ConfigurableDouble ROLLER_ACTIVE = new ConfigurableDouble("Rolelr Active", 10); + // public static final ConfigurableDouble ROLL = new ConfigurableDouble("Arm angle extended", 0.25); + + // public static final AngularVelocity ROLLER_MAX_VELOCITY = RotationsPerSecond.of(4.0); + // public static final AngularVelocity ROLLER_STOP = RotationsPerSecond.of(0.0); public static final Slot0Configs ARM_PID = new Slot0Configs() - .withKP(2.0) + .withKP(0.2) .withKI(0.0) - .withKD(10.0); + .withKD(0.0); public static final Slot0Configs ROLLER_PID = new Slot0Configs() - .withKP(2.0) + .withKP(0.2) .withKI(0.0) - .withKD(10.0); + .withKD(0.0); // 0 is paralell to the ground, 90 is directly up // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index 92ba274..b97ad76 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -39,4 +39,5 @@ public interface IntakeIO { public default void setRollerVelocity(IntakeState state, AngularVelocity angularVelocity) {} public default void updateInputs(IntakeState state) {} + public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 83ec7cf..ce89c55 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -95,6 +95,10 @@ public class IntakeReal implements IntakeIO { state.rollerVelocity = m_rollerMotor.getVelocity().getValue(); state.rollerMotorCurrent = m_rollerMotor.getStatorCurrent().getValue(); + } + + @Override + public void updateGains() { IntakeConstants.ARM_PID.kP = arm_kP.get(); IntakeConstants.ARM_PID.kI = arm_kI.get(); diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 2809bd2..488d634 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -1,5 +1,9 @@ package frc4388.robot.subsystems.shooter; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; + import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -11,7 +15,7 @@ import frc4388.robot.subsystems.intake.IntakeConstants; import frc4388.robot.subsystems.shooter.ShooterIO.ShooterState; public class Shooter extends SubsystemBase { - ShooterIO io; + public ShooterIO io; ShooterStateAutoLogged state = new ShooterStateAutoLogged(); // Supplier m_swervePoseSupplier; @@ -46,19 +50,19 @@ public class Shooter extends SubsystemBase { public void setMode(ShooterMode mode) { switch (mode) { case Active: - io.setMotor1Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); - io.setIndexerVelocity(state, ShooterConstants.INDEXER_ACTIVE_VELOCITY); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_ACTIVE_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_ACTIVE_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_ACTIVE_VELOCITY.get())); break; case Resting: - io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); break; case Inactive: - io.setMotor1Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); - io.setIndexerVelocity(state, ShooterConstants.INDEXER_INACTIVE_VELOCITY); + io.setShooterVelocity(state, RotationsPerSecond.of(ShooterConstants.SHOOTER_RESTING_VELOCITY.get())); + // io.setMotor2Velocity(state, ShooterConstants.SHOOTER_RESTING_VELOCITY); + io.setIndexerVelocity(state, RotationsPerSecond.of(ShooterConstants.INDEXER_INACTIVE_VELOCITY.get())); break; } } diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 34c5b76..69b7f1a 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -11,6 +11,7 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; public class ShooterConstants { @@ -20,16 +21,20 @@ public class ShooterConstants { public static final double SHOOTERMOTOR1_GEAR_RATIO = 1.; public static final double SHOOTERMOTOR2_GEAR_RATIO = 1.; public static final double INDEXER_GEAR_RATIO = 1.; - - public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(-15); - public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(0); - public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); - + // public static final AngularVelocity SHOOTER_ACTIVE_VELOCITY = RotationsPerSecond.of(30); + // public static final AngularVelocity SHOOTER_RESTING_VELOCITY = RotationsPerSecond.of(15); + // public static final AngularVelocity SHOOTER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + // public static final AngularVelocity INDEXER_ACTIVE_VELOCITY = RotationsPerSecond.of(10); + // public static final AngularVelocity INDEXER_INACTIVE_VELOCITY = RotationsPerSecond.of(0.0); + public static final ConfigurableDouble SHOOTER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 30); + public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 15); + public static final ConfigurableDouble SHOOTER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); + + public static final ConfigurableDouble INDEXER_ACTIVE_VELOCITY = new ConfigurableDouble("Shooter Active Velocity", 10); + public static final ConfigurableDouble INDEXER_INACTIVE_VELOCITY = new ConfigurableDouble("Shooter Inactive Velocity", 0); public static Slot0Configs SHOOTER_PID = new Slot0Configs() .withKV(0.0) @@ -37,6 +42,12 @@ public class ShooterConstants { .withKI(0.0) .withKD(0.0); + public static Slot0Configs INDEXER_PID = new Slot0Configs() + .withKV(0.0) + .withKP(0.0) + .withKI(0.0) + .withKD(0.0); + // Limits // 0 is the forward angle on the robot. @@ -45,8 +56,8 @@ public class ShooterConstants { // public static final Angle ANGLE_LIMIT_RIGHT = Degrees.of(180); // 0 is paralell to the ground, 90 is directly up - public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); - public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); + // public static final Angle PITCH_LIMIT_UPPER = Degrees.of(90); + // public static final Angle PITCH_LIMIT_LOWER = Degrees.of(0); // Motor configs // public static final TalonFXConfiguration ANGLE_MOTOR_CONFIG = new TalonFXConfiguration() @@ -61,10 +72,10 @@ public class ShooterConstants { // ); - public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); - public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); - public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); - + public static final CanDevice SHOOTER1_ID = new CanDevice("SHOOTER 1", 22); + public static final CanDevice SHOOTER2_ID = new CanDevice("SHOOTER 2", 23); + public static final CanDevice INDEXER_ID = new CanDevice("INDEXER",24); + public static final TalonFXConfiguration SHOOTER1_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( @@ -73,7 +84,7 @@ public class ShooterConstants { .withStatorCurrentLimitEnable(true) ).withMotorOutput( new MotorOutputConfigs() - .withNeutralMode(NeutralModeValue.Brake) // Must be break because this has to be accurate + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); public static final TalonFXConfiguration SHOOTER2_MOTOR_CONFIG = new TalonFXConfiguration() @@ -86,6 +97,7 @@ public class ShooterConstants { .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means ); + public static final TalonFXConfiguration INDEXER_MOTOR_CONFIG = new TalonFXConfiguration() .withCurrentLimits( new CurrentLimitsConfigs() diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 5f0a1b8..2e6402b 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -36,14 +36,17 @@ public interface ShooterIO { Current motor2Current = Amps.of(0); Current indexerCurrent = Amps.of(0); - LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); + // LinearVelocity motorLinearVelocity = InchesPerSecond.of(0); } // public default void setShooterAngle(ShooterState state, Angle angle) {} // public default void setShooterPitch(ShooterState state, Angle angle) {} - public default void setMotor1Velocity(ShooterState state, AngularVelocity angularVelocity) {} - public default void setMotor2Velocity(ShooterState state, AngularVelocity linearVelocity) {} + public default void setShooterVelocity(ShooterState state, AngularVelocity angularVelocity) {} + // public default void setMotor2Velocity(ShooterState state, AngularVelocity angularVelocity) {} public default void setIndexerVelocity(ShooterState state, AngularVelocity linearVelocity) {} public default void updateInputs(ShooterState state) {} + + + public default void updateGains() {} } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 0a39b1f..4d1b2c9 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -8,12 +8,15 @@ import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; +import frc4388.robot.subsystems.intake.IntakeConstants; +import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { TalonFX m_shooter1Motor; TalonFX m_shooter2Motor; TalonFX m_indexerMotor; + VelocityDutyCycle shooter1Velocity = new VelocityDutyCycle(0); VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0); VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0); @@ -27,14 +30,18 @@ public class ShooterReal implements ShooterIO { m_shooter1Motor = shooter1Motor; m_shooter2Motor = shooter2Motor; m_indexerMotor = indexerMotor; + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); m_indexerMotor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); - m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER1_MOTOR_CONFIG); m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER2_MOTOR_CONFIG); m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_MOTOR_CONFIG); + + shooter1Velocity.Slot = 0; + shooter2Velocity.Slot = 0; + m_indexerVelocity.Slot = 0; } private Angle clampAng(Angle x, Angle min, Angle max){ @@ -83,34 +90,60 @@ public class ShooterReal implements ShooterIO { @Override - public void setMotor1Velocity(ShooterState state, AngularVelocity target) { + public void setShooterVelocity(ShooterState state, AngularVelocity target) { state.motor1TargetVelocity = target; state.motor2TargetVelocity = target; - double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; - m_shooter1Motor.setControl(new VelocityDutyCycle(motorRps)); - m_shooter2Motor.setControl(new VelocityDutyCycle(motorRps)); + AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + + m_shooter1Motor.setControl(shooter1Velocity.withVelocity(motorRps)); + m_shooter2Motor.setControl(shooter2Velocity.withVelocity(motorRps)); } @Override public void setIndexerVelocity(ShooterState state, AngularVelocity target) { state.indexerTargetVelocity = target; - double motorRps = target.in(RotationsPerSecond) / ShooterConstants.INDEXER_GEAR_RATIO; - m_indexerMotor.setControl(new VelocityDutyCycle(motorRps)); + AngularVelocity motorRps = target.div(ShooterConstants.INDEXER_GEAR_RATIO); + m_indexerMotor.setControl(m_indexerVelocity.withVelocity(motorRps)); } + + ConfigurableDouble indexer_kP = new ConfigurableDouble("Indexer KP", 0.2); + ConfigurableDouble indexer_kI = new ConfigurableDouble("Indexer KP", 0); + ConfigurableDouble indexer_kD = new ConfigurableDouble("Indexer KP", 0); + + ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.2); + ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0); + ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0); + @Override public void updateInputs(ShooterState state) { - state.motor1Velocity = m_shooter1Motor.getVelocity().getValue(); - state.motor2Velocity = m_shooter2Motor.getVelocity().getValue(); - state.indexerVelocity = m_indexerMotor.getVelocity().getValue(); + state.motor1Velocity = m_shooter1Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR1_GEAR_RATIO); + state.motor2Velocity = m_shooter2Motor.getVelocity().getValue().times(ShooterConstants.SHOOTERMOTOR2_GEAR_RATIO); + state.indexerVelocity = m_indexerMotor.getVelocity().getValue().times(ShooterConstants.INDEXER_GEAR_RATIO); - state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); + // state.motorLinearVelocity = InchesPerSecond.of(m_shooter1Motor.getVelocity().getValue().in(RotationsPerSecond) * ShooterConstants.FEEDER_INCHES_PER_ROT); state.motor1Current = m_shooter1Motor.getStatorCurrent().getValue(); state.motor2Current = m_shooter2Motor.getStatorCurrent().getValue(); state.indexerCurrent = m_indexerMotor.getStatorCurrent().getValue(); + + } + + @Override + public void updateGains() { + // TEMPORARY PIDs + ShooterConstants.SHOOTER_PID.kP = shooter_kP.get(); + ShooterConstants.SHOOTER_PID.kI = shooter_kI.get(); + ShooterConstants.SHOOTER_PID.kD = shooter_kD.get(); + m_shooter1Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + m_shooter2Motor.getConfigurator().apply(ShooterConstants.SHOOTER_PID); + + ShooterConstants.INDEXER_PID.kP = indexer_kP.get(); + ShooterConstants.INDEXER_PID.kI = indexer_kI.get(); + ShooterConstants.INDEXER_PID.kD = indexer_kD.get(); + m_indexerMotor.getConfigurator().apply(ShooterConstants.INDEXER_PID); } } diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index af2f6fc..4e1139f 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -22,6 +22,7 @@ import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.Angle; +import frc4388.robot.constants.Constants; import frc4388.utility.configurable.ConfigurableDouble; //import edu.wpi.first.units.measure.measure.Distance; import frc4388.utility.status.CanDevice; @@ -31,7 +32,6 @@ import frc4388.utility.structs.Gains; // No mans land // Beware, there be dragons. public final class SwerveDriveConstants { - public static final String CANBUS_NAME = "drivetrain"; public static final double MAX_ROT_SPEED = Math.PI * 2; public static final double AUTO_MAX_ROT_SPEED = 1.5; public static final double MIN_ROT_SPEED = 1.0; @@ -209,7 +209,7 @@ public final class SwerveDriveConstants { } public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants() - .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(CANBUS_NAME); + .withPigeon2Id(IDs.DRIVE_PIGEON.id).withCANBusName(Constants.CANIVORE_CANBUS.getName()); private static final SwerveModuleConstantsFactory ConstantCreator = new SwerveModuleConstantsFactory() // holy verbosity batman. diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java index afd4dd4..f4066f8 100644 --- a/src/main/java/frc4388/utility/status/FaultReporter.java +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -85,13 +85,12 @@ public class FaultReporter { } } - // CAN header System.out.println(CAN_HEADER); - CANBus canBus = new CANBus(Constants.CANBUS_NAME); + // CANBus canBus = new CANBus(); - CANBusStatus canInfo = canBus.getStatus(); + CANBusStatus canInfo = Constants.RIO_CANBUS.getStatus(); System.out.println("CANInfo BusOffCount - " + canInfo.BusOffCount); System.out.println("CANInfo BusUtilization - " + canInfo.BusUtilization);