Shooter-commands

.vscode/settings.json

@@ -54,13 +54,20 @@
     }
   },
   "eslint.rules.customizations": [
-    { "rule": "*", "severity": "warn" },
-    { "rule": "prettier/prettier", "severity": "off" }
+    {
+      "rule": "*",
+      "severity": "warn"
+    },
+    {
+      "rule": "prettier/prettier",
+      "severity": "off"
+    }
   ],
   "eslint.validate": ["javascript", "typescript", "svelte"],
   "cSpell.words": [
     "Brushless",
     "Deadband",
+    "digitalinputs",
     "dtheta",
     "Holonomic",
     "Odometry",
@@ -70,6 +77,7 @@
     "Setpoints",
     "teleop",
     "Uncomitted",
+    "wpilibj",
     "WPILOG"
   ]
 }

src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIO.java

@@ -0,0 +1,14 @@
+package com.team1701.lib.drivers.digitalinputs;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface DigitalIO {
+    @AutoLog
+    public static class DigitalInputs {
+        public boolean blocked;
+    }
+
+    public default void updateInputs(DigitalInputs inputs) {}
+
+    public default void getBlocked() {}
+}

src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIOSensor.java

@@ -0,0 +1,16 @@
+package com.team1701.lib.drivers.digitalinputs;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class DigitalIOSensor implements DigitalIO {
+    private final DigitalInput mSensor;
+
+    public DigitalIOSensor(int channel) {
+        mSensor = new DigitalInput(channel);
+    }
+
+    @Override
+    public void updateInputs(DigitalInputs inputs) {
+        inputs.blocked = mSensor.get();
+    }
+}

src/main/java/com/team1701/lib/drivers/digitalinputs/DigitalIOSim.java

@@ -0,0 +1,16 @@
+package com.team1701.lib.drivers.digitalinputs;
+
+import java.util.function.Supplier;
+
+public class DigitalIOSim implements DigitalIO {
+    private final Supplier<Boolean> mBlockedSupplier;
+
+    public DigitalIOSim(Supplier<Boolean> blockedSupplier) {
+        mBlockedSupplier = blockedSupplier;
+    }
+
+    @Override
+    public void updateInputs(DigitalInputs inputs) {
+        inputs.blocked = mBlockedSupplier.get();
+    }
+}

src/main/java/com/team1701/lib/drivers/encoders/EncoderIOAnalog.java

