Added Indexer and commands

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

@@ -195,4 +195,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 double kIndexerReduction = 1;
+        public static final int kIndexerDeviceId = 3;
+        public static final int kIndexerDeviceId2 = 4;
+        public static final double kIndexerLoadPercent = 25;
+        public static final double kIndexerFeedPercent = 25;
+
+        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");
+
+        public static int kIndexerEntranceSensorPort;
+        public static int kIndexerExitSensorPort; 
+    }
 }

src/main/java/com/team1701/robot/commands/IndexCommand.java

@@ -0,0 +1,32 @@
+package com.team1701.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import com.team1701.robot.subsystems.indexer.IndexerIO;
+import com.team1701.robot.subsystems.indexer.Indexer.IndexerIOSensors;
+//import com.team1701.robot.subsystems.elevator.*;
+//import com.team1701.robot.subsystems.intake.*;
+
+
+public class IndexCommand extends Command {
+    private boolean shouldLoad;
+    private boolean enterSensor = IndexerIOSensors.entranceSensorBlocked;
+    private boolean exitSensor = IndexerIOSensors.exitSensorBlocked;
+    //private boolean elevatorReady = elevator is down
+    //private boolean intakeSensor = intake has piece
+    public static void loadPiece(
+        boolean shouldLoad,
+        boolean enterSensor,
+        boolean exitSensor,
+        boolean intakeSensor,
+        boolean elevatorReady
+    ) {
+        if(intakeSensor && !exitSensor && shouldLoad) {
+            return LoadPiece.Index;
+        } else if (exitSensor && shouldLoad && elevatorReady) {
+            return LoadPiece.Load;
+        } else {
+            //do nothing?
+        }
+    }
+
+}

src/main/java/com/team1701/robot/subsystems/indexer/Indexer.java

@@ -0,0 +1,96 @@
+package com.team1701.robot.subsystems.indexer;
+
+import com.team1701.lib.drivers.motors.MotorIO;
+import com.team1701.lib.drivers.motors.MotorIOSim;
+import com.team1701.lib.drivers.motors.MotorInputsAutoLogged;
+import com.team1701.robot.Constants;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.AutoLogOutput;
+import org.littletonrobotics.junction.Logger;
+
+public class Indexer extends SubsystemBase {
+    private MotorIO mIndexerMotorIO;
+    private final MotorInputsAutoLogged mIndexerMotorInputsAutoLogged = new MotorInputsAutoLogged();
+    private final IndexerInputsAutoLogged mIndexerInputsAutoLogged = new IndexerInputsAutoLogged();
+    private final IndexerIO.IndexerInputs mIndexerInputs = new IndexerIO.IndexerInputs();
+    private DigitalInput mEntranceSensor;
+    private DigitalInput mExitSensor;
+    private IndexerIO indexerIO = new IndexerIOSensors();
+
+    @AutoLogOutput(key = "Indexer/Motor/DemandRadiansPerSecond")
+    private double mDemandRadiansPerSecond;
+
+    public Indexer(MotorIO motor) {
+        mEntranceSensor = new DigitalInput(Constants.Indexer.kIndexerEntranceSensorPort);
+        mExitSensor = new DigitalInput(Constants.Indexer.kIndexerExitSensorPort);
+        mIndexerMotorIO = motor;
+        setPID(
+                Constants.Indexer.kIndexerKff.get(),
+                Constants.Indexer.kIndexerKp.get(),
+                0,
+                Constants.Indexer.kIndexerKd.get());
+        mIndexerMotorIO.setBrakeMode(false);
+
+    }
+
+    public static MotorIOSim createSim(DCMotor IndexerMotor) {
+        return new MotorIOSim(IndexerMotor, Constants.Indexer.kIndexerReduction, 0.025, Constants.kLoopPeriodSeconds);
+    }
+
+    @Override
+    public void periodic() {
+        var hash = hashCode();
+        mIndexerMotorIO.updateInputs(mIndexerMotorInputsAutoLogged);
+        Logger.processInputs("Indexer/Motor", mIndexerMotorInputsAutoLogged);
+        if (Constants.Indexer.kIndexerKff.hasChanged(hash)
+                || Constants.Indexer.kIndexerKp.hasChanged(hash)
+                || Constants.Indexer.kIndexerKd.hasChanged(hash)) {
+            setPID(
+                    Constants.Indexer.kIndexerKff.get(),
+                    Constants.Indexer.kIndexerKp.get(),
+                    0,
+                    Constants.Indexer.kIndexerKd.get());
+        }
+       indexerIO.updateInputs(mIndexerInputsAutoLogged);
+    }
+
+    public void setPID(double ff, double p, double i, double d) {
+        mIndexerMotorIO.setPID(ff, p, i, d);
+    }
+
+    public void setSpeed(double radiansPerSecond) {
+        mIndexerMotorIO.setVelocityControl(radiansPerSecond);
+    }
+
+    public void loadIndexer() {
+        if(mIndexerInputs.entranceSensorBlocked == true) {
+            mIndexerMotorIO.setPercentOutput(Constants.Indexer.kIndexerLoadPercent);
+        } else if(mIndexerInputs.exitSensorBlocked == true) {
+            mIndexerMotorIO.setPercentOutput(0);
+        }
+    }
+
+    public void setForward() {
+        mIndexerMotorIO.setPercentOutput(Constants.Indexer.kIndexerFeedPercent);
+    }
+
+    public void setReverse() {
+        mIndexerMotorIO.setPercentOutput(-Constants.Indexer.kIndexerFeedPercent);
+    }
+
+    public void stop () {
+        mIndexerMotorIO.setPercentOutput(0);
+    }
+
+
+    public class IndexerIOSensors implements IndexerIO {
+
+        @Override
+        public void updateInputs(IndexerInputs inputs) {
+            inputs.entranceSensorBlocked = mEntranceSensor.get();
+            inputs.exitSensorBlocked = mExitSensor.get();
+        }
+    }
+}

