Added Basic Intake code

Co-authored-by: PeskyBuzz <[email protected]>

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

@@ -46,7 +46,7 @@ public Robot() {
 		Logger.recordMetadata("ProjectName", "SwerveDrive2025");
 		Logger.recordMetadata("Robot Mode", RobotConstants.ROBOT_MODE.toString());
 		Logger.recordMetadata("Git Branch", BuildConstants.GIT_BRANCH);
-		Logger.recordMetadata("Authors", "(WindingMotor) Isaac S");
+		Logger.recordMetadata("Authors", "(WindingMotor) Isaac S - (PeskyBuzz) Rae");
 
 		switch (RobotConstants.ROBOT_MODE) {
 			case REAL:

src/main/java/frc/robot/RobotContainer.java

@@ -8,16 +8,15 @@
 package frc.robot;
 
 import com.pathplanner.lib.auto.NamedCommands;
-import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
-import frc.robot.commands.CMD_AimAtAprilTag;
-import frc.robot.commands.CMD_AimAtPose;
 import frc.robot.commands.CMD_DriveAlign;
 import frc.robot.constants.FieldConstants;
 import frc.robot.constants.InputConstants;
+import frc.robot.intake.IO_IntakeReal;
+import frc.robot.intake.SUB_Intake;
 import frc.robot.swerve.IO_SwerveReal;
 import frc.robot.swerve.SUB_Swerve;
 import frc.robot.vision.IO_VisionReal;
@@ -33,6 +32,7 @@ public class RobotContainer {
 
 	private final SUB_Swerve swerve;
 	private final SUB_Vision vision;
+	private final SUB_Intake intake;
 
 	private final InputConstants globalInputMap;
 
@@ -49,24 +49,18 @@ public RobotContainer() {
 				new SUB_Swerve(
 						vision, new IO_SwerveReal(new File(Filesystem.getDeployDirectory(), "swerve")));
 
+		intake = new SUB_Intake(new IO_IntakeReal());
+
 		configureDefaultCommands();
 		configureWebserverCommands();
 		configurePathPlannerCommands();
+		configureButtonBindings();
 	}
 
 	private void configureDefaultCommands() {
 
 		swerve.setDefaultCommand(
 				new CMD_DriveAlign(swerve, driverController, globalInputMap)); // Drive and aim at speaker
-
-		// Aim at 0,0 for testing
-		driverController.a().onTrue(new CMD_AimAtPose(swerve, new Pose2d(), 0.1));
-
-		// Aim at tag 16 for testing
-		driverController.b().onTrue(new CMD_AimAtAprilTag(swerve, 16, 0.1));
-
-		// Start of pesky branch
-		// Change 2
 	}
 
 	private void configureWebserverCommands() {
@@ -82,9 +76,19 @@ private void configurePathPlannerCommands() {
 		NamedCommands.registerCommand("Intake_Algae", new PrintCommand("Hi"));
 	}
 
+	private void configureButtonBindings() {
+
+		// Aim at 0,0 for testing
+		// driverController.a().onTrue(new CMD_AimAtPose(swerve, new Pose2d(), 0.1));
+
+		// Aim at tag 16 for testing
+		// driverController.b().onTrue(new CMD_AimAtAprilTag(swerve, 16, 0.1));
+
+		driverController.a().onTrue(intake.setState(SUB_Intake.State.ALGAE_GROUND));
+		driverController.a().toggleOnFalse(intake.setState(SUB_Intake.State.STOWED));
+	}
+
 	public Command getAutonomousCommand() {
 		return swerve.getAutonomousCommand("Test");
 	}
 }
-
-// G(>o<)⅁ <- he's a running!

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

@@ -12,12 +12,12 @@ public final class BuildConstants {
 	public static final String MAVEN_GROUP = "";
 	public static final String MAVEN_NAME = "SwerveDrive2025";
 	public static final String VERSION = "unspecified";
-	public static final int GIT_REVISION = 28;
-	public static final String GIT_SHA = "504d3f274f9b0769fa704e3c2dd90039574768c7";
-	public static final String GIT_DATE = "2025-01-10 16:56:47 EST";
-	public static final String GIT_BRANCH = "main";
-	public static final String BUILD_DATE = "2025-01-10 17:32:56 EST";
-	public static final long BUILD_UNIX_TIME = 1736548376941L;
+	public static final int GIT_REVISION = 32;
+	public static final String GIT_SHA = "d45110397bccd5d27febd5a779bcede324db9cfa";
+	public static final String GIT_DATE = "2025-01-10 17:46:32 EST";
+	public static final String GIT_BRANCH = "Pesky-Branch";
+	public static final String BUILD_DATE = "2025-01-10 19:40:19 EST";
+	public static final long BUILD_UNIX_TIME = 1736556019556L;
 	public static final int DIRTY = 1;
 
 	private BuildConstants() {}

src/main/java/frc/robot/intake/IO_IntakeBase.java

@@ -0,0 +1,53 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public interface IO_IntakeBase {
+
+	@AutoLog
+	public static class IntakeInputs implements LoggableInputs {
+
+		public double armAngleDegrees = 0.0;
+		public double armMotorCurrent = 0.0;
+		public double wheelMotorCurrent = 0.0;
+		public double wheelRPM = 0.0;
+		public boolean toggleSensor = false;
+		public double distanceSensorCM = 0.0;
+
+		@Override
+		public void toLog(LogTable table) {
+			table.put("ArmAngleDegrees", armAngleDegrees);
+			table.put("ArmMotorCurrent", armMotorCurrent);
+			table.put("wheelMotorCurrent", wheelMotorCurrent);
+			table.put("wheelRPM", wheelRPM);
+			table.put("toggleSensor", toggleSensor);
+			table.put("distanceSensorCM", distanceSensorCM);
+		}
+
+		@Override
+		public void fromLog(LogTable table) {
+			armAngleDegrees = table.get("ArmAngleDegrees", armAngleDegrees);
+			armMotorCurrent = table.get("ArmMotorCurrent", armMotorCurrent);
+			wheelMotorCurrent = table.get("wheelMotorCurrent", wheelMotorCurrent);
+			wheelRPM = table.get("wheelRPM", wheelRPM);
+			toggleSensor = table.get("toggleSensor", toggleSensor);
+			distanceSensorCM = table.get("distanceSensorCM", distanceSensorCM);
+		}
+	}
+
+	/** Updates the set of loggable inputs. */
+	public void updateInputs(IntakeInputs inputs);
+
+	public void setArmAngle(double angle);
+
+	public void setIntakeSpeed(double speed);
+}

src/main/java/frc/robot/intake/IO_IntakeReal.java

@@ -0,0 +1,49 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.intake;
+
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import frc.robot.util.IRBeamBreak;
+
+public class IO_IntakeReal implements IO_IntakeBase {
+
+	private SparkMax armMotor;
+	private SparkMax wheelMotor;
+
+	private IRBeamBreak toggleSensor;
+
+	public IO_IntakeReal() {
+
+		armMotor = new SparkMax(0, MotorType.kBrushless);
+		wheelMotor = new SparkMax(0, MotorType.kBrushless);
+		toggleSensor = new IRBeamBreak(0);
+	}
+
+	@Override
+	public void updateInputs(IntakeInputs inputs) {
+
+		inputs.armAngleDegrees = armMotor.getEncoder().getPosition();
+		inputs.armMotorCurrent = armMotor.getOutputCurrent();
+		inputs.wheelMotorCurrent = wheelMotor.getOutputCurrent();
+		inputs.wheelRPM = wheelMotor.getEncoder().getVelocity();
+		inputs.toggleSensor = toggleSensor.getState();
+		inputs.distanceSensorCM = 0;
+	}
+
+	@Override
+	public void setArmAngle(double angle) {
+		armMotor.getClosedLoopController().setReference(angle, ControlType.kMAXMotionPositionControl);
+	}
+
+	@Override
+	public void setIntakeSpeed(double speed) {
+		wheelMotor.set(speed);
+	}
+}

src/main/java/frc/robot/intake/SUB_Intake.java

@@ -0,0 +1,70 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class SUB_Intake extends SubsystemBase {
+
+	public enum State {
+		STOWED(-15, 0.0),
+		CORAL_STATION(120, -0.3),
+		ALGAE_GROUND(45, -1.0),
+		ALGAE_REMOVAL(85, 0.8),
+		ALGAE_PROCESSOR(85, 1.0),
+		ALGAE_BARGE(135, 0.25),
+		L1_SCORING(30, 0.8),
+		L2_L3_SCORING(45, 0.8),
+		L4_SCORING(30, 0.8);
+
+		private final int deg;
+		private final double speed;
+
+		State(int deg, double speed) {
+			this.deg = deg;
+			this.speed = speed;
+		}
+
+		public int getDeg() {
+			return deg;
+		}
+
+		public double getSpeed() {
+			return speed;
+		}
+	}
+
+	private final IO_IntakeBase io;
+
+	private final IO_IntakeBase.IntakeInputs inputs = new IO_IntakeBase.IntakeInputs();
+
+	public SUB_Intake(IO_IntakeBase io) {
+		this.io = io;
+	}
+
+	@Override
+	public void periodic() {
+
+		// Update inputs
+		io.updateInputs(inputs);
+
+		// Process inputs
+		Logger.processInputs("Intake", inputs);
+	}
+
+	public Command setState(State state) {
+		return new InstantCommand(
+				() -> {
+					io.setArmAngle(state.getDeg());
+					io.setIntakeSpeed(state.getSpeed());
+				});
+	}
+}

src/main/java/frc/robot/swerve/IO_SwerveReal.java

@@ -33,6 +33,7 @@
 import swervelib.telemetry.SwerveDriveTelemetry;
 
 public class IO_SwerveReal implements IO_SwerveBase {
+
 	private final SwerveDrive swerveDrive;
 	private final SwerveInputs inputs = new SwerveInputs();
 
@@ -53,7 +54,7 @@ public IO_SwerveReal(File directory) {
 
 		// Configure SwerveDrive settings
 		swerveDrive.setHeadingCorrection(false);
-		swerveDrive.setCosineCompensator(false);
+		swerveDrive.setCosineCompensator(true);
 		swerveDrive.setAngularVelocityCompensation(true, false, 0.1);
 		swerveDrive.setModuleEncoderAutoSynchronize(false, 1);
 		swerveDrive.pushOffsetsToEncoders();

src/main/java/frc/robot/util/IRBeamBreak.java

@@ -0,0 +1,34 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+
+public class IRBeamBreak {
+
+	private final DigitalInput sensor;
+
+	/**
+	 * Creates a new IRBeamBreak digital IR sensor object.
+	 *
+	 * @param channel The digital input channel
+	 */
+	public IRBeamBreak(int channel) {
+		sensor = new DigitalInput(channel);
+	}
+
+	/**
+	 * Get the the current state of the sensor. Returns true if the sensor is triggered (beam is
+	 * broken).
+	 *
+	 * @return The state obtained from the sensor
+	 */
+	public boolean getState() {
+		return !sensor.get();
+	}
+}

src/main/java/frc/robot/util/SparkConfigurator.java

@@ -0,0 +1,35 @@
+// Copyright (c) 2024 - 2025 : FRC 2106 : The Junkyard Dogs
+// https://www.team2106.org
+
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+
+package frc.robot.util;
+
+import com.revrobotics.spark.SparkMax;
+
+public class SparkConfigurator {
+
+	public static void configureSparkMaxForPID(SparkMax sparkMax, double p, double i, double d) {}
+
+	/*
+			SparkMaxConfig sparkMaxConfig = new SparkMaxConfig();
+			ClosedLoopConfig closedLoopConfig = new ClosedLoopConfig();
+
+			closedLoopConfig.apply(
+					new MAXMotionConfig().
+			)
+
+
+
+			sparkMax.configure(
+
+			new SparkMaxConfig().apply(
+					new ClosedLoopConfig().apply
+			),
+
+			), SparkBase.ResetMode.kNoResetSafeParameters, SparkBase.PersistMode.kPersistParameters);
+	}
+			*/
+}