Funnel -> subsystems-dev

src/main/java/frc/robot/Robot.java

@@ -6,6 +6,9 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc.robot.subsystems.funnel.Funnel;
+import frc.robot.subsystems.funnel.FunnelConstants;
+import frc.robot.subsystems.funnel.factory.FunnelFactory;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very little robot logic should
@@ -16,7 +19,11 @@ public class Robot {
 
 	public static final RobotType ROBOT_TYPE = RobotType.determineRobotType();
 
+	private final Funnel funnel;
+
 	public Robot() {
+		this.funnel = new Funnel(FunnelFactory.create(FunnelConstants.LOG_PATH));
+
 		configureBindings();
 	}
 

src/main/java/frc/robot/constants/IDs.java

@@ -1,7 +1,9 @@
 package frc.robot.constants;
 
 
+import com.revrobotics.CANSparkLowLevel;
 import edu.wpi.first.wpilibj.PowerDistribution;
+import frc.robot.hardware.motor.sparkmax.SparkMaxDeviceID;
 import frc.utils.battery.PowerDistributionDeviceID;
 
 public class IDs {
@@ -11,4 +13,10 @@ public class IDs {
 		PowerDistribution.ModuleType.kRev
 	);
 
+	public static class CANSparkMAXIDs {
+
+		public static final SparkMaxDeviceID FUNNEL = new SparkMaxDeviceID(1, CANSparkLowLevel.MotorType.kBrushless);
+
+	}
+
 }

src/main/java/frc/robot/subsystems/funnel/Funnel.java

@@ -0,0 +1,70 @@
+package frc.robot.subsystems.funnel;
+
+import frc.robot.hardware.digitalinput.DigitalInputInputsAutoLogged;
+import frc.robot.hardware.digitalinput.IDigitalInput;
+import frc.robot.hardware.motor.IMotor;
+import frc.utils.GBSubsystem;
+import org.littletonrobotics.junction.Logger;
+
+public class Funnel extends GBSubsystem {
+
+	private final IMotor motor;
+	private final FunnelStuff funnelStuff;
+	private final FunnelCommandsBuilder commandsBuilder;
+	private final IDigitalInput shooterDigitalInput;
+	private final IDigitalInput ampDigitalInput;
+	private final DigitalInputInputsAutoLogged shooterDigitalInputInputs;
+	private final DigitalInputInputsAutoLogged ampDigitalInputInputs;
+
+	public Funnel(FunnelStuff funnelStuff) {
+		super(funnelStuff.logPath());
+		this.motor = funnelStuff.motor();
+		this.shooterDigitalInput = funnelStuff.shooterDigitalInput();
+		this.ampDigitalInput = funnelStuff.ampDigitalInput();
+		this.funnelStuff = funnelStuff;
+		this.shooterDigitalInputInputs = new DigitalInputInputsAutoLogged();
+		this.ampDigitalInputInputs = new DigitalInputInputsAutoLogged();
+
+		this.commandsBuilder = new FunnelCommandsBuilder(this);
+
+		updateInputs();
+	}
+
+	public FunnelCommandsBuilder getCommandsBuilder() {
+		return commandsBuilder;
+	}
+
+	public boolean isNoteInShooter() {
+		return shooterDigitalInputInputs.debouncedValue;
+	}
+
+	public boolean isNoteInAmp() {
+		return ampDigitalInputInputs.debouncedValue;
+	}
+
+	public void setBrake(boolean brake) {
+		motor.setBrake(brake);
+	}
+
+	public void stop() {
+		motor.stop();
+	}
+
+	public void setPower(double power) {
+		motor.setPower(power);
+	}
+
+	public void updateInputs() {
+		shooterDigitalInput.updateInputs(shooterDigitalInputInputs);
+		ampDigitalInput.updateInputs(ampDigitalInputInputs);
+		motor.updateSignals(funnelStuff.voltageSignal(), funnelStuff.positionSignal());
+	}
+
+	@Override
+	protected void subsystemPeriodic() {
+		updateInputs();
+		Logger.processInputs(funnelStuff.shooterDigitalInputLogPath(), shooterDigitalInputInputs);
+		Logger.processInputs(funnelStuff.ampDigitalInputLogPath(), ampDigitalInputInputs);
+	}
+
+}

src/main/java/frc/robot/subsystems/funnel/FunnelCommandsBuilder.java

@@ -0,0 +1,43 @@
+package frc.robot.subsystems.funnel;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+
+import java.util.function.DoubleSupplier;
+
+public class FunnelCommandsBuilder {
+
+	private final Funnel funnel;
+
+	public FunnelCommandsBuilder(Funnel funnel) {
+		this.funnel = funnel;
+	}
+
+	//@formatter:off
+    public Command setFunnelPower(double power) {
+        return new FunctionalCommand(
+                () -> {},
+                () -> funnel.setPower(power),
+                interrupted -> funnel.stop(),
+                () -> false,
+                funnel
+        ).withName("Set power: " + power);
+    }
+
+    public Command setFunnelPower(DoubleSupplier power) {
+        return new FunctionalCommand(
+                () -> {},
+                () -> funnel.setPower(power.getAsDouble()),
+                interrupted -> funnel.stop(),
+                () -> false,
+                funnel
+        ).withName("Set power by supplier");
+    }
+    //@formatter:on
+
+	public Command stopFunnel() {
+		return new RunCommand(funnel::stop, funnel).withName("Stop");
+	}
+
+}

src/main/java/frc/robot/subsystems/funnel/FunnelConstants.java

@@ -0,0 +1,7 @@
+package frc.robot.subsystems.funnel;
+
+public class FunnelConstants {
+
+	public static final String LOG_PATH = "Subsystems/Funnel/";
+
+}

src/main/java/frc/robot/subsystems/funnel/FunnelStuff.java

@@ -0,0 +1,40 @@
+package frc.robot.subsystems.funnel;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.hardware.digitalinput.IDigitalInput;
+import frc.robot.hardware.motor.IMotor;
+import frc.robot.hardware.signal.InputSignal;
+
+public record FunnelStuff(
+	String logPath,
+	String shooterDigitalInputLogPath,
+	String ampDigitalInputLogPath,
+	IMotor motor,
+	InputSignal<Double> voltageSignal,
+	InputSignal<Rotation2d> positionSignal,
+	IDigitalInput shooterDigitalInput,
+	IDigitalInput ampDigitalInput
+
+) {
+
+	public FunnelStuff(
+		String logPath,
+		IMotor motor,
+		InputSignal<Double> voltageSignal,
+		InputSignal<Rotation2d> positionSignal,
+		IDigitalInput shooterDigitalInput,
+		IDigitalInput ampDigitalInput
+	) {
+		this(
+			logPath,
+			logPath + "shooterDigitalInput",
+			logPath + "ampDigitalInput",
+			motor,
+			voltageSignal,
+			positionSignal,
+			shooterDigitalInput,
+			ampDigitalInput
+		);
+	}
+
+}

src/main/java/frc/robot/subsystems/funnel/factory/FunnelFactory.java

@@ -0,0 +1,15 @@
+package frc.robot.subsystems.funnel.factory;
+
+import frc.robot.Robot;
+import frc.robot.subsystems.funnel.FunnelStuff;
+
+public class FunnelFactory {
+
+	public static FunnelStuff create(String logPath) {
+		return switch (Robot.ROBOT_TYPE) {
+			case REAL -> RealFunnelConstants.generateFunnelStuff(logPath);
+			case SIMULATION -> null;
+		};
+	}
+
+}

src/main/java/frc/robot/subsystems/funnel/factory/RealFunnelConstants.java

@@ -0,0 +1,57 @@
+package frc.robot.subsystems.funnel.factory;
+
+import com.revrobotics.SparkLimitSwitch;
+import edu.wpi.first.math.filter.Debouncer;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.constants.IDs;
+import frc.robot.hardware.digitalinput.supplied.SuppliedDigitalInput;
+import frc.robot.hardware.motor.sparkmax.BrushlessSparkMAXMotor;
+import frc.robot.hardware.motor.sparkmax.SparkMaxWrapper;
+import frc.robot.hardware.signal.supplied.SuppliedAngleSignal;
+import frc.robot.hardware.signal.supplied.SuppliedDoubleSignal;
+import frc.robot.subsystems.funnel.FunnelStuff;
+import frc.utils.AngleUnit;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.Supplier;
+
+public class RealFunnelConstants {
+
+	private final static double DEBOUNCE_TIME_SECONDS = 0.05;
+	private final static Debouncer.DebounceType DEBOUNCE_TYPE = Debouncer.DebounceType.kBoth;
+
+	private final static SparkLimitSwitch.Type REVERSE_LIMIT_SWITCH_TYPE = SparkLimitSwitch.Type.kNormallyOpen;
+	private final static SparkLimitSwitch.Type FORWARD_LIMIT_SWITCH_TYPE = SparkLimitSwitch.Type.kNormallyOpen;
+
+	private final static int CURRENT_LIMIT = 30;
+	private final static double GEAR_RATIO = 1;
+
+
+	public static FunnelStuff generateFunnelStuff(String logPath) {
+		SparkMaxWrapper sparkMAXWrapper = new SparkMaxWrapper(IDs.CANSparkMAXIDs.FUNNEL);
+
+		SysIdRoutine.Config config = new SysIdRoutine.Config();
+
+		sparkMAXWrapper.setSmartCurrentLimit(CURRENT_LIMIT);
+		sparkMAXWrapper.getEncoder().setPositionConversionFactor(GEAR_RATIO);
+		sparkMAXWrapper.getEncoder().setVelocityConversionFactor(GEAR_RATIO);
+
+		BrushlessSparkMAXMotor motor = new BrushlessSparkMAXMotor(logPath, sparkMAXWrapper, config);
+
+		SuppliedDoubleSignal voltageSignal = new SuppliedDoubleSignal("voltage", sparkMAXWrapper::getVoltage);
+
+		Supplier<Double> position = () -> sparkMAXWrapper.getEncoder().getPosition();
+		SuppliedAngleSignal positionSignal = new SuppliedAngleSignal("position", position, AngleUnit.ROTATIONS);
+
+		BooleanSupplier isShooterBeamBroken = () -> sparkMAXWrapper.getReverseLimitSwitch(REVERSE_LIMIT_SWITCH_TYPE).isPressed();
+		sparkMAXWrapper.getReverseLimitSwitch(REVERSE_LIMIT_SWITCH_TYPE).enableLimitSwitch(false);
+		SuppliedDigitalInput shooterBeamBreaker = new SuppliedDigitalInput(isShooterBeamBroken, DEBOUNCE_TYPE, DEBOUNCE_TIME_SECONDS);
+
+		BooleanSupplier isAmpBeamBroken = () -> sparkMAXWrapper.getForwardLimitSwitch(REVERSE_LIMIT_SWITCH_TYPE).isPressed();
+		sparkMAXWrapper.getForwardLimitSwitch(FORWARD_LIMIT_SWITCH_TYPE).enableLimitSwitch(false);
+		SuppliedDigitalInput ampBeamBreaker = new SuppliedDigitalInput(isAmpBeamBroken, DEBOUNCE_TYPE, DEBOUNCE_TIME_SECONDS);
+
+		return new FunnelStuff(logPath, motor, voltageSignal, positionSignal, shooterBeamBreaker, ampBeamBreaker);
+	}
+
+}