src/main/java/com/team1701/robot/subsystems/indexer/IndexerIO.java

@@ -0,0 +1,20 @@
+package com.team1701.robot.subsystems.indexer;
+
+import org.littletonrobotics.junction.AutoLog;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public interface IndexerIO{
+    @AutoLog
+    public static class IndexerInputs {
+        public boolean entranceSensorBlocked; 
+        public boolean exitSensorBlocked;
+
+    }
+    boolean entranceSensorBlocked = false;
+    boolean exitSensorBlocked = false;
+    public default void updateInputs(IndexerInputs inputs) {
+
+    }
+
+}

src/main/java/com/team1701/robot/subsystems/indexer/IndexerMotorFactory.java

@@ -0,0 +1,52 @@
+package com.team1701.robot.subsystems.indexer;
+
+import java.util.function.Supplier;
+
+import com.revrobotics.CANSparkFlex;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.REVLibError;
+import com.team1701.lib.alerts.REVAlert;
+import com.team1701.lib.drivers.motors.MotorIOSparkFlex;
+import com.team1701.robot.Constants;
+
+public class IndexerMotorFactory {
+    public static MotorIOSparkFlex createDriveMotorIOSparkFlex(int deviceId) {
+        var motor = new CANSparkFlex(deviceId, MotorType.kBrushless);
+        var encoder = motor.getEncoder();
+        var controller = motor.getPIDController();
+        var errorAlert = new REVAlert(motor, deviceId);
+
+        motor.setCANTimeout(200);
+
+        configureWithRetry(() -> motor.restoreFactoryDefaults(), errorAlert);
+
+        configureWithRetry(() -> motor.setSmartCurrentLimit(80), errorAlert);
+        configureWithRetry(() -> motor.enableVoltageCompensation(12), errorAlert);
+
+        configureWithRetry(() -> encoder.setPosition(0), errorAlert);
+        configureWithRetry(() -> encoder.setMeasurementPeriod(10), errorAlert);
+        configureWithRetry(() -> encoder.setAverageDepth(2), errorAlert);
+
+        configureWithRetry(() -> controller.setP(Constants.Indexer.kIndexerKp.get()), errorAlert);
+        configureWithRetry(() -> controller.setD(Constants.Indexer.kIndexerKd.get()), errorAlert);
+        configureWithRetry(() -> controller.setFF(Constants.Indexer.kIndexerKff.get()), errorAlert);
+
+        configureWithRetry(() -> motor.burnFlash(), errorAlert);
+
+        motor.setCANTimeout(0);
+
+        return new MotorIOSparkFlex(motor, Constants.Shooter.kShooterReduction);
+    }
+
+    private static void configureWithRetry(Supplier<REVLibError> config, REVAlert failureAlert) {
+        REVLibError error = REVLibError.kUnknown;
+        for (var i = 0; i < 4; i++) {
+            error = config.get();
+            if (error == REVLibError.kOk) {
+                return;
+            }
+        }
+
+        failureAlert.enable(error);
+    }
+}