create shooter and indexer

src/main/java/frc/robot/Constants.java

@@ -0,0 +1,9 @@
+package frc.robot;
+
+import com.ctre.phoenix6.CANBus;
+
+public class Constants {
+    public static final CANBus canBus = new CANBus("GTX7130");
+    public static final double goalPoseX = 4.625;
+    public static final double goalPoseY = 4.034;
+}

src/main/java/frc/robot/RobotContainer.java

@@ -21,101 +21,110 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+import frc.robot.commands.Shoot;
 import frc.robot.commands.SwerveWithAim;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.indexer.IndexerConstants;
+import frc.robot.subsystems.indexer.IndexerSubsystem;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
 
 public class RobotContainer {
-    private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
-    private double MaxAngularRate = RotationsPerSecond.of(0.95).in(RadiansPerSecond); // 3/4 of a rotation per second max angular velocity
-
-    /* Setting up bindings for necessary control of the swerve drive platform */
-    private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
-    private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
-    private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric()
-            .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
-
-    private final Telemetry logger = new Telemetry(MaxSpeed);
-
-    private final CommandXboxController joystick = new CommandXboxController(0);
-
-    public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-
-    // private final Intake intake = new Intake();
-
-    /* Path follower */
-    private final SendableChooser<Command> autoChooser;
-
-    public RobotContainer() {
-        autoChooser = AutoBuilder.buildAutoChooser("Tests");
-        SmartDashboard.putData("Auto Mode", autoChooser);
-
-        configureBindings();
-
-        // Warmup PathPlanner to avoid Java pauses
-        FollowPathCommand.warmupCommand().schedule();
-    }
-
-    private void configureBindings() {
-        // Note that X is defined as forward according to WPILib convention,
-        // and Y is defined as to the left according to WPILib convention.
-        drivetrain.setDefaultCommand(
-            // Drivetrain will execute this command periodically
-            new SwerveWithAim(
-                drivetrain,
-                () -> -joystick.getLeftY() * MaxSpeed, // Drive forward with negative Y (forward)
-                () -> -joystick.getLeftX() * MaxSpeed, // Drive left with negative X (left)
-                () -> -joystick.getRightX() * MaxAngularRate, // Drive counterclockwise with negative X (left)
-                joystick.getHID()::getRightBumperButton
-            )
-        );
-
-        // joystick.rightTrigger(0.2).whileTrue(
-        //     Commands.runEnd(() -> intake.setVoltage(-12), () -> intake.setVoltage(0), intake)
-        // );
-
-
-        // Idle while the robot is disabled. This ensures the configured
-        // neutral mode is applied to the drive motors while disabled.
-        final var idle = new SwerveRequest.Idle();
-        RobotModeTriggers.disabled().whileTrue(
-            drivetrain.applyRequest(() -> idle).ignoringDisable(true)
-        );
-
-        joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
-        joystick.b().whileTrue(drivetrain.applyRequest(() ->
-            point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))
-        ));
-
-        joystick.povUp().whileTrue(drivetrain.applyRequest(() ->
-            forwardStraight.withVelocityX(0.5).withVelocityY(0))
-        );
-        joystick.povDown().whileTrue(drivetrain.applyRequest(() ->
-            forwardStraight.withVelocityX(-0.5).withVelocityY(0))
-        );
-
-        // Run SysId routines when holding back/start and X/Y.
-        // Note that each routine should be run exactly once in a single log.
-        joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
-        joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
-        joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
-        joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
-
-        // Reset the field-centric heading on left bumper press.
-        joystick.leftBumper().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
-
-        drivetrain.registerTelemetry(logger::telemeterize);
-    }
-
-    public Command getAutonomousCommand() {
-        /* Run the path selected from the auto chooser */
-        return autoChooser.getSelected();
-    }
-
-    public Command rumble(double strength, double timeSeconds) {
-        return Commands.runEnd(
-            () -> joystick.setRumble(RumbleType.kBothRumble, strength),
-            () -> joystick.setRumble(RumbleType.kBothRumble, 0)
-        ).withTimeout(timeSeconds);
-    }
+        private double MaxSpeed = 1.0 * TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired
+                                                                                            // top
+                                                                                            // speed
+        private double MaxAngularRate = RotationsPerSecond.of(0.95).in(RadiansPerSecond); // 3/4 of a rotation per
+                                                                                          // second
+                                                                                          // max angular velocity
+
+        /* Setting up bindings for necessary control of the swerve drive platform */
+        private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+        private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+        private final SwerveRequest.RobotCentric forwardStraight = new SwerveRequest.RobotCentric()
+                        .withDriveRequestType(DriveRequestType.OpenLoopVoltage);
+
+        private final Telemetry logger = new Telemetry(MaxSpeed);
+
+        private final CommandXboxController joystick = new CommandXboxController(0);
+
+        public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+
+        // private final Intake intake = new Intake();
+
+        /* Path follower */
+        private final SendableChooser<Command> autoChooser;
+
+        private final ShooterSubsystem shooterSubsystem = new ShooterSubsystem();
+        private final IndexerSubsystem indexerSubsystem = new IndexerSubsystem();
+
+        public RobotContainer() {
+                autoChooser = AutoBuilder.buildAutoChooser("Tests");
+                SmartDashboard.putData("Auto Mode", autoChooser);
+
+                configureBindings();
+
+                // Warmup PathPlanner to avoid Java pauses
+                FollowPathCommand.warmupCommand().schedule();
+                indexerSubsystem.setDefaultCommand(indexerSubsystem.run(0, 0));
+        }
+
+        private void configureBindings() {
+                // Note that X is defined as forward according to WPILib convention,
+                // and Y is defined as to the left according to WPILib convention.
+                drivetrain.setDefaultCommand(
+                                // Drivetrain will execute this command periodically
+                                new SwerveWithAim(
+                                                drivetrain,
+                                                () -> -joystick.getLeftY() * MaxSpeed, // Drive forward with negative Y
+                                                                                       // (forward)
+                                                () -> -joystick.getLeftX() * MaxSpeed, // Drive left with negative X
+                                                                                       // (left)
+                                                () -> -joystick.getRightX() * MaxAngularRate, // Drive counterclockwise
+                                                                                              // with negative X (left)
+                                                joystick.getHID()::getRightBumperButton));
+
+                // joystick.rightTrigger(0.2).whileTrue(
+                // Commands.runEnd(() -> intake.setVoltage(-12), () -> intake.setVoltage(0),
+                // intake)
+                // );
+
+                // Idle while the robot is disabled. This ensures the configured
+                // neutral mode is applied to the drive motors while disabled.
+                final var idle = new SwerveRequest.Idle();
+                RobotModeTriggers.disabled().whileTrue(drivetrain.applyRequest(() -> idle).ignoringDisable(true));
+
+                joystick.a().whileTrue(drivetrain.applyRequest(() -> brake));
+                joystick.b().whileTrue(drivetrain.applyRequest(() -> point.withModuleDirection(new Rotation2d(-joystick.getLeftY(), -joystick.getLeftX()))));
+
+                joystick.povUp().whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(0.5).withVelocityY(0)));
+                joystick.povDown().whileTrue(drivetrain.applyRequest(() -> forwardStraight.withVelocityX(-0.5).withVelocityY(0)));
+
+                // Run SysId routines when holding back/start and X/Y.
+                // Note that each routine should be run exactly once in a single log.
+                joystick.back().and(joystick.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
+                joystick.back().and(joystick.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
+                joystick.start().and(joystick.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
+                joystick.start().and(joystick.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
+
+                // Reset the field-centric heading on left bumper press.
+                joystick.leftBumper().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
+
+                drivetrain.registerTelemetry(logger::telemeterize);
+
+                joystick.rightTrigger().whileTrue(new Shoot(shooterSubsystem, () -> drivetrain.getState().Pose));
+                joystick.rightBumper().whileTrue(indexerSubsystem.run(IndexerConstants.indexerUpSpeed,IndexerConstants.indexerDownSpeed));
+
+                shooterSubsystem.registerTelemetry(logger::telemeterizeShooter);
+        }
+
+        public Command getAutonomousCommand() {
+                /* Run the path selected from the auto chooser */
+                return autoChooser.getSelected();
+        }
+
+        public Command rumble(double strength, double timeSeconds) {
+                return Commands.runEnd(
+                                () -> joystick.setRumble(RumbleType.kBothRumble, strength),
+                                () -> joystick.setRumble(RumbleType.kBothRumble, 0)).withTimeout(timeSeconds);
+        }
 }

src/main/java/frc/robot/Telemetry.java

@@ -9,6 +9,7 @@
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.networktables.DoubleArrayPublisher;
 import edu.wpi.first.networktables.DoublePublisher;
 import edu.wpi.first.networktables.NetworkTable;
@@ -57,6 +58,9 @@ public Telemetry(double maxSpeed) {
     private final StructPublisher<Pose3d> shooterPose = componentStateTable.getStructTopic("ShooterPose", Pose3d.struct).publish();
     private final StructPublisher<Pose3d> climberPose = componentStateTable.getStructTopic("ClimberPose", Pose3d.struct).publish();
 
+    private final NetworkTable shooterStateTable = inst.getTable("ShooterState");
+    private final DoublePublisher shooterAngle = shooterStateTable.getDoubleTopic("ShooterAngle").publish();
+
     /* Robot pose for field positioning */
     private final NetworkTable table = inst.getTable("Pose");
     private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
@@ -126,9 +130,10 @@ public void telemeterize(SwerveDriveState state) {
     }
 
     /** Accept the shooter angle and telemeterize it to SmartDashboard */
-    public void telemeterizeShooter(double pitchRadius) {
+    public void telemeterizeShooter(double pitchRotations) {
         // non-zero values are model asset offset for ascope
-        shooterPose.set(new Pose3d(-0.085, 0.0, 0.507, new Rotation3d(0.0, pitchRadius - 0.62, 0.0)));
+        shooterPose.set(new Pose3d(-0.085, 0.0, 0.507, new Rotation3d(0.0, Units.rotationsToRadians(pitchRotations) - 0.62, 0.0)));
+        shooterAngle.set(pitchRotations);
     }
 
     /** Accept the shooter angle and telemeterize it to SmartDashboard*/

src/main/java/frc/robot/commands/Shoot.java

@@ -0,0 +1,63 @@
+package frc.robot.commands;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.FSLib.util.AllianceFlipUtil;
+import frc.robot.Constants;
+import frc.robot.subsystems.shooter.ShooterSubsystem;
+
+public class Shoot extends Command {
+    private final ShooterSubsystem shooterSubsystem;
+    private final Supplier<Pose2d> robotPoseSupplier;
+    private final InterpolatingDoubleTreeMap speedUpMap = new InterpolatingDoubleTreeMap();
+    private final InterpolatingDoubleTreeMap speedDownMap = new InterpolatingDoubleTreeMap();
+    private final InterpolatingDoubleTreeMap angleMap = new InterpolatingDoubleTreeMap();
+
+    public Shoot(
+        ShooterSubsystem shooterSubsystem,
+        Supplier<Pose2d> robotPoseSupplier) {
+            this.shooterSubsystem = shooterSubsystem;
+            this.robotPoseSupplier = robotPoseSupplier;
+            addRequirements(shooterSubsystem);
+
+            speedUpMap.put(0.0, 0.0);// rps
+            speedUpMap.put(1.0, -4.0);// rps
+            speedDownMap.put(0.0, 0.0);// rps
+            speedDownMap.put(0.0, 0.0);// rps
+            angleMap.put(0.0, 0.0);
+            angleMap.put(0.0, 0.0);
+    }
+
+    @Override
+    public void initialize() {
+    }
+
+    @Override
+    public void execute() {
+        Pose2d currentPose = robotPoseSupplier.get();
+        Translation2d goalPose = new Translation2d(Constants.goalPoseX, Constants.goalPoseY);
+        Translation2d trueTarget = AllianceFlipUtil.flip(goalPose);
+        double distance = currentPose.getTranslation().getDistance(trueTarget);
+        // some calculations here
+        double shooterUpSpeed = speedUpMap.get(distance); // replace with actual calculation
+        shooterSubsystem.setShooterUpSpeed(shooterUpSpeed);
+        double ShooterDownSpeed = speedDownMap.get(distance);
+        shooterSubsystem.setShooterDownSpeed(ShooterDownSpeed);
+        double shooterAngleTarget = angleMap.get(distance);
+        shooterSubsystem.setShooterAngle(shooterAngleTarget);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        shooterSubsystem.stop();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+    }
+}

src/main/java/frc/robot/subsystems/indexer/IndexerConstants.java

@@ -0,0 +1,8 @@
+package frc.robot.subsystems.indexer;
+
+public class IndexerConstants {
+    public static final int indexerUpID = 44;
+    public static final int indexerDownID = 43;
+    public static final double indexerUpSpeed = 1.0;
+    public static final double indexerDownSpeed = -0.1;
+}

src/main/java/frc/robot/subsystems/indexer/IndexerSubsystem.java

@@ -0,0 +1,51 @@
+package frc.robot.subsystems.indexer;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+
+public class IndexerSubsystem extends SubsystemBase {
+    TalonFX indexerUp = new TalonFX(IndexerConstants.indexerUpID, Constants.canBus);
+    TalonFX indexerDown = new TalonFX(IndexerConstants.indexerDownID, Constants.canBus);
+
+    public IndexerSubsystem() {
+        TalonFXConfiguration indexerUpConfig = new TalonFXConfiguration();
+        indexerUpConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+        indexerUpConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+        indexerUpConfig.CurrentLimits.SupplyCurrentLimit = 70;
+        indexerUp.getConfigurator().apply(indexerUpConfig);
+
+        TalonFXConfiguration indexerDownConfig = new TalonFXConfiguration();
+        indexerDownConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+        indexerDownConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+        indexerDownConfig.CurrentLimits.SupplyCurrentLimit = 70;
+        indexerDown.getConfigurator().apply(indexerDownConfig);
+    }
+
+    public void setIndexerUpSpeed(double speedUp) {
+        indexerUp.set(speedUp);
+    }
+
+    public void setIndexerDownSpeed(double speedDown) {
+        indexerDown.set(speedDown);
+    }
+
+    public void stop() {
+        indexerUp.set(0);
+        indexerDown.set(0);
+    }
+
+    public Command run(double speedUp, double speedDown) {
+        return this.runEnd(
+            () -> {
+                setIndexerUpSpeed(speedUp);
+                setIndexerDownSpeed(speedDown);
+            },
+            () -> stop()
+        );
+    }
+}