diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 58cef45..9db71e6 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/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: diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae4a59a..af2f8c8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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,23 +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)); - - // Hello rae. You are a running! } private void configureWebserverCommands() { @@ -81,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<)⅁ <- pov: The Junkyard Dogs diff --git a/src/main/java/frc/robot/constants/BuildConstants.java b/src/main/java/frc/robot/constants/BuildConstants.java index 1285e22..693d304 100644 --- a/src/main/java/frc/robot/constants/BuildConstants.java +++ b/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() {} diff --git a/src/main/java/frc/robot/intake/IO_IntakeBase.java b/src/main/java/frc/robot/intake/IO_IntakeBase.java new file mode 100644 index 0000000..ab3be7c --- /dev/null +++ b/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); +} diff --git a/src/main/java/frc/robot/intake/IO_IntakeReal.java b/src/main/java/frc/robot/intake/IO_IntakeReal.java new file mode 100644 index 0000000..0a74236 --- /dev/null +++ b/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); + } +} diff --git a/src/main/java/frc/robot/intake/SUB_Intake.java b/src/main/java/frc/robot/intake/SUB_Intake.java new file mode 100644 index 0000000..86482c7 --- /dev/null +++ b/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()); + }); + } +} diff --git a/src/main/java/frc/robot/swerve/IO_SwerveReal.java b/src/main/java/frc/robot/swerve/IO_SwerveReal.java index 055e74b..64b5d80 100644 --- a/src/main/java/frc/robot/swerve/IO_SwerveReal.java +++ b/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(); diff --git a/src/main/java/frc/robot/util/IRBeamBreak.java b/src/main/java/frc/robot/util/IRBeamBreak.java new file mode 100644 index 0000000..8a53646 --- /dev/null +++ b/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(); + } +} diff --git a/src/main/java/frc/robot/util/SparkConfigurator.java b/src/main/java/frc/robot/util/SparkConfigurator.java new file mode 100644 index 0000000..d3cb791 --- /dev/null +++ b/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); + } + */ +}