37 create a basic intake subsystem

src/main/java/org/jmhsrobotics/frc2024/Constants.java

@@ -7,6 +7,26 @@
 import edu.wpi.first.math.util.Units;
 
 public class Constants {
+
+	/**
+	 * CAN id assignments
+	 */
+	public static class CAN {
+		public static final int kIntakeId = 32;
+
+	};
+	/**
+	 * Digital IO ports
+	 */
+	public static class DIO {
+		public static final int kIntakeSwitch = 2;
+	}
+	/**
+	 * PWM output ports
+	 */
+	public static class PWM {
+		public static final int kLedStrip = 9;
+	}
 	public static class SwerveConstants {
 		// SPARK MAX CAN IDs
 		public static final int kFrontLeftDrivingCanId = 10;

src/main/java/org/jmhsrobotics/frc2024/Robot.java

@@ -6,7 +6,6 @@
 
 import java.util.List;
 
-import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand;
 import org.jmhsrobotics.warcore.util.BuildDataLogger;
 
 import com.pathplanner.lib.path.GoalEndState;
@@ -21,7 +20,6 @@
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import monologue.Logged;
@@ -35,7 +33,6 @@ public class Robot extends TimedRobot implements Logged {
 	@Override
 	public void robotInit() {
 		m_robotContainer = new RobotContainer();
-		SmartDashboard.putData("IntakeCommand", new IntakeCommand(this.m_robotContainer.getDriveSubsystem(), 5));
 		setupLogs();
 	}
 

src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java

@@ -10,7 +10,7 @@
 import org.jmhsrobotics.frc2024.subsystems.LED.commands.RainbowLEDCommand;
 import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.drive.commands.DriveCommand;
-import org.jmhsrobotics.frc2024.subsystems.drive.commands.IntakeCommand;
+import org.jmhsrobotics.frc2024.subsystems.intake.IntakeSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem;
 
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -31,13 +31,14 @@
 
 public class RobotContainer implements Logged {
 
-	private ControlBoard control = new CompControl();
+	public ControlBoard control = new CompControl();
 	// Subsystems
 	private final DriveSubsystem driveSubsystem = new DriveSubsystem();
 
 	private final VisionSubsystem vision = new VisionSubsystem(this.driveSubsystem);
 
 	private final LEDSubsystem ledSubsystem = new LEDSubsystem();
+	private final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();
 
 	private final SendableChooser<Command> autoChooser;
 
@@ -62,7 +63,6 @@ private void configurePathPlanner() {
 				new HolonomicPathFollowerConfig(new PIDConstants(.5, 0, 0), new PIDConstants(1.5, 0, 0),
 						Constants.SwerveConstants.kMaxSpeedMetersPerSecond, .5, new ReplanningConfig()),
 				this::getAllianceFlipState, driveSubsystem);
-		NamedCommands.registerCommand("Intake", new IntakeCommand(driveSubsystem, 5));
 		NamedCommands.registerCommand("Wait", new WaitCommand(30));
 	}
 

src/main/java/org/jmhsrobotics/frc2024/subsystems/intake/IntakeSubsystem.java

@@ -0,0 +1,70 @@
+package org.jmhsrobotics.frc2024.subsystems.intake;
+
+import org.jmhsrobotics.frc2024.Constants;
+import org.jmhsrobotics.warcore.rev.RevEncoderSimWrapper;
+
+import com.revrobotics.CANSparkBase.IdleMode;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import com.revrobotics.CANSparkMax;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class IntakeSubsystem extends SubsystemBase {
+	private CANSparkMax intakeMotor;
+	private DigitalInput intakeSwitch;
+
+	public IntakeSubsystem() {
+		intakeMotor = new CANSparkMax(Constants.CAN.kIntakeId, MotorType.kBrushless);
+		intakeMotor.setInverted(true);
+		intakeMotor.setIdleMode(IdleMode.kBrake);
+
+		intakeMotor.setSmartCurrentLimit(35);
+
+		intakeSwitch = new DigitalInput(Constants.DIO.kIntakeSwitch);
+		if (RobotBase.isSimulation()) {
+			simInit();
+		}
+	}
+
+	@Override
+	public void periodic() {
+		SmartDashboard.putNumber("intake/velocityRPM", intakeMotor.getEncoder().getVelocity());
+		SmartDashboard.putNumber("intake/currentDrawAmps", intakeMotor.getOutputCurrent());
+		SmartDashboard.putBoolean("intake/hasNote", this.hasNote());
+	}
+
+	public void set(double speed) {
+		intakeMotor.set(speed);
+	}
+
+	public boolean hasNote() {
+		return intakeSwitch.get();
+	}
+
+	private DIOSim intakeSwitchSim;
+	private DCMotorSim intakeSim;
+	private RevEncoderSimWrapper intakeEncSim;
+
+	public void simInit() {
+		intakeSwitchSim = new DIOSim(Constants.DIO.kIntakeSwitch);
+		intakeSim = new DCMotorSim(DCMotor.getNEO(1), 1, 0.3);
+		intakeEncSim = RevEncoderSimWrapper.create(intakeMotor);
+	}
+
+	@Override
+	public void simulationPeriodic() {
+		double intakeVolts = intakeMotor.get() * 12;
+		intakeSim.setInput(intakeVolts);
+		intakeSim.update(0.2);
+		intakeSwitchSim.setValue(true); // TODO placeholder.
+		intakeEncSim.setDistance(intakeSim.getAngularPositionRotations());
+		intakeEncSim.setVelocity(intakeSim.getAngularVelocityRPM());
+
+	}
+}