From f6509edfa9c6f6d5b4071429cd75dda97dcc7bd3 Mon Sep 17 00:00:00 2001 From: Tenumars Date: Thu, 25 Jan 2024 16:31:57 -0500 Subject: [PATCH] Added Indexer and commands --- .../java/com/team1701/robot/Constants.java | 16 ++++ .../team1701/robot/commands/IndexCommand.java | 32 +++++++ .../robot/subsystems/indexer/Indexer.java | 96 +++++++++++++++++++ .../robot/subsystems/indexer/IndexerIO.java | 20 ++++ .../indexer/IndexerMotorFactory.java | 52 ++++++++++ 5 files changed, 216 insertions(+) create mode 100644 src/main/java/com/team1701/robot/commands/IndexCommand.java create mode 100644 src/main/java/com/team1701/robot/subsystems/indexer/Indexer.java create mode 100644 src/main/java/com/team1701/robot/subsystems/indexer/IndexerIO.java create mode 100644 src/main/java/com/team1701/robot/subsystems/indexer/IndexerMotorFactory.java diff --git a/src/main/java/com/team1701/robot/Constants.java b/src/main/java/com/team1701/robot/Constants.java index 5335e61e..2e03f33e 100644 --- a/src/main/java/com/team1701/robot/Constants.java +++ b/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; + } } diff --git a/src/main/java/com/team1701/robot/commands/IndexCommand.java b/src/main/java/com/team1701/robot/commands/IndexCommand.java new file mode 100644 index 00000000..b87e3b6b --- /dev/null +++ b/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? + } + } + +} \ No newline at end of file diff --git a/src/main/java/com/team1701/robot/subsystems/indexer/Indexer.java b/src/main/java/com/team1701/robot/subsystems/indexer/Indexer.java new file mode 100644 index 00000000..c43a3ffa --- /dev/null +++ b/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(); + } + } +} \ No newline at end of file diff --git a/src/main/java/com/team1701/robot/subsystems/indexer/IndexerIO.java b/src/main/java/com/team1701/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 00000000..e021e93e --- /dev/null +++ b/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) { + + } + +} \ No newline at end of file diff --git a/src/main/java/com/team1701/robot/subsystems/indexer/IndexerMotorFactory.java b/src/main/java/com/team1701/robot/subsystems/indexer/IndexerMotorFactory.java new file mode 100644 index 00000000..eaa12173 --- /dev/null +++ b/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 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); + } +}