diff --git a/src/main/java/com/team1701/robot/Constants.java b/src/main/java/com/team1701/robot/Constants.java index ce6ad71b..67b8ed03 100644 --- a/src/main/java/com/team1701/robot/Constants.java +++ b/src/main/java/com/team1701/robot/Constants.java @@ -216,4 +216,26 @@ 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 kIntakeEntranceId = 1; + public static final int kIntakeExitId = 3; + public static final int kShooterEnterId = 4; + public static final int kShooterExitId = 5; + 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"); + + // TODO: Add IDs for Sensors + public static final int kIndexerExitSensorId = 0; + public static final int kIndexerEntranceSensorId = 1; + + public static int kIndexerEntranceSensorPort; + public static int kIndexerExitSensorPort; + } } diff --git a/src/main/java/com/team1701/robot/RobotContainer.java b/src/main/java/com/team1701/robot/RobotContainer.java index d0721847..552caf08 100644 --- a/src/main/java/com/team1701/robot/RobotContainer.java +++ b/src/main/java/com/team1701/robot/RobotContainer.java @@ -11,21 +11,28 @@ 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.SwerveModule.SwerveModuleIO; +import com.team1701.robot.subsystems.indexer.Indexer; +import com.team1701.robot.subsystems.indexer.IndexerMotorFactory; import com.team1701.robot.subsystems.shooter.Shooter; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -45,6 +52,8 @@ 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); @@ -55,6 +64,7 @@ public RobotContainer() { Optional drive = Optional.empty(); // Optional vision = Optional.empty(); Optional shooter = Optional.empty(); + Optional indexer = Optional.empty(); if (Configuration.getMode() != Mode.REPLAY) { switch (Configuration.getRobot()) { @@ -90,6 +100,10 @@ public RobotContainer() { SparkFlexMotorFactory.createShooterMotorIOSparkFlex( Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); + indexer = Optional.of(new Indexer( + IndexerMotorFactory.createDriveMotorIOSparkFlex(Constants.Indexer.kIntakeExitId), + new DigitalIOSensor(Constants.Indexer.kIndexerEntranceSensorId), + new DigitalIOSensor(Constants.Indexer.kIndexerExitSensorId))); break; case SIMULATION_BOT: var gyroIO = new GyroIOSim(mRobotState::getHeading); @@ -110,6 +124,10 @@ public RobotContainer() { 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; @@ -144,6 +162,8 @@ public RobotContainer() { Constants.Shooter.kShooterRotationMotorId, ShooterMotorUsage.ROTATION), new EncoderIOAnalog(Constants.Shooter.kShooterThroughBoreEncoderId))); + this.mIndexer = indexer.orElseGet(() -> new Indexer(new MotorIO() {}, new DigitalIO() {}, new DigitalIO() {})); + setupControllerBindings(); setupAutonomous(); setupStateTriggers(); @@ -165,13 +185,9 @@ private void setupControllerBindings() { () -> mDriverController.rightTrigger().getAsBoolean() ? Constants.Drive.kSlowKinematicLimits : Constants.Drive.kFastKinematicLimits)); - /* mDriverController - .b() - .onTrue(runOnce( - () -> DriveCommands.rotateRelativeToRobot( - mDrive, new Rotation2d(2.5), Constants.Drive.kFastKinematicLimits, true), - mDrive)); - TriggeredAlert.info("Driver B button pressed", mDriverController.b()); */ + + // TODO: update should load when intake is completed + mIndexer.setDefaultCommand(new IndexCommand(mIndexer, () -> true)); mDriverController .x() 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..05c7c6bc --- /dev/null +++ b/src/main/java/com/team1701/robot/commands/IndexCommand.java @@ -0,0 +1,31 @@ +package com.team1701.robot.commands; + +import java.util.function.BooleanSupplier; + +import com.team1701.robot.subsystems.indexer.Indexer; +import edu.wpi.first.wpilibj2.command.Command; + +public class IndexCommand extends Command { + private final Indexer mIndexer; + private final BooleanSupplier mShouldLoad; + + public IndexCommand(Indexer indexer, BooleanSupplier shouldLoad) { + mIndexer = indexer; + mShouldLoad = shouldLoad; + addRequirements(indexer); + } + + @Override + public void execute() { + if (mIndexer.noteIsLoaded() || !mShouldLoad.getAsBoolean()) { + mIndexer.stop(); + } else { + mIndexer.setForwardLoad(); + } + } + + @Override + public void end(boolean interrupted) { + mIndexer.stop(); + } +} 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..e1451abc --- /dev/null +++ b/src/main/java/com/team1701/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,64 @@ +package com.team1701.robot.subsystems.indexer; + +import com.team1701.lib.drivers.digitalinputs.DigitalIO; +import com.team1701.lib.drivers.digitalinputs.DigitalInputsAutoLogged; +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.wpilibj2.command.SubsystemBase; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; + +public class Indexer extends SubsystemBase { + private final MotorIO mIndexerMotorIO; + private final MotorInputsAutoLogged mIndexerMotorInputsAutoLogged = new MotorInputsAutoLogged(); + private final DigitalIO mEntranceSensor; + private final DigitalInputsAutoLogged mEntranceSensorInputs = new DigitalInputsAutoLogged(); + private final DigitalIO mExitSensor; + private final DigitalInputsAutoLogged mExitSensorInputs = new DigitalInputsAutoLogged(); + + @AutoLogOutput(key = "Indexer/Motor/DemandRadiansPerSecond") + private double mDemandRadiansPerSecond; + + public Indexer(MotorIO motor, DigitalIO entranceSensor, DigitalIO exitSensor) { + mEntranceSensor = entranceSensor; + mExitSensor = exitSensor; + mIndexerMotorIO = motor; + } + + public static MotorIOSim createSim(DCMotor IndexerMotor) { + return new MotorIOSim(IndexerMotor, Constants.Indexer.kIndexerReduction, 0.025, Constants.kLoopPeriodSeconds); + } + + @Override + public void periodic() { + mIndexerMotorIO.updateInputs(mIndexerMotorInputsAutoLogged); + mEntranceSensor.updateInputs(mEntranceSensorInputs); + mExitSensor.updateInputs(mExitSensorInputs); + Logger.processInputs("Indexer/EntranceSensor", mEntranceSensorInputs); + Logger.processInputs("Indexer/Motor", mIndexerMotorInputsAutoLogged); + Logger.processInputs("Indexer/ExitSensor", mExitSensorInputs); + } + + public boolean noteIsLoaded() { + return mExitSensorInputs.blocked; + } + + public void setForwardLoad() { + mIndexerMotorIO.setPercentOutput(Constants.Indexer.kIndexerLoadPercent); + } + + public void setForwardShoot() { + mIndexerMotorIO.setPercentOutput(Constants.Indexer.kIndexerFeedPercent); + } + + public void setReverse() { + mIndexerMotorIO.setPercentOutput(-Constants.Indexer.kIndexerFeedPercent); + } + + public void stop() { + mIndexerMotorIO.setPercentOutput(0); + } +} 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..f41b8763 --- /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(20), 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); + } +}