diff --git a/Dreadbots2026 b/Dreadbots2026 new file mode 160000 index 0000000..d11e292 --- /dev/null +++ b/Dreadbots2026 @@ -0,0 +1 @@ +Subproject commit d11e292e48db7bd8f0eaaea9c28378b4ba6f4749 diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java index f1a50d3..9227f8e 100644 --- a/src/main/java/frc/robot/BuildConstants.java +++ b/src/main/java/frc/robot/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "Dreadbots2026"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 12; - public static final String GIT_SHA = "7b13ffa48be066b1546d17fa83b739acae12826e"; - public static final String GIT_DATE = "2026-01-27 18:12:48 EST"; - public static final String GIT_BRANCH = "main"; - public static final String BUILD_DATE = "2026-01-28 12:56:56 EST"; - public static final long BUILD_UNIX_TIME = 1769623016280L; + public static final int GIT_REVISION = 36; + public static final String GIT_SHA = "0248de651fff2924bb45cef2f3de7f00590990e8"; + public static final String GIT_DATE = "2026-02-07 14:11:40 EST"; + public static final String GIT_BRANCH = "Climb"; + public static final String BUILD_DATE = "2026-02-09 17:42:39 EST"; + public static final long BUILD_UNIX_TIME = 1770676959719L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index c50f217..98b65b2 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -15,18 +15,35 @@ public static enum Mode { REPLAY } + public static class IndexerConstants { + public static final double INTAKE_VOLTAGE = 1.5; + public static final double OUTAKE_VOLTAGE = -5.0; + public static final int MOTOR_ID = 1; + } + + public static class FlywheelConstants { + public static final double SHOOT_VOLTAGE = 3.0; + public static final int MOTOR_ID_1 = 13; + public static final int MOTOR_ID_2 = 14; + } + + public static class ClimbConstants { + public static final double INTAKE_VOLTAGE = 1.5; + public static final double OUTAKE_VOLTAGE = -1.5; + public static final int MOTOR_ID = 1; + } + public static final class AutoAlignConstants { - public static final double TRANSLATION_KP = 0.5; - public static final double TRANSLATION_KD = 0.0; - public static final double TRANSLATION_VELOCITY = 3.5; // Meters/Sec - public static final double TRANSLATION_ACCELERATION = 3.5; // Meters/Sec^2 - public static final double TRANSLATION_JERK = 3.0; // Meters/Sec^3 - public static final double ROTATION_KP = 2.0; - public static final double ROTATION_KD = 0.0; - public static final double ROTATION_MAX_VELOCITY = 5.0; - public static final double ROTATION_MAX_ACCELERATION = 10.0; - public static final double LEFT_REEF_BRANCH_OFFSET = Units.inchesToMeters(11.5 / 2.0); - public static final double RIGHT_REEF_BRANCH_OFFSET = Units.inchesToMeters(13.5 / 2.0); - - } -} + public static final double TRANSLATION_KP = 0.5; + public static final double TRANSLATION_KD = 0.0; + public static final double TRANSLATION_VELOCITY = 3.5; // Meters/Sec + public static final double TRANSLATION_ACCELERATION = 3.5; // Meters/Sec^2 + public static final double TRANSLATION_JERK = 3.0; // Meters/Sec^3 + public static final double ROTATION_KP = 2.0; + public static final double ROTATION_KD = 0.0; + public static final double ROTATION_MAX_VELOCITY = 5.0; + public static final double ROTATION_MAX_ACCELERATION = 10.0; + public static final double LEFT_REEF_BRANCH_OFFSET = Units.inchesToMeters(11.5 / 2.0); + public static final double RIGHT_REEF_BRANCH_OFFSET = Units.inchesToMeters(13.5 / 2.0); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 7457eb7..d5f0795 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -99,11 +99,6 @@ public void robotPeriodic() { Threads.setCurrentThreadPriority(false, 10); } - /** This function is called once when the robot is disabled. */ - @Override - public void disabledInit() {} - - /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() {} @@ -138,7 +133,6 @@ public void teleopInit() { @Override public void teleopPeriodic() {} - /** This function is called once when test mode is enabled. */ @Override public void testInit() { // Cancels all running commands at the start of test mode. @@ -156,4 +150,6 @@ public void simulationInit() {} /** This function is called periodically whilst in simulation. */ @Override public void simulationPeriodic() {} + + } \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a267e8e..131e31d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,9 @@ import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOCamera; +import frc.robot.subsystems.flywheel.*; +import frc.robot.subsystems.climb.*; + public class RobotContainer { private final CommandXboxController primaryController = new CommandXboxController(0); @@ -30,94 +33,150 @@ public class RobotContainer { private final Drive drive; private final Vision vision; private final List cameras; + private final Flywheel flywheel; + private final Climb climb; public RobotContainer() { switch (Constants.currentMode) { - case REAL: - - drive = - new Drive( - new GyroIONavX(), - new ModuleIOSpark(0), - new ModuleIOSpark(1), - new ModuleIOSpark(2), - new ModuleIOSpark(3)); - // turret = new Turret(new TurretIOSparkMax()); - cameras = List.of( - new VisionCamera( - new VisionIOCamera(VisionConstants.frontRightCameraName), - 0), - new VisionCamera( - new VisionIOCamera(VisionConstants.frontLeftCameraName), - 1), - new VisionCamera( - new VisionIOCamera(VisionConstants.backCameraName), - 2)); - vision = new Vision( - cameras, - drive::addVisionMeasurement, - drive::getPose); - CameraServer.startAutomaticCapture(0); - break; - case SIM: - - drive = - new Drive( - new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); - cameras = List.of( - new VisionCamera( - new VisionIOCamera(VisionConstants.frontRightCameraName), - 0), - new VisionCamera( - new VisionIOCamera(VisionConstants.frontLeftCameraName), - 1), - new VisionCamera( - new VisionIOCamera(VisionConstants.backCameraName), - 2)); - vision = new Vision( - cameras, - drive::addVisionMeasurement, - drive::getPose); - - // turret = new Turret(new TurretIOSim()); - break; - - default: - - drive = + case REAL: + + // drive = + // new Drive( + // new GyroIONavX(), + // new ModuleIOSpark(0), + // new ModuleIOSpark(1), + // new ModuleIOSpark(2), + // new ModuleIOSpark(3)); + + + drive = + new Drive( + new GyroIO() {}, + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + + // turret = new Turret(new TurretIOSparkMax()); + // cameras = List.of( + // new VisionCamera( + // new VisionIOCamera(VisionConstants.frontRightCameraName), + // 0), + // new VisionCamera( + // new VisionIOCamera(VisionConstants.frontLeftCameraName), + // 1), + // new VisionCamera( + // new VisionIOCamera(VisionConstants.backCameraName), + // 2)); + // vision = new Vision( + // cameras, + // drive::addVisionMeasurement, + // drive::getPose); + // CameraServer.startAutomaticCapture(0); + + cameras = List.of( + new VisionCamera( + new VisionIOCamera(VisionConstants.frontRightCameraName), + 0), + new VisionCamera( + new VisionIOCamera(VisionConstants.frontLeftCameraName), + 1), + new VisionCamera( + new VisionIOCamera(VisionConstants.backCameraName), + 2)); + vision = new Vision( + cameras, + drive::addVisionMeasurement, + drive::getPose); + + + flywheel = new Flywheel(new FlywheelIOSim()); + //flywheel = new Flywheel(new FlywheelIOSparkFlex()); + climb = new Climb(new ClimbIOSparkFlex()); + break; + + + + + + + case SIM: + + drive = new Drive( new GyroIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}, - new ModuleIO() {}); - - cameras = List.of( - new VisionCamera( - new VisionIO() {}, - 0), - new VisionCamera( - new VisionIO() {}, - 1), - new VisionCamera( - new VisionIO() {}, - 2)); - vision = new Vision( - cameras, - drive::addVisionMeasurement, - drive::getPose); - // turret = new Turret(new TurretIO() {}); - break; + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim(), + new ModuleIOSim()); + cameras = List.of( + new VisionCamera( + new VisionIOCamera(VisionConstants.frontRightCameraName), + 0), + new VisionCamera( + new VisionIOCamera(VisionConstants.frontLeftCameraName), + 1), + new VisionCamera( + new VisionIOCamera(VisionConstants.backCameraName), + 2)); + vision = new Vision( + cameras, + drive::addVisionMeasurement, + drive::getPose); + + flywheel = new Flywheel(new FlywheelIOSim()); + // turret = new Turret(new TurretIOSim()); + + + climb = new Climb(new ClimbIOSim()); + break; + + + + + default: + + drive = + new Drive( + new GyroIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}, + new ModuleIO() {}); + + cameras = List.of( + new VisionCamera( + new VisionIO() {}, + 0), + new VisionCamera( + new VisionIO() {}, + 1), + new VisionCamera( + new VisionIO() {}, + 2)); + vision = new Vision( + cameras, + drive::addVisionMeasurement, + drive::getPose); + + climb = new Climb(new ClimbIO() {}); + flywheel = new Flywheel(new FlywheelIOSim()); + // turret = new Turret(new TurretIO() {}); + break; + } + configureButtonBindings(); } - - configureButtonBindings(); - } - private void configureButtonBindings() { + + + + + + + + +//This Configures the Button's bindings for the controller with the system for the robot +private void configureButtonBindings() { VisionUtil.getApriltagPose(1); drive.setDefaultCommand( DriveCommands.joystickDrive( @@ -131,9 +190,16 @@ private void configureButtonBindings() { () -> drive.setPose( new Pose2d(vision.getLastVisionPose().getTranslation(), new Rotation2d())), drive).ignoringDisable(true)); - } - public Command getAutonomousCommand() { + + + // //Slapdown Algae Buttons (Left Trigger Intakes wheels/ Right Trigger Outakes wheels) (D-pad Up will pull in the intake system while D-pad down will push the intake system out to grab Algae) + primaryController.rightTrigger().whileTrue(climb.doClimbSequence()); + primaryController.leftTrigger().whileTrue(climb.unClimbSequence()); + + + } + public Command getAutonomousCommand() { return null; //choreoAutoChooser.selectedCommand(); } diff --git a/src/main/java/frc/robot/subsystems/climb/Climb.java b/src/main/java/frc/robot/subsystems/climb/Climb.java index 04d2ba8..c5d637a 100644 --- a/src/main/java/frc/robot/subsystems/climb/Climb.java +++ b/src/main/java/frc/robot/subsystems/climb/Climb.java @@ -1,5 +1,85 @@ package frc.robot.subsystems.climb; -public class Climb { +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.Logger; +import org.littletonrobotics.junction.inputs.LoggableInputs; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants.ClimbConstants; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.DriverStation; + + +public class Climb extends SubsystemBase { +//Auto logging output to something + private ClimbIO io; + private ClimbIOInputsAutoLogged inputs = new ClimbIOInputsAutoLogged(); + + + //PID work? + private final PIDController pid = new PIDController(0.013, 0.0, 0); + private final TrapezoidProfile profile = new TrapezoidProfile(new TrapezoidProfile.Constraints(540, 540)); + private TrapezoidProfile.State goal = new TrapezoidProfile.State(0, 0); + private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(); + public final ArmFeedforward feedforward = new ArmFeedforward(0.00, 0.0, 0.023); + + + -} + @AutoLogOutput + // Setting up the boolean Varible, which is for right now isClimbed (Basic will be updated later) + public boolean isClimbed = false; + public Climb(ClimbIO io) { + this.io = io; + } + + public Command doClimbSequence() { + System.out.println("Climbing/Climbed"); + Logger.recordOutput("Climbing/Climbed", isClimbed); + return Commands.sequence( + Commands.startEnd( + () -> io.runVoltage(ClimbConstants.INTAKE_VOLTAGE), + () -> io.runVoltage(0.0) + ) + + ); + } + + public Command unClimbSequence() { + isClimbed = false; + System.out.println("UnClimbing/UnClimbed"); + Logger.recordOutput("unClimbSequence", isClimbed); + return Commands.sequence( + Commands.startEnd( + () -> io.runVoltage(ClimbConstants.OUTAKE_VOLTAGE), + () -> io.runVoltage(0.0) + ) + ); + } + + // Updates the inputs of ClimbIO perodic. + // ClimbIO takes the inputs and outputs of Climb from the contorller + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Climb", inputs); + if (DriverStation.isDisabled()) { + setpoint = new TrapezoidProfile.State(inputs.absolutePosition, 0); + goal = setpoint; + } + + Logger.recordOutput("Climb/SetpointPosition", setpoint.position); + Logger.recordOutput("Climb/GoalPosition", goal.position); + setpoint = profile.calculate(0.02, setpoint, goal); + io.runScrewMotorVoltage( + pid.calculate(inputs.absolutePosition, setpoint.position) + + feedforward.calculate(inputs.absolutePosition + 90, setpoint.velocity) + // use acutal position degrees to make sure that we always apply the correct gravity feed forward. + ); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIO.java b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java new file mode 100644 index 0000000..5352e52 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIO.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.climb; + +import org.littletonrobotics.junction.AutoLog; + +public interface ClimbIO { + + //Autologging the Rotations Per Minute, the Applied Volts to the motor and the Current Amps of the motor + @AutoLog + class ClimbIOInputs { + //Screw Rotations + public double RPM = 0.0; + public double absolutePosition = 0.0; + + //The Volts being applied to Screw Motor + public double appliedVolts = 0.0; + + //The Amps Curently in Screw Motor + public double currentAmps = 0.0; + } + + // functions + + //Updates inputs of Climb IO + public default void updateInputs(ClimbIOInputs inputs) {}; + + // Updates the runVoltage command to update how many volts it needs to run + public default void runVoltage(double volts) {}; + + //Changes the current limit of the volts on the motor + public default void changeCurrentLimit(double current) {}; + + //The Volts + public double screwMotorAppliedVolts = 0.0; + + //Current + public double screwMotorCurrentAmps = 0.0; + + //gets the voltage of the motor + public default void runScrewMotorVoltage(double volts) {} + + public default void stopMotors() {}; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java new file mode 100644 index 0000000..dea9ef7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOSim.java @@ -0,0 +1,39 @@ +package frc.robot.subsystems.climb; + + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + +public class ClimbIOSim implements ClimbIO { + + + + private final DCMotorSim motorSim; + private final double rollerWheelMOI = 0.5 * Units.lbsToKilograms(0.12) * Units.inchesToMeters(1.5) * Units.inchesToMeters(1.5); + public ClimbIOSim() { + this.motorSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(1), 3 * rollerWheelMOI, 1.0), + DCMotor.getNeoVortex(1) + ); + } + + + @Override + public void updateInputs(ClimbIOInputs inputs) { + motorSim.update(0.02); + + inputs.appliedVolts = 0.0; + + inputs.RPM = motorSim.getAngularVelocityRPM(); + + inputs.currentAmps = motorSim.getCurrentDrawAmps(); + + } + + @Override + public void runVoltage(double volts) { + motorSim.setInputVoltage(volts); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkFlex.java b/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkFlex.java new file mode 100644 index 0000000..dc69845 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbIOSparkFlex.java @@ -0,0 +1,35 @@ +package frc.robot.subsystems.climb; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.sim.SparkFlexSim; +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.subsystems.indexer.IndexerIO; +import frc.robot.subsystems.indexer.IndexerIO.IndexerIOInputs; + +import frc.robot.Constants.ClimbConstants; + +public class ClimbIOSparkFlex implements ClimbIO { + private SparkFlex screwMotor; + + public ClimbIOSparkFlex() { + screwMotor = new SparkFlex(ClimbConstants.MOTOR_ID, MotorType.kBrushless); + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(IdleMode.kBrake).smartCurrentLimit(50); + screwMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void updateInputs(ClimbIOInputs inputs) { + inputs.appliedVolts = screwMotor.getAppliedOutput() * screwMotor.getBusVoltage(); + inputs.currentAmps = screwMotor.getOutputCurrent(); + inputs.RPM = screwMotor.getEncoder().getVelocity(); + } + + public void runVoltage(double volts) { + screwMotor.setVoltage(volts); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java index 417ab9e..67d2072 100644 --- a/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java +++ b/src/main/java/frc/robot/subsystems/flywheel/Flywheel.java @@ -1,5 +1,40 @@ package frc.robot.subsystems.flywheel; -public class Flywheel { - +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Flywheel extends SubsystemBase { + private final FlywheelIO io; + private final FlywheelIO.FlywheelIOInputs inputs = new FlywheelIO.FlywheelIOInputs(); + + public Flywheel(FlywheelIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + public void runAtVoltage(double volts) { + io.setVoltage(volts); + } + + public double getRPM() { + return inputs.velocityRPM; + } + + public Command start() { + return startEnd( + () -> io.setVoltage(6.0), // Example voltage + () -> io.setVoltage(0.0) + ); + } + + public Command stop() { + return startEnd( + () -> io.setVoltage(0.0), + () -> {} + ); + } } diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java new file mode 100644 index 0000000..49cebb1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.flywheel; + +import org.littletonrobotics.junction.AutoLog; + +public interface FlywheelIO { + + @AutoLog + public static class FlywheelIOInputs { + public double velocityRPM = 0.0; + public double appliedVolts = 0.0; + } + + public default void updateInputs(FlywheelIOInputs inputs) {} + + public default void setVoltage(double volts) {} +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java new file mode 100644 index 0000000..6213d4a --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSim.java @@ -0,0 +1,42 @@ +package frc.robot.subsystems.flywheel; + +import edu.wpi.first.wpilibj.simulation.FlywheelSim; +import edu.wpi.first.math.system.LinearSystem; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.util.Units; + +public class FlywheelIOSim implements FlywheelIO { + + private static final LinearSystem flywheelPlant = + LinearSystemId.createFlywheelSystem( + DCMotor.getNeoVortex(2), + 0.001, + 1.0 + ); + + private final FlywheelSim sim = + new FlywheelSim( + flywheelPlant, + DCMotor.getNeoVortex(2), + 0.0 + ); + + private double appliedVolts = 0.0; + + @Override + public void setVoltage(double volts) { + appliedVolts = volts; + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + sim.setInputVoltage(appliedVolts); + sim.update(0.02); + + inputs.velocityRPM = Units.radiansPerSecondToRotationsPerMinute(sim.getAngularVelocityRadPerSec()); + + inputs.appliedVolts = appliedVolts; + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkFlex.java b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkFlex.java new file mode 100644 index 0000000..d86559e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/flywheel/FlywheelIOSparkFlex.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.flywheel; + +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 com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; + +import frc.robot.Constants.FlywheelConstants; + +public class FlywheelIOSparkFlex implements FlywheelIO { + + private final SparkFlex motor1; + private final SparkFlex motor2; + + private double appliedVolts = 0.0; + + public FlywheelIOSparkFlex() { + motor1 = new SparkFlex(FlywheelConstants.MOTOR_ID_1, MotorType.kBrushless); + SparkFlexConfig config1 = new SparkFlexConfig(); + config1 + .idleMode(IdleMode.kBrake) + .smartCurrentLimit(50); + motor1.configure(config1, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + motor2 = new SparkFlex(FlywheelConstants.MOTOR_ID_2, MotorType.kBrushless); + SparkFlexConfig config2 = new SparkFlexConfig(); + config2.follow(FlywheelConstants.MOTOR_ID_1); + motor2.configure(config2, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + @Override + public void setVoltage(double volts) { + appliedVolts = volts; + motor1.setVoltage(volts); + } + + @Override + public void updateInputs(FlywheelIOInputs inputs) { + inputs.velocityRPM = motor1.getEncoder().getVelocity(); + inputs.appliedVolts = appliedVolts; + } +} diff --git a/src/main/java/frc/robot/subsystems/hood/Hood.java b/src/main/java/frc/robot/subsystems/hood/Hood.java index 1bd18a2..ab6018b 100644 --- a/src/main/java/frc/robot/subsystems/hood/Hood.java +++ b/src/main/java/frc/robot/subsystems/hood/Hood.java @@ -1,5 +1,29 @@ package frc.robot.subsystems.hood; -public class Hood { - +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Hood extends SubsystemBase { + private final HoodIO io; + private final HoodIO.HoodIOInputs inputs = new HoodIO.HoodIOInputs(); + + public Hood(HoodIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + } + + public void setVoltage(double volts) { + io.setVoltage(volts); + } + + public double getAngleDegrees() { + return inputs.angleDeg; + } + + public double getVelocityDegPerSec() { + return inputs.velocityDegPerSec; + } } diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIO.java b/src/main/java/frc/robot/subsystems/hood/HoodIO.java new file mode 100644 index 0000000..a9e05af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodIO.java @@ -0,0 +1,16 @@ +package frc.robot.subsystems.hood; + +import org.littletonrobotics.junction.AutoLog; + +public interface HoodIO { + + @AutoLog + public static class HoodIOInputs { + public double angleDeg = 0.0; + public double velocityDegPerSec = 0.0; + public double appliedVolts = 0.0; + } + + default void updateInputs(HoodIOInputs inputs) {} + default void setVoltage(double volts) {} +} diff --git a/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java b/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java new file mode 100644 index 0000000..adc50b8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/hood/HoodIOSim.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.hood; + +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.util.Units; + +public class HoodIOSim implements HoodIO { + + private static final double kGearRatio = 100.0; + private static final double kArmLengthMeters = 0.3; + private static final double kArmMassKg = 2.0; + + private final SingleJointedArmSim sim = + new SingleJointedArmSim( + DCMotor.getNeo550(1), + kGearRatio, + SingleJointedArmSim.estimateMOI(kArmLengthMeters, kArmMassKg), + kArmLengthMeters, + Units.degreesToRadians(10), // min angle + Units.degreesToRadians(45), // max angle + true, // simulate gravity + Units.degreesToRadians(10) // initial angle + ); + + private double appliedVolts = 0.0; + + @Override + public void setVoltage(double volts) { + appliedVolts = volts; + } + + @Override + public void updateInputs(HoodIOInputs inputs) { + sim.setInputVoltage(appliedVolts); + sim.update(0.02); + + inputs.angleDeg = Units.radiansToDegrees(sim.getAngleRads()); + + inputs.velocityDegPerSec = Units.radiansToDegrees(sim.getVelocityRadPerSec()); + + inputs.appliedVolts = appliedVolts; + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/Indexer.java b/src/main/java/frc/robot/subsystems/indexer/Indexer.java new file mode 100644 index 0000000..5afb176 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/Indexer.java @@ -0,0 +1,38 @@ +package frc.robot.subsystems.indexer; + +import frc.robot.Constants.IndexerConstants; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + + +public class Indexer extends SubsystemBase { + + private IndexerIOInputsAutoLogged inputs = new IndexerIOInputsAutoLogged(); + private IndexerIO io; + private boolean isIntaking = false; + private boolean hasGamepiece = false; + + + public Indexer(IndexerIO io) { + this.io = io; + } + + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Indexer", inputs); + } + + + public Command intake() { + return startEnd( + () -> io.runVoltage(IndexerConstants.INTAKE_VOLTAGE), + () -> { io.runVoltage(0.0); isIntaking = false; } + ); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java new file mode 100644 index 0000000..5af350b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIO.java @@ -0,0 +1,29 @@ +package frc.robot.subsystems.indexer; + + +import org.littletonrobotics.junction.AutoLog; + + +public interface IndexerIO { + + + @AutoLog + public static class IndexerIOInputs { + public double RPM = 0.0; + + + public double appliedVolts = 0.0; + + + public double currentAmps = 0.0; + } + public default void updateInputs(IndexerIOInputs inputs) {} + + + public default void runVoltage(double volts) {}; + + + public default void changeCurrentLimit(double current) {}; + + +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java new file mode 100644 index 0000000..13b3f58 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSim.java @@ -0,0 +1,45 @@ +package frc.robot.subsystems.indexer; + + +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.math.system.plant.LinearSystemId; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.simulation.DCMotorSim; + + +public class IndexerIOSim implements IndexerIO { + + + + private final DCMotorSim motorSim; + private final double rollerWheelMOI = 0.5 * Units.lbsToKilograms(0.12) * Units.inchesToMeters(1.5) * Units.inchesToMeters(1.5); + public IndexerIOSim() { + this.motorSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(DCMotor.getNeoVortex(1), 3 * rollerWheelMOI, 1.0), + DCMotor.getNeoVortex(1) + ); + } + + + @Override + public void updateInputs(IndexerIOInputs inputs) { + motorSim.update(0.02); + + + inputs.appliedVolts = 0.0; + + + inputs.RPM = motorSim.getAngularVelocityRPM(); + + + inputs.currentAmps = motorSim.getCurrentDrawAmps(); + + + } + + + @Override + public void runVoltage(double volts) { + motorSim.setInputVoltage(volts); + } +} diff --git a/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkFlex.java b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkFlex.java new file mode 100644 index 0000000..69e898d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/indexer/IndexerIOSparkFlex.java @@ -0,0 +1,31 @@ +package frc.robot.subsystems.indexer; + +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import frc.robot.Constants.IndexerConstants; + +public class IndexerIOSparkFlex implements IndexerIO { + private final SparkFlex motor; + + public IndexerIOSparkFlex() { + this.motor = new SparkFlex(IndexerConstants.MOTOR_ID, MotorType.kBrushless); + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(IdleMode.kBrake).smartCurrentLimit(50); + motor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void updateInputs(IndexerIOInputs inputs) { + inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); + inputs.currentAmps = motor.getOutputCurrent(); + inputs.RPM = motor.getEncoder().getVelocity(); + } + + public void runVoltage(double volts) { + motor.setVoltage(volts); + } +} diff --git a/vendordeps/AmLib-2026.0.1.json b/vendordeps/AmLib-2026.0.2.json similarity index 82% rename from vendordeps/AmLib-2026.0.1.json rename to vendordeps/AmLib-2026.0.2.json index 90df51e..1d8ef5d 100644 --- a/vendordeps/AmLib-2026.0.1.json +++ b/vendordeps/AmLib-2026.0.2.json @@ -1,26 +1,26 @@ { - "fileName": "AmLib-2026.0.1.json", + "fileName": "AmLib-2026.0.2.json", "name": "AndyMark AM Library", - "version": "2026.0.1", + "version": "2026.0.2", "frcYear": "2026", "uuid": "f463c495-f27c-4102-9dde-af4e65a3eb2d", "mavenUrls": [ "https://andymarkproductsoftware.github.io/amlib-vendordep/repo/2026/" ], - "jsonUrl": "https://andymarkproductsoftware.github.io/amlib-vendordep/vendordeps/2026/AmLib-2026.0.1.json", + "jsonUrl": "https://andymarkproductsoftware.github.io/amlib-vendordep/vendordeps/2026/AmLib-2026.0.2.json", "jniDependencies": [], "javaDependencies": [ { "groupId": "com.andymark.frc", "artifactId": "AmLib-java", - "version": "2026.0.1" + "version": "2026.0.2" } ], "cppDependencies": [ { "groupId": "com.andymark.frc", "artifactId": "AmLib-cpp", - "version": "2026.0.1", + "version": "2026.0.2", "libName": "AmLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/DogLog.json b/vendordeps/DogLog.json index 73503e0..de3ef13 100644 --- a/vendordeps/DogLog.json +++ b/vendordeps/DogLog.json @@ -3,7 +3,7 @@ { "groupId": "com.github.jonahsnider", "artifactId": "doglog", - "version": "2026.3.1" + "version": "2026.4.0" } ], "fileName": "DogLog.json", @@ -15,6 +15,6 @@ "https://jitpack.io" ], "cppDependencies": [], - "version": "2026.3.1", + "version": "2026.4.0", "uuid": "65592ce1-2251-4a31-8e4b-2df20dacebe4" } \ No newline at end of file diff --git a/vendordeps/LumynLabs-2026.0.0.json b/vendordeps/LumynLabs-2026.1.2.json similarity index 60% rename from vendordeps/LumynLabs-2026.0.0.json rename to vendordeps/LumynLabs-2026.1.2.json index 7880bd2..a3d700c 100644 --- a/vendordeps/LumynLabs-2026.0.0.json +++ b/vendordeps/LumynLabs-2026.1.2.json @@ -1,7 +1,7 @@ { - "fileName": "LumynLabs-2026.0.0.json", + "fileName": "LumynLabs-2026.1.2.json", "name": "LumynLabs", - "version": "2026.0.0", + "version": "2026.1.2", "frcYear": "2026", "uuid": "eebd34fc-a6d3-48a2-a286-ac1cec59ab89", "mavenUrls": [ @@ -12,29 +12,14 @@ { "groupId": "com.lumynlabs.frc", "artifactId": "LumynLabs-java", - "version": "2026.0.0" + "version": "2026.1.2" } ], "jniDependencies": [ { "groupId": "com.lumynlabs.frc", "artifactId": "LumynLabs-driver", - "version": "2026.0.0", - "skipInvalidPlatforms": true, - "isJar": false, - "validPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] - }, - { - "groupId": "com.lumynlabs.frc", - "artifactId": "LumynLabs-common", - "version": "2026.0.0", + "version": "2026.1.2", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -51,7 +36,7 @@ { "groupId": "com.lumynlabs.frc", "artifactId": "LumynLabs-cpp", - "version": "2026.0.0", + "version": "2026.1.2", "libName": "Lumyn", "headerClassifier": "headers", "sharedLibrary": true, @@ -68,7 +53,7 @@ { "groupId": "com.lumynlabs.frc", "artifactId": "LumynLabs-driver", - "version": "2026.0.0", + "version": "2026.1.2", "libName": "LumynDriver", "headerClassifier": "headers", "sharedLibrary": true, @@ -81,23 +66,6 @@ "linuxarm32", "osxuniversal" ] - }, - { - "groupId": "com.lumynlabs.frc", - "artifactId": "LumynLabs-common", - "version": "2026.0.0", - "libName": "LumynCommon", - "headerClassifier": "headers", - "sharedLibrary": true, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxarm64", - "linuxx86-64", - "linuxathena", - "linuxarm32", - "osxuniversal" - ] } ] } \ No newline at end of file diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json index a1bc5a5..afac990 100644 --- a/vendordeps/photonlib.json +++ b/vendordeps/photonlib.json @@ -1,7 +1,7 @@ { "fileName": "photonlib.json", "name": "photonlib", - "version": "v2026.1.1", + "version": "v2026.2.1", "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", "frcYear": "2026", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.2.1", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -28,7 +28,7 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-cpp", - "version": "v2026.1.1", + "version": "v2026.2.1", "libName": "photonlib", "headerClassifier": "headers", "sharedLibrary": true, @@ -43,7 +43,7 @@ { "groupId": "org.photonvision", "artifactId": "photontargeting-cpp", - "version": "v2026.1.1", + "version": "v2026.2.1", "libName": "photontargeting", "headerClassifier": "headers", "sharedLibrary": true, @@ -60,12 +60,12 @@ { "groupId": "org.photonvision", "artifactId": "photonlib-java", - "version": "v2026.1.1" + "version": "v2026.2.1" }, { "groupId": "org.photonvision", "artifactId": "photontargeting-java", - "version": "v2026.1.1" + "version": "v2026.2.1" } ] } \ No newline at end of file diff --git a/vendordeps/yams.json b/vendordeps/yams.json index 01bfad1..9557c14 100644 --- a/vendordeps/yams.json +++ b/vendordeps/yams.json @@ -1,7 +1,7 @@ { "fileName": "yams.json", "name": "Yet Another Mechanism System", - "version": "2026.1.23", + "version": "2026.2.3", "frcYear": "2026", "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "yams", "artifactId": "YAMS-java", - "version": "2026.1.23" + "version": "2026.2.3" } ], "cppDependencies": [],