diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d9ef664..6eb9045 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -57,6 +57,19 @@ public final class Constants { public static final int operatorId = 1; public static final boolean tuningMode = false; + /** + * Current threshold that indicates an algae is in possestion + */ + public static final double HAS_ALGAE_CURRENT_THRESHOLD = 0; + + /** + * Algae misc values + */ + public static final class Algae { + public static final int VOLTAGE = 0; + public static final int NEGATIVE_VOLTAGE = 0; + public static final int BEAM_BRAKE_ID = 0; + } // Controller "3" public static final int controllerThreeId = 3; @@ -71,8 +84,16 @@ public static final class Motors { * Primary Coral Scoring CAN id's */ public static final class PrimaryCoralScoring { - public static final int Coral_Scoring_NEO_ID = 8; + public static final int Coral_Scoring_NEO_ID = 5; } + + /** + * Algae Motor CAN id's + */ + public static final class AlgaeMotors { + public static final int ALGAE_MOTOR_ID = 6; + } + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 49fa147..4abf8a0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,13 +7,13 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.LEDPattern; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.lib.util.viz.FieldViz; import frc.lib.util.viz.Viz2025; import frc.robot.Robot.RobotRunType; @@ -24,6 +24,8 @@ import frc.robot.subsystems.coral.CoralScoring; import frc.robot.subsystems.coral.CoralScoringIO; import frc.robot.subsystems.coral.CoralScoringReal; +import frc.robot.subsystems.elevator_algae.ElevatorAlgae; +import frc.robot.subsystems.elevator_algae.ElevatorAlgaeReal; import frc.robot.subsystems.swerve.Swerve; import frc.robot.subsystems.swerve.SwerveIO; import frc.robot.subsystems.swerve.SwerveReal; @@ -47,6 +49,7 @@ public class RobotContainer { public final CommandXboxController driver = new CommandXboxController(Constants.driverId); public final CommandXboxController controllerThree = new CommandXboxController(Constants.controllerThreeId); + public final CommandXboxController operator = new CommandXboxController(1); /** Simulation */ @@ -60,11 +63,15 @@ public class RobotContainer { private final RobotState state; /* Subsystems */ - + private ElevatorAlgae s_ElevatorAlgae; private LEDs leds = new LEDs(); private final Swerve s_Swerve; private final Vision s_Vision; private CoralScoring coralScoring; + + /* Triggers */ + private Trigger algaeInIntake = new Trigger(() -> s_ElevatorAlgae.hasAlgae()); + private Climber climb; /** @@ -79,6 +86,7 @@ public RobotContainer(RobotRunType runtimeType) { s_Swerve = new Swerve(state, new SwerveReal()); s_Vision = new Vision(state, VisionReal::new); coralScoring = new CoralScoring(new CoralScoringReal()); + s_ElevatorAlgae = new ElevatorAlgae(new ElevatorAlgaeReal()); climb = new Climber(new ClimberReal()); break; case kSimulation: @@ -112,6 +120,13 @@ public RobotContainer(RobotRunType runtimeType) { * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings(RobotRunType runtimeType) { + algaeInIntake.onTrue(leds.blinkLEDs(Color.kCyan)); + operator.rightTrigger() + .whileTrue(s_ElevatorAlgae.setMotorVoltageCommand(Constants.Algae.VOLTAGE)); + operator.leftTrigger() + .whileTrue(s_ElevatorAlgae.setMotorVoltageCommand(Constants.Algae.NEGATIVE_VOLTAGE)); + + driver.y().onTrue(new InstantCommand(() -> s_Swerve.resetFieldRelativeOffset())); driver.a().onTrue(new Command() { Timer timer = new Timer(); @@ -155,10 +170,11 @@ public boolean isFinished() { driver.x().onTrue(new InstantCommand(() -> { s_Swerve.resetOdometry(new Pose2d(7.24, 4.05, Rotation2d.kZero)); })); - driver.y().whileTrue(coralScoring.runScoringMotor(2)); + operator.x().whileTrue(coralScoring.runScoringMotor(2)); driver.rightStick().whileTrue(climb.runClimberMotorCommand()); controllerThree.y().whileTrue(climb.resetClimberCommand()); + } /** @@ -168,8 +184,7 @@ public boolean isFinished() { public void configureTriggerBindings() { coralScoring.intakedCoralRight.onTrue(leds.setLEDsSolid(Color.kRed).withTimeout(5)); coralScoring.intakedCoralRight.onTrue(coralScoring.runPreScoringMotor(2)); - coralScoring.outtakedCoral - .onTrue(leds.blinkLEDs(LEDPattern.solid(Color.kCyan)).withTimeout(5)); + coralScoring.outtakedCoral.onTrue(leds.blinkLEDs(Color.kCyan).withTimeout(5)); climb.resetButton.and(controllerThree.y()).onTrue(climb.restEncoder()); } diff --git a/src/main/java/frc/robot/subsystems/LEDs.java b/src/main/java/frc/robot/subsystems/LEDs.java index b440c2e..2addb4a 100644 --- a/src/main/java/frc/robot/subsystems/LEDs.java +++ b/src/main/java/frc/robot/subsystems/LEDs.java @@ -41,9 +41,10 @@ public LEDs() { * * @return sets leds to blink */ - public Command blinkLEDs(LEDPattern mainColor) { - LEDPattern blinkPattern = mainColor.blink(Seconds.of(1)); - return run(() -> blinkPattern.applyTo(buffer)); + public Command blinkLEDs(Color mainColor) { + LEDPattern colorToPattern = LEDPattern.solid(mainColor); + LEDPattern blinkPattern = colorToPattern.blink(Seconds.of(1)); + return run(() -> blinkPattern.applyTo(buffer)).withTimeout(2); } /** diff --git a/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgae.java b/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgae.java new file mode 100644 index 0000000..7eb54c1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgae.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems.elevator_algae; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +/** + * Elevator Algae class + */ +public class ElevatorAlgae extends SubsystemBase { + ElevatorAlgaeIO io; + AlgaeIOInputsAutoLogged inputs = new AlgaeIOInputsAutoLogged(); + + /* + * Constructor for Elevator Algae class + */ + public ElevatorAlgae(ElevatorAlgaeIO io) { + this.io = io; + io.updateInputs(inputs); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + + public void setAlgaeMotorVoltage(double voltage) { // set motor speed function + io.setAlgaeMotorVoltage(voltage); + } + + public boolean hasAlgae() { + return inputs.algaeMotorCurrent > Constants.HAS_ALGAE_CURRENT_THRESHOLD; + } + + public Command setMotorVoltageCommand(double speed) { // set motor speed Command + return runEnd(() -> setAlgaeMotorVoltage(speed), () -> setAlgaeMotorVoltage(0)) + .until(() -> hasAlgae()); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgaeIO.java b/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgaeIO.java new file mode 100644 index 0000000..21ddadc --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgaeIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.elevator_algae; + +import org.littletonrobotics.junction.AutoLog; + +/** + * elevator algae io class + */ +public interface ElevatorAlgaeIO { + /** + * Elevator Algae inputs + */ + @AutoLog + public class AlgaeIOInputs { + double algaeMotorCurrent; + boolean beamBrakeStatus; + } + + public default void setAlgaeMotorVoltage(double voltage) {} + + public default void updateInputs(AlgaeIOInputs inputs) {} + +} diff --git a/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgaeReal.java b/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgaeReal.java new file mode 100644 index 0000000..825ef77 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator_algae/ElevatorAlgaeReal.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.elevator_algae; + +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import frc.robot.Constants; + +/** + * Algae Real Class + */ +public class ElevatorAlgaeReal implements ElevatorAlgaeIO { + private final SparkFlex algaeMotor = // Algae motor + new SparkFlex(Constants.Motors.AlgaeMotors.ALGAE_MOTOR_ID, MotorType.kBrushless); + private final SparkFlexConfig algaeMotorConfig = new SparkFlexConfig(); + + /** + * Algae Real constructor + */ + public ElevatorAlgaeReal() { + algaeMotorConfig.idleMode(IdleMode.kBrake); + algaeMotor.configure(algaeMotorConfig, ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters); + } + + @Override + public void updateInputs(AlgaeIOInputs inputs) { // update inputs to IO layer + inputs.algaeMotorCurrent = algaeMotor.getOutputCurrent(); + } + + @Override + public void setAlgaeMotorVoltage(double voltage) { // set hardware speed + algaeMotor.setVoltage(voltage); + } + +} diff --git a/vendordeps/PathplannerLib-2025.2.1.json b/vendordeps/PathplannerLib.json similarity index 87% rename from vendordeps/PathplannerLib-2025.2.1.json rename to vendordeps/PathplannerLib.json index 71e25f3..305fa6d 100644 --- a/vendordeps/PathplannerLib-2025.2.1.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { - "fileName": "PathplannerLib-2025.2.1.json", + "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.2.1", + "version": "2025.2.2", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.1" + "version": "2025.2.2" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.1", + "version": "2025.2.2", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index a524490..a85f5c1 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.2", + "version": "0.3.6", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.2" + "version": "0.3.6" }, { "groupId": "org.dyn4j",