@@ -5,7 +5,7 @@
 import edu.wpi.first.wpilibj.AnalogEncoder;
 
 public class EncoderIOAnalog implements EncoderIO {
-    private final AnalogEncoder mEncoder;
+    public final AnalogEncoder mEncoder;
 
     public EncoderIOAnalog(int channel) {
         mEncoder = new AnalogEncoder(channel);

src/main/java/com/team1701/lib/drivers/encoders/EncoderIODigital.java

@@ -0,0 +1,30 @@
+package com.team1701.lib.drivers.encoders;
+
+import com.team1701.robot.Constants;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+
+public class EncoderIODigital implements EncoderIO {
+    public final DutyCycleEncoder mEncoder;
+
+    public EncoderIODigital(int channel) {
+        mEncoder = new DutyCycleEncoder(Constants.Shooter.kShooterThroughBoreEncoderId);
+        mEncoder.setDistancePerRotation(Constants.Shooter.kThroughBoreEncoderDistancePerRotation);
+    }
+
+    @Override
+    public void updateInputs(EncoderInputs inputs) {
+        var rotations = mEncoder.get();
+        inputs.position = Rotation2d.fromRotations(
+                rotations == Double.NaN || rotations == Double.POSITIVE_INFINITY ? 0.0 : rotations);
+    }
+
+    public void setPositionOffset(Rotation2d offset) {
+        mEncoder.setPositionOffset(MathUtil.inputModulus(offset.getRotations(), 0.0, 1.0));
+    }
+
+    public void setDistancePerRotation(double distancePerRotation) {
+        mEncoder.setDistancePerRotation(distancePerRotation);
+    }
+}

src/main/java/com/team1701/lib/drivers/encoders/EncoderIOSim.java

@@ -5,10 +5,12 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 
 public class EncoderIOSim implements EncoderIO {
-    private final Supplier<Rotation2d> mRotationSupplier;
+    public final Supplier<Rotation2d> mRotationSupplier;
+    public double rotations;
 
     public EncoderIOSim(Supplier<Rotation2d> rotationSupplier) {
         mRotationSupplier = rotationSupplier;
+        rotations = mRotationSupplier.get().getRotations();
     }
 
     @Override

src/main/java/com/team1701/robot/Constants.java

@@ -158,26 +158,47 @@ public static final class Drive {
     public static final class Shooter {
         // TODO: Update values
         public static final double kShooterReduction = 1;
-        public static final int kShooterDeviceId = 0;
+        public static final int kShooterUpperRollerMotorId = 0;
+        public static final int kShooterLowerRollerMotorId = 1;
+        public static final int kShooterRotationMotorId = 2;
 
         public static final double kShooterAxisHeight = Units.inchesToMeters(10);
         public static final double kShooterAxisOffset = Units.inchesToMeters(10); // + is toward front of bot
 
-        public static final LoggedTunableNumber kShooterKff = new LoggedTunableNumber("Shooter/Motor/Kff");
-        public static final LoggedTunableNumber kShooterKp = new LoggedTunableNumber("Shooter/Motor/Kp");
-        public static final LoggedTunableNumber kShooterKd = new LoggedTunableNumber("Shooter/Motor/Kd");
+        public static final LoggedTunableNumber kRollerKff = new LoggedTunableNumber("Shooter/Motor/Roller/Kff");
+        public static final LoggedTunableNumber kRollerKp = new LoggedTunableNumber("Shooter/Motor/Roller/Kp");
+        public static final LoggedTunableNumber kRollerKd = new LoggedTunableNumber("Shooter/Motor/Roller/Kd");
+
+        public static final LoggedTunableNumber kRotationKff = new LoggedTunableNumber("Shooter/Motor/Rotation/Kff");
+        public static final LoggedTunableNumber kRotationKp = new LoggedTunableNumber("Shooter/Motor/Rotation/Kp");
+        public static final LoggedTunableNumber kRotationKd = new LoggedTunableNumber("Shooter/Motor/Rotation/Kd");
+
+        public static int kShooterEntranceSensorId;
+        public static int kShooterExitSensorId;
+        public static int kShooterThroughBoreEncoderId;
+
+        public static double kThroughBoreEncoderDistancePerRotation;
 
         static {
             switch (Configuration.getRobot()) {
                 case COMPETITION_BOT:
-                    kShooterKd.initDefault(1);
-                    kShooterKp.initDefault(0.6);
-                    kShooterKff.initDefault(0);
+                    kRollerKd.initDefault(1);
+                    kRollerKp.initDefault(0.6);
+                    kRollerKff.initDefault(0);
+
+                    kRotationKd.initDefault(1);
+                    kRotationKp.initDefault(0.6);
+                    kRotationKff.initDefault(0);
+
                     break;
                 case SIMULATION_BOT:
-                    kShooterKd.initDefault(1);
-                    kShooterKp.initDefault(0.6);
-                    kShooterKff.initDefault(0);
+                    kRollerKd.initDefault(1);
+                    kRollerKp.initDefault(0.6);
+                    kRollerKff.initDefault(0);
+
+                    kRotationKd.initDefault(1);
+                    kRotationKp.initDefault(0.6);
+                    kRotationKff.initDefault(0);
                     break;
                 default:
                     throw new UnsupportedOperationException("No shooter configuration for " + Configuration.getRobot());
@@ -195,4 +216,20 @@ public static final class Elevator {
         public static final LoggedTunableNumber kElevatorKp = new LoggedTunableNumber("Elevator/Motor/Kp");
         public static final LoggedTunableNumber kElevatorKd = new LoggedTunableNumber("Elevator/Motor/Kd");
     }
+
+    public static final class Indexer {
+        // TODO: Update values and set names
+        public static final int kIndexerMotorId = 0;
+        public static final double kIndexerReduction = 1;
+
+        public static final int kIndexerEntranceSensorId = 1;
+        public static final int kIndexerExitSensorId = 3;
+
+        public static final double kIndexerLoadPercent = .25;
+        public static final double kIndexerFeedPercent = 1;
+
+        public static final LoggedTunableNumber kIndexerKff = new LoggedTunableNumber("Indexer/Motor/Kff");
+        public static final LoggedTunableNumber kIndexerKp = new LoggedTunableNumber("Indexer/Motor/Kp");
+        public static final LoggedTunableNumber kIndexerKd = new LoggedTunableNumber("Indexer/Motor/Kd");
+    }
 }

src/main/java/com/team1701/robot/RobotContainer.java

@@ -11,23 +11,33 @@
 import com.pathplanner.lib.commands.PathPlannerAuto;
 import com.pathplanner.lib.util.PathPlannerLogging;
 import com.team1701.lib.alerts.TriggeredAlert;
+import com.team1701.lib.drivers.digitalinputs.DigitalIO;
+import com.team1701.lib.drivers.digitalinputs.DigitalIOSensor;
+import com.team1701.lib.drivers.digitalinputs.DigitalIOSim;
 import com.team1701.lib.drivers.encoders.EncoderIO;
 import com.team1701.lib.drivers.encoders.EncoderIOAnalog;
 import com.team1701.lib.drivers.gyros.GyroIO;
 import com.team1701.lib.drivers.gyros.GyroIOPigeon2;
 import com.team1701.lib.drivers.gyros.GyroIOSim;
 import com.team1701.lib.drivers.motors.MotorIO;
+import com.team1701.lib.drivers.motors.MotorIOSim;
 import com.team1701.lib.util.GeometryUtil;
+import com.team1701.lib.util.LoggedTunableNumber;
 import com.team1701.robot.Configuration.Mode;
+import com.team1701.robot.SparkFlexMotorFactory.ShooterMotorUsage;
 import com.team1701.robot.commands.AutonomousCommands;
+import com.team1701.robot.commands.DriveCommands;
+import com.team1701.robot.commands.IndexCommand;
 import com.team1701.robot.states.RobotState;
 import com.team1701.robot.subsystems.drive.Drive;
-import com.team1701.robot.subsystems.drive.DriveMotorFactory;
 import com.team1701.robot.subsystems.drive.SwerveModule.SwerveModuleIO;
+import com.team1701.robot.subsystems.indexer.Indexer;
+import com.team1701.robot.subsystems.indexer.IndexerMotorFactory;
 import com.team1701.robot.subsystems.shooter.Shooter;
-import com.team1701.robot.subsystems.shooter.ShooterMotorFactory;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
@@ -42,15 +52,19 @@ public class RobotContainer {
     private final RobotState mRobotState = new RobotState();
     public final Drive mDrive;
     public final Shooter mShooter;
+    public final Indexer mIndexer;
+
     // public final Vision mVision;
 
     private final CommandXboxController mDriverController = new CommandXboxController(0);
+    // Trigger bButton = mDriverController.b();
     private final LoggedDashboardChooser<Command> autonomousModeChooser = new LoggedDashboardChooser<>("Auto Mode");
 
     public RobotContainer() {
         Optional<Drive> drive = Optional.empty();
         // Optional<Vision> vision = Optional.empty();
         Optional<Shooter> shooter = Optional.empty();
+        Optional<Indexer> indexer = Optional.empty();
 
         if (Configuration.getMode() != Mode.REPLAY) {
             switch (Configuration.getRobot()) {
@@ -59,27 +73,37 @@ public RobotContainer() {
                             new GyroIOPigeon2(10),
                             new SwerveModuleIO[] {
                                 new SwerveModuleIO(
-                                        DriveMotorFactory.createDriveMotorIOSparkMax(10),
-                                        DriveMotorFactory.createSteerMotorIOSparkMax(11),
+                                        SparkFlexMotorFactory.createDriveMotorIOSparkMax(10),
+                                        SparkFlexMotorFactory.createSteerMotorIOSparkMax(11),
                                         new EncoderIOAnalog(0)),
                                 new SwerveModuleIO(
-                                        DriveMotorFactory.createDriveMotorIOSparkMax(12),
-                                        DriveMotorFactory.createSteerMotorIOSparkMax(13),
+                                        SparkFlexMotorFactory.createDriveMotorIOSparkMax(12),
+                                        SparkFlexMotorFactory.createSteerMotorIOSparkMax(13),
                                         new EncoderIOAnalog(1)),
                                 new SwerveModuleIO(
-                                        DriveMotorFactory.createDriveMotorIOSparkMax(16),
-                                        DriveMotorFactory.createSteerMotorIOSparkMax(17),
+                                        SparkFlexMotorFactory.createDriveMotorIOSparkMax(16),
+                                        SparkFlexMotorFactory.createSteerMotorIOSparkMax(17),
                                         new EncoderIOAnalog(3)),
                                 new SwerveModuleIO(
-                                        DriveMotorFactory.createDriveMotorIOSparkMax(14),
-                                        DriveMotorFactory.createSteerMotorIOSparkMax(15),
+                                        SparkFlexMotorFactory.createDriveMotorIOSparkMax(14),
+                                        SparkFlexMotorFactory.createSteerMotorIOSparkMax(15),
                                         new EncoderIOAnalog(2)),
                             },
                             mRobotState));
 
-                    // TODO: update ID
+                    // TODO: update IDs
                     shooter = Optional.of(new Shooter(
-                            ShooterMotorFactory.createDriveMotorIOSparkFlex(Constants.Shooter.kShooterDeviceId)));
+                            SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
+                                    Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
+                            SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
+                                    Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
+                            SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
+                                    Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION),
+                            new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId)));
+                    indexer = Optional.of(new Indexer(
+                            IndexerMotorFactory.createDriveMotorIOSparkFlex(Constants.Indexer.kIndexerMotorId),
+                            new DigitalIOSensor(Constants.Indexer.kIndexerEntranceSensorId),
+                            new DigitalIOSensor(Constants.Indexer.kIndexerExitSensorId)));
                     break;
                 case SIMULATION_BOT:
                     var gyroIO = new GyroIOSim(mRobotState::getHeading);
@@ -93,7 +117,17 @@ public RobotContainer() {
                             () -> simDrive.getVelocity().omegaRadiansPerSecond, Constants.kLoopPeriodSeconds);
 
                     drive = Optional.of(simDrive);
-                    shooter = Optional.of(new Shooter(Shooter.createSim(DCMotor.getNeoVortex(1))));
+                    shooter = Optional.of(new Shooter(
+                            Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
+                            Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
+                            Shooter.createMotorSim(DCMotor.getNeoVortex(1)),
+                            Shooter.createEncoderSim(() -> new Rotation2d(Units.degreesToRadians(
+                                    new LoggedTunableNumber("SimulatedThroughBoreEncoder/InitialAngleDegrees", 30)
+                                            .get())))));
+                    indexer = Optional.of(new Indexer(
+                            new MotorIOSim(DCMotor.getNeoVortex(1), 1, 0.001, Constants.kLoopPeriodSeconds),
+                            new DigitalIOSim(() -> false),
+                            new DigitalIOSim(() -> false)));
                     break;
                 default:
                     break;
@@ -119,8 +153,16 @@ public RobotContainer() {
         new AprilTagCameraIO() {},
         new AprilTagCameraIO() {})); */
 
-        this.mShooter = shooter.orElseGet(
-                () -> new Shooter(ShooterMotorFactory.createDriveMotorIOSparkFlex(Constants.Shooter.kShooterDeviceId)));
+        this.mShooter = shooter.orElseGet(() -> new Shooter(
+                SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
+                        Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
+                SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
+                        Constants.Shooter.kShooterUpperRollerMotorId, ShooterMotorUsage.ROLLER),
+                SparkFlexMotorFactory.createShooterMotorIOSparkFlex(
+                        Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION),
+                new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId)));
+
+        this.mIndexer = indexer.orElseGet(() -> new Indexer(new MotorIO() {}, new DigitalIO() {}, new DigitalIO() {}));
 
         setupControllerBindings();
         setupAutonomous();
@@ -143,13 +185,21 @@ private void setupControllerBindings() {
                 () -> mDriverController.rightTrigger().getAsBoolean()
                         ? Constants.Drive.kSlowKinematicLimits
                         : Constants.Drive.kFastKinematicLimits));
+
+        // TODO: update should load when intake is completed
+        mIndexer.setDefaultCommand(new IndexCommand(mIndexer, () -> true));
+
         mDriverController
                 .x()
                 .onTrue(runOnce(() -> mDrive.zeroGyroscope(
                                 Configuration.isBlueAlliance()
                                         ? GeometryUtil.kRotationIdentity
                                         : GeometryUtil.kRotationPi))
                         .withName("ZeroGyroscopeToHeading"));
+        mDriverController
+                .rightBumper()
+                .whileTrue(DriveCommands.rotateRelativeToRobot(
+                        mDrive, new Rotation2d(2), Constants.Drive.kFastKinematicLimits, true));
         mDriverController.leftTrigger().whileTrue(swerveLock(mDrive));
         TriggeredAlert.info("Driver right bumper pressed", mDriverController.rightBumper());