diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index fe1260d..b3cf6d8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -100,7 +100,7 @@ public static final class Rollers { DESIGNED_SURFACE_SPEED_FT_PER_SEC * 12.0; // public static final double TARGET_ROLLER_RPM = - // (DESIGNED_SURFACE_SPEED_IN_PER_SEC * 60.0) / ROLLER_CIRCUMFERENCE_INCHES; + // (DESIGNED_SURFACE_SPEED_IN_PER_SEC * 60.0) / ROLLER_CIRCUMFERENCE_INCHES; public static final double TARGET_ROLLER_RPM = 2700; public static final double TARGET_ROLLER_RPS = TARGET_ROLLER_RPM / 60.0; public static final double TARGET_MOTOR_RPS = TARGET_ROLLER_RPS * MOTOR_ROTS_PER_ROLLERS_ROT; @@ -705,12 +705,12 @@ public static final class Shooter { public static final class Hood { public static final int HOOD_ID = 12; - public static final double KP = 0.0; + public static final double KP = 2000.0; public static final double KI = 0.0; - public static final double KD = 0.0; - public static final double KV = 0.0; + public static final double KD = 80.0; + public static final double KV = 0.124; public static final double KS = 0.0; - public static final double KG = 0.0; + public static final double KG = 0.4; public static final double STATOR_CURRENT_LIMIT = 40.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; @@ -727,8 +727,8 @@ public static final class Hood { public static final double HOOD_TOLERANCE_DEG = 0.5; - public static final double MIN_HOOD_POSITION = 3.8; - public static final double MAX_HOOD_POSITION = 18.369; + public static final double MIN_HOOD_ANGLE = 3.8; + public static final double MAX_HOOD_ANGLE = 18.369; public static double ENCODER_OFFSET = 0.296; // TODO public static final int ENCODER_PORT = 18; @@ -748,20 +748,20 @@ public static final class Rollers { public static final int WARMUP_2_ID = 9; public static final int WARMUP_3_ID = 11; - public static final double KP = 0.0; + public static final double KP = 0.85; public static final double KI = 0.0; public static final double KD = 0.0; - public static final double KV = 0.0; + public static final double KV = 0.124; public static final double KS = 0.0; public static final double STATOR_CURRENT_LIMIT = 120.0; public static final double SUPPLY_CURRENT_LIMIT = 40.0; public static final double MOTOR_ROTS_PER_WHEEL_ROT = 15.0 / 12.0; + public static final double WHEEL_ROTS_PER_MOTOR_ROT = 1.0 / MOTOR_ROTS_PER_WHEEL_ROT; public static final double SHOOTER_WHEEL_DIAMETER = 3.0; } - } public static class OI { @@ -827,21 +827,21 @@ public static class Landmarks { new Pose2d(1.6428194046020508, 4.1721110343933105, new Rotation2d(Math.PI)); // public static Pose2d RED_LEFT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(10.908942222595215, 2.54630184173584), - // new Rotation2d(1.5707963267948966)); + // new Pose2d( + // new Translation2d(10.908942222595215, 2.54630184173584), + // new Rotation2d(1.5707963267948966)); // public static Pose2d RED_RIGHT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(10.921140670776367, 5.613290309906006), - // new Rotation2d(-1.5707963267948966)); + // new Pose2d( + // new Translation2d(10.921140670776367, 5.613290309906006), + // new Rotation2d(-1.5707963267948966)); // public static Pose2d BLUE_LEFT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(5.6342058181762695, 5.505496978759766), - // new Rotation2d(-1.5649821399611368)); + // new Pose2d( + // new Translation2d(5.6342058181762695, 5.505496978759766), + // new Rotation2d(-1.5649821399611368)); // public static Pose2d BLUE_RIGHT_INTAKE_TO_BUMP = - // new Pose2d( - // new Translation2d(5.624283313751221, 2.4593770503997803), - // new Rotation2d(1.57873264137917)); + // new Pose2d( + // new Translation2d(5.624283313751221, 2.4593770503997803), + // new Rotation2d(1.57873264137917)); public static Pose2d RED_LEFT_INTAKE_TO_BUMP = new Pose2d( new Translation2d(10.908942222595215, 2.2220301628112793), diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 565cfc0..b0b194a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -4,15 +4,13 @@ package frc.robot; -import choreo.auto.AutoChooser; // * KEEP FOR WIN COMMAND TESTING // import edu.wpi.first.math.geometry.Pose2d; // import edu.wpi.first.math.geometry.Rotation2d; // import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; -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.button.CommandXboxController; // * KEEP FOR WIN COMMAND TESTING import frc.robot.commands.SwerveCommands.SwerveJoystickCommand; @@ -22,88 +20,79 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.IntakeVisionDetection; -import frc.robot.subsystems.LEDSubsystem; import frc.robot.subsystems.ShooterSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.util.CustomController; -import frc.robot.util.MiscUtils; import frc.robot.util.VisionUtils; import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; public class RobotContainer { - private BooleanSupplier redside = RobotContainer::isRedAlliance; - - // private Field2d field = new Field2d(); - private final Telemetry logger = - new Telemetry(Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); - - private final CommandXboxController joystick = new CommandXboxController(0); - private final CustomController secondController = new CustomController(4); - - public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - - public final HopperSubsystem hopperSubsystem = - Constants.hopperOnRobot ? new HopperSubsystem() : null; - public final IntakeSubsystem intakeSubsystem = - Constants.intakeOnRobot ? new IntakeSubsystem() : null; - - -// private final AutoRoutines autoRoutines; -// private final AutoChooser autoChooser; - - public final VisionSubsystem visionFrontRight = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_RIGHT_CAM, drivetrain) - : null; - public final VisionSubsystem visionFrontLeft = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_LEFT_CAM, drivetrain) - : null; - public final VisionSubsystem visionRearRight = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_RIGHT_CAM, drivetrain) - : null; - public final VisionSubsystem visionRearLeft = - Constants.visionOnRobot - ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_LEFT_CAM, drivetrain) - : null; - - public final FuelGaugeDetection visionFuelGauge = - Constants.fuelGaugeOnRobot - ? new FuelGaugeDetection(Constants.FuelGaugeDetection.FuelGaugeCamera.FUEL_GAUGE_CAM) - : null; - public final IntakeVisionDetection visionIntake = - Constants.intakeVisionOnRobot - ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM) - : null; - - // * KEEP FOR INTERMAP TESTING - // private double hoodAngle = 18.369; - // private double shooterSpeed = 58.0; - - public RobotContainer() { - // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, drivetrain, redside); - // autoChooser = autoRoutines.getAutoChooser(); - // SmartDashboard.putData("Auto Chooser", autoChooser); - // SmartDashboard.putData("Elastic/Field2d", field); - configureBindings(); - } + private BooleanSupplier redside = RobotContainer::isRedAlliance; + + // private Field2d field = new Field2d(); + private final Telemetry logger = new Telemetry(Constants.Swerve.PHYSICAL_MAX_SPEED_METERS_PER_SECOND); + + private final CommandXboxController joystick = new CommandXboxController(0); + private final CustomController secondController = new CustomController(4); + + public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain(); - // public void doTelemetry() { - // logger.telemeterize(drivetrain.getCurrentState()); + public final HopperSubsystem hopperSubsystem = Constants.hopperOnRobot ? new HopperSubsystem() : null; + public final IntakeSubsystem intakeSubsystem = Constants.intakeOnRobot ? new IntakeSubsystem() : null; - // String commandName = "nah"; + public final ShooterSubsystem shooterSubsystem = new ShooterSubsystem(); - // if (drivetrain.getCurrentCommand() != null) { - // commandName = drivetrain.getCurrentCommand().getName(); - // } - // DogLog.log("Robot/SwerveDriveCommand", commandName); - // } + // private final AutoRoutines autoRoutines; + // private final AutoChooser autoChooser; - // public CommandSwerveDrivetrain getDrivetrain() { - // return drivetrain; - // } + public final VisionSubsystem visionFrontRight = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_RIGHT_CAM, drivetrain) + : null; + public final VisionSubsystem visionFrontLeft = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.FRONT_LEFT_CAM, drivetrain) + : null; + public final VisionSubsystem visionRearRight = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_RIGHT_CAM, drivetrain) + : null; + public final VisionSubsystem visionRearLeft = Constants.visionOnRobot + ? new VisionSubsystem(Constants.Vision.VisionCamera.REAR_LEFT_CAM, drivetrain) + : null; + + public final FuelGaugeDetection visionFuelGauge = Constants.fuelGaugeOnRobot + ? new FuelGaugeDetection(Constants.FuelGaugeDetection.FuelGaugeCamera.FUEL_GAUGE_CAM) + : null; + public final IntakeVisionDetection visionIntake = Constants.intakeVisionOnRobot + ? new IntakeVisionDetection(Constants.IntakeVision.IntakeVisionCamera.INTAKE_CAM) + : null; + + // * KEEP FOR INTERMAP TESTING + // private double hoodAngle = 18.369; + // private double shooterSpeed = 58.0; + + public RobotContainer() { + // autoRoutines = new AutoRoutines(intakeSubsystem, lebron, hopperSubsystem, + // drivetrain, redside); + // autoChooser = autoRoutines.getAutoChooser(); + // SmartDashboard.putData("Auto Chooser", autoChooser); + // SmartDashboard.putData("Elastic/Field2d", field); + configureBindings(); + } + + // public void doTelemetry() { + // logger.telemeterize(drivetrain.getCurrentState()); + + // String commandName = "nah"; + + // if (drivetrain.getCurrentCommand() != null) { + // commandName = drivetrain.getCurrentCommand().getName(); + // } + // DogLog.log("Robot/SwerveDriveCommand", commandName); + // } + + // public CommandSwerveDrivetrain getDrivetrain() { + // return drivetrain; + // } private void configureBindings() { // Swerve @@ -144,33 +133,33 @@ private void configureBindings() { // * KEEP FOR WIN COMMAND // joystick - // .a() - // .whileTrue( - // new DriveToAndShoot( - // () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())), - // lebron, - // intakeSubsystem, - // hopperSubsystem, - // drivetrain, - // redside)); + // .a() + // .whileTrue( + // new DriveToAndShoot( + // () -> (new Pose2d(new Translation2d(3.0, 5.0), new Rotation2d())), + // lebron, + // intakeSubsystem, + // hopperSubsystem, + // drivetrain, + // redside)); // joystick - // .a() - // .whileTrue( - // new DriveToPose( - // drivetrain, - // () -> - // new Pose2d( - // new Translation2d(2.462480068206787, 2.26101016998291), new + // .a() + // .whileTrue( + // new DriveToPose( + // drivetrain, + // () -> + // new Pose2d( + // new Translation2d(2.462480068206787, 2.26101016998291), new // Rotation2d()))); // * KEEP FOR INTERMAP TESTING // joystick - // .rightTrigger() - // .whileTrue( - // lebron - // .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle) - // .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen + // .rightTrigger() + // .whileTrue( + // lebron + // .shootAtSpeedHoodCommand(() -> shooterSpeed, () -> hoodAngle) + // .alongWith(Commands.waitUntil(lebron::isAtSpeed).andThen // // (hopperSubsystem.runHopperUntilInterruptedCommand().alongWith(Commands.waitSeconds(0.4).andThen(intakeSubsystem.powerRetractRollersCommand()))))); @@ -184,36 +173,74 @@ private void configureBindings() { // joystick.y().onTrue(new InstantCommand(() -> hoodAngle-=0.2)); // joystick.a().onTrue(new InstantCommand(() -> shooterSpeed+=0.5)); // joystick.b().onTrue(new InstantCommand(() -> shooterSpeed-=0.5)); - } - public static boolean isRedAlliance() { - return DriverStation.getAlliance().isEmpty() - ? false - : DriverStation.getAlliance().get() == Alliance.Red; - } + shooterSubsystem.setDefaultCommand(shooterSubsystem.runOnce(shooterSubsystem::stopShooter)); + // joystick + // .rightBumper() + // .whileTrue( + // new ShootBasicHood( + // 44.2, + // Constants.Shooter.Hood.MIN_HOOD_ANGLE, + // shooterSubsystem, + // intakeSubsystem, + // hopperSubsystem)); + // joystick + // .rightTrigger() + // .whileTrue( + // new ShootBasicHood( + // 58.2, + // Constants.Shooter.Hood.MAX_HOOD_ANGLE, + // shooterSubsystem, + // intakeSubsystem, + // hopperSubsystem)); - public void visionPeriodic() { - VisionUtils.visionPeriodic( - visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft, drivetrain); + joystick + .rightBumper() + .whileTrue( + shooterSubsystem + .shootWithHood(44.2, Constants.Shooter.Hood.MIN_HOOD_ANGLE) + .alongWith( + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); + joystick + .rightTrigger() + .whileTrue( + shooterSubsystem + .shootWithHood(58.2, Constants.Shooter.Hood.MAX_HOOD_ANGLE) + .alongWith( + Commands.waitUntil(shooterSubsystem::isSho + oterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand()))); } - public void doTelemetry() { - logger.telemeterize(drivetrain.getCurrentState()); + public static boolean isRedAlliance() { + return DriverStation.getAlliance().isEmpty() + ? false + : DriverStation.getAlliance().get() == Alliance.Red; + } + + public void visionPeriodic() { + VisionUtils.visionPeriodic( + visionFrontRight, visionFrontLeft, visionRearRight, visionRearLeft, drivetrain); + } + + public void doTelemetry() { + logger.telemeterize(drivetrain.getCurrentState()); - String commandName = "nah"; + String commandName = "nah"; - if (drivetrain.getCurrentCommand() != null) { - commandName = drivetrain.getCurrentCommand().getName(); + if (drivetrain.getCurrentCommand() != null) { + commandName = drivetrain.getCurrentCommand().getName(); + } } - } - public boolean inAllianceSide() { - return redside.getAsBoolean() - ? drivetrain.getPose().getX() > Constants.Landmarks.RED_HUB.getX() - : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); - } + public boolean inAllianceSide() { + return redside.getAsBoolean() + ? drivetrain.getPose().getX() > Constants.Landmarks.RED_HUB.getX() + : drivetrain.getPose().getX() < Constants.Landmarks.BLUE_HUB.getX(); + } -// public Command getAutonomousCommand() { -// return autoChooser.selectedCommand(); -// } + // public Command getAutonomousCommand() { + // return autoChooser.selectedCommand(); + // } } diff --git a/src/main/java/frc/robot/commandGroups/ShootBasicHood.java b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java new file mode 100644 index 0000000..500ad32 --- /dev/null +++ b/src/main/java/frc/robot/commandGroups/ShootBasicHood.java @@ -0,0 +1,21 @@ +package frc.robot.commandGroups; + +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.subsystems.IntakeSubsystem; +import frc.robot.subsystems.ShooterSubsystem; + +public class ShootBasicHood extends ParallelCommandGroup { + public ShootBasicHood( + double speed, + double angle, + ShooterSubsystem shooterSubsystem, + IntakeSubsystem intakeSubsystem, + HopperSubsystem hopperSubsystem) { + addCommands( + shooterSubsystem.shootWithHood(speed, angle), + Commands.waitUntil(shooterSubsystem::isShooterAtSpeed) + .andThen(hopperSubsystem.runHopperUntilInterruptedCommand())); + } +} diff --git a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java index 09fce43..5eeedd7 100644 --- a/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java +++ b/src/main/java/frc/robot/commands/SwerveCommands/SwerveJoystickCommand.java @@ -3,8 +3,6 @@ import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; import dev.doglog.DogLog; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; import frc.robot.subsystems.CommandSwerveDrivetrain; @@ -131,8 +129,7 @@ public void execute() { ? fieldCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn) : robotCentricDrive.withVelocityX(x).withVelocityY(y).withRotationalRate(turn); - boolean isPointed = - true; + boolean isPointed = true; boolean intentionallyStationary = (xSpdFunction.getAsDouble() < 0.1) && (ySpdFunction.getAsDouble() < 0.1) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index ee6829c..97f6e7f 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -1,88 +1,202 @@ package frc.robot.subsystems; import static edu.wpi.first.units.Units.Rotations; + import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.configs.CANcoderConfiguration; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.FeedbackConfigs; import com.ctre.phoenix6.configs.MagnetSensorConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.controls.PositionVoltage; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.signals.FeedbackSensorSourceValue; +import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorAlignmentValue; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.signals.SensorDirectionValue; - +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.util.LoggedTalonFX; - public class ShooterSubsystem extends SubsystemBase { - - private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; - private final CANcoder hoodEncoder; - - - /* - * make instance variables for necessary closed loop control requests: VelocityVoltage, PositionVoltage. Initialize them with value 0.0. - * make instance variables of type double for the target roller speed and target hood angle - */ - - public ShooterSubsystem() { - // The canbus is a communication system that can connect devices like the roboRIO, pdh, and motors. - CANBus canbus = Constants.Swerve.CAN_BUS; - - // LoggedTalonFX is our version of the existing TalonFX class, which automatically logs some motor information - warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); - warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); - warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); - - shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so later we can just reference the `shooter` variable - - // Initialize the hood motor just like the warmup motors. You can find the necessary ID in Constants. - - // Create a variable of type Slot0Configs called rollersSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - - // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and initialize it with the stator and supply limits found in Constants - // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, and initialize it to treat clockwise as positive, and neutral mode as coast - - TalonFXConfiguration rollersConfig = new TalonFXConfiguration(); - // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` to the configs you just created. - - warmup1.getConfigurator().apply(rollersConfig); - // I just applied the configuration that you just created to warmup1. Apply it to warmup2 and warmup3 as well. - - Follower follower = new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); - // use the `setControl` method of warmup1 and 2 to configure them to use this follower configuration. - - hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); - - // Create a variable of type Slot0Configs called hoodSlot0Configs, and initialize it with the pid and feedforward gains found in Constants - - // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and initialize it with the stator and supply limits found in Constants - - // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and initialize it to treat counterclockwise as positive, and neutral mode as coast - - FeedbackConfigs hoodFeedbackConfigs = + private final LoggedTalonFX warmup1, warmup2, warmup3, shooter, hood; + private final CANcoder hoodEncoder; + + /* + * make instance variables for necessary closed loop control requests: + * VelocityVoltage, PositionVoltage. Initialize them with value 0.0. + * make instance variables of type double for the target roller speed and target + * hood angle + */ + + private final VelocityVoltage m_velocityVoltageRequest = new VelocityVoltage(0.0); + private final PositionVoltage m_positionVoltageRequest = new PositionVoltage(0.0); + private double target_roller_speed = 0.0, target_hood_angle = 0.0; + + public ShooterSubsystem() { + // The canbus is a communication system that can connect devices like the + // roboRIO, pdh, and motors. + CANBus canbus = Constants.Swerve.CAN_BUS; + + // LoggedTalonFX is our version of the existing TalonFX class, which + // automatically logs some motor information + warmup1 = new LoggedTalonFX("ShooterWarmup1", Constants.Shooter.Rollers.WARMUP_1_ID, canbus); + warmup2 = new LoggedTalonFX("ShooterWarmup2", Constants.Shooter.Rollers.WARMUP_2_ID, canbus); + warmup3 = new LoggedTalonFX("ShooterWarmup3", Constants.Shooter.Rollers.WARMUP_3_ID, canbus); + + shooter = warmup3; // warmup1 and warmup2 will be set as "followers" of the "lead" warmup3, so + // later we can just reference the `shooter` variable + + // Initialize the hood motor just like the warmup motors. You can find the + // necessary ID in Constants. + hood = new LoggedTalonFX("Hood", Constants.Shooter.Hood.HOOD_ID, canbus); + + // Create a variable of type Slot0Configs called rollersSlot0Configs, and + // initialize it with the pid and feedforward gains found in Constants + Slot0Configs rollersSlot0Configs = + new Slot0Configs() + .withKP(Constants.Shooter.Rollers.KP) + .withKI(Constants.Shooter.Rollers.KI) + .withKD(Constants.Shooter.Rollers.KD) + .withKV(Constants.Shooter.Rollers.KV) + .withKS(Constants.Shooter.Rollers.KS); + + // Create a variable of type CurrentLimitsConfigs called rollersClConfigs, and + // initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs rollersClConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Shooter.Rollers.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Shooter.Rollers.SUPPLY_CURRENT_LIMIT); + + // Create a variable of type MotorOutputConifigs called rollersOutputConfigs, + // and initialize it to treat clockwise as positive, and neutral mode as coast + MotorOutputConfigs rollersOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.Clockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + // Set the Slot0, CurrentLimits, and MotorOutput parameters of `rollersConfig` + // to the configs you just created. + TalonFXConfiguration rollersConfig = + new TalonFXConfiguration() + .withSlot0(rollersSlot0Configs) + .withCurrentLimits(rollersClConfigs) + .withMotorOutput(rollersOutputConfigs); + + warmup1.getConfigurator().apply(rollersConfig); + // I just applied the configuration that you just created to warmup1. Apply it + // to warmup2 and warmup3 as well. + warmup2.getConfigurator().apply(rollersConfig); + warmup3.getConfigurator().apply(rollersConfig); + + Follower follower = + new Follower(Constants.Shooter.Rollers.WARMUP_3_ID, MotorAlignmentValue.Aligned); + // use the `setControl` method of warmup1 and 2 to configure them to use this + // follower configuration. + warmup1.setControl(follower); + warmup2.setControl(follower); + + hoodEncoder = new CANcoder(Constants.Shooter.Hood.ENCODER_PORT, Constants.Swerve.CAN_BUS); + + // Create a variable of type Slot0Configs called hoodSlot0Configs, and + // initialize it with the pid and feedforward gains found in Constants + Slot0Configs hoodSlot0Configs = + new Slot0Configs() + .withKP(Constants.Shooter.Hood.KP) + .withKI(Constants.Shooter.Hood.KI) + .withKD(Constants.Shooter.Hood.KD) + .withKV(Constants.Shooter.Hood.KV) + .withKS(Constants.Shooter.Hood.KS) + .withKG(Constants.Shooter.Hood.KG); + + // Create a variable of type CurrentLimitsConfigs called hoodClConfigs, and + // initialize it with the stator and supply limits found in Constants + CurrentLimitsConfigs hoodClConfigs = + new CurrentLimitsConfigs() + .withStatorCurrentLimit(Constants.Shooter.Hood.STATOR_CURRENT_LIMIT) + .withSupplyCurrentLimit(Constants.Shooter.Hood.SUPPLY_CURRENT_LIMIT); + + // Create a variable of type MotorOutputConifigs called hoodOutputConfigs, and + // initialize it to treat counterclockwise as positive, and neutral mode as + // coast + MotorOutputConfigs hoodOutputConfigs = + new MotorOutputConfigs() + .withInverted(InvertedValue.CounterClockwise_Positive) + .withNeutralMode(NeutralModeValue.Coast); + + FeedbackConfigs hoodFeedbackConfigs = new FeedbackConfigs() .withFeedbackRemoteSensorID(hoodEncoder.getDeviceID()) .withFeedbackSensorSource(FeedbackSensorSourceValue.FusedCANcoder) .withSensorToMechanismRatio(Constants.Shooter.Hood.ENCODER_ROTS_PER_HOOD_ROT) .withRotorToSensorRatio(Constants.Shooter.Hood.MOTOR_ROTS_PER_ENCODER_ROT); - // Create a TalonFXConfiguration called hoodConfig - // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the device configs above + // Create a TalonFXConfiguration called hoodConfig + // Set the Slot0, CurrentLimits, MotorOutput, and Feedback parameters to the + // device configs above + TalonFXConfiguration hoodConfig = + new TalonFXConfiguration() + .withSlot0(hoodSlot0Configs) + .withCurrentLimits(hoodClConfigs) + .withMotorOutput(hoodOutputConfigs) + .withFeedback(hoodFeedbackConfigs); - // Apply the created configuration to the hood motor + // Apply the created configuration to the hood motor + hood.getConfigurator().apply(hoodConfig); - MagnetSensorConfigs hoodCANcoderConfig = + MagnetSensorConfigs hoodCANcoderConfig = new CANcoderConfiguration() .MagnetSensor.withAbsoluteSensorDiscontinuityPoint(Rotations.of(1)) .withSensorDirection(SensorDirectionValue.CounterClockwise_Positive) .withMagnetOffset(Rotations.of(Constants.Shooter.Hood.ENCODER_OFFSET)); - // Apply this config to the `hoodEncoder`. Notice how applying a device configuration is similar between motors and other devices - - } + // Apply this config to the `hoodEncoder`. Notice how applying a device + // configuration is similar between motors and other devices + hoodEncoder.getConfigurator().apply(hoodCANcoderConfig); + } + + public void setHoodAngle(double degrees) { + target_hood_angle = degrees; + hood.setControl( + m_positionVoltageRequest.withPosition( + MathUtil.clamp( + target_hood_angle / 360.0, + Constants.Shooter.Hood.MIN_HOOD_ANGLE, + Constants.Shooter.Hood.MAX_HOOD_ANGLE))); + } + + public void setShooterSpeed(double velocityRps) { + target_roller_speed = velocityRps; + shooter.setControl( + m_velocityVoltageRequest.withVelocity( + velocityRps * Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT)); + } + + public void stopShooter() { + target_roller_speed = 0.0; + shooter.stopMotor(); + } + + public boolean isShooterAtSpeed() { + double currentVelocity = + shooter.getCachedVelocityRps() / Constants.Shooter.Rollers.MOTOR_ROTS_PER_WHEEL_ROT; + return Math.abs(currentVelocity - target_roller_speed) <= 1.0; + } + + public Command shootWithHood(double targetShooterSpeed, double targetHoodAngle) { + return runEnd( + () -> { + setShooterSpeed(targetShooterSpeed); + setHoodAngle(targetHoodAngle); + }, + this::stopShooter); + } }