diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.roboRIOTeamNumberSetter/roborioteamnumbersetter.json @@ -0,0 +1 @@ +{} diff --git a/src/main/deploy/pathplanner/autos/Command Test.auto b/src/main/deploy/pathplanner/autos/Command Test.auto new file mode 100644 index 0000000..8293a44 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Command Test.auto @@ -0,0 +1,58 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "L4" + } + }, + { + "type": "wait", + "data": { + "waitTime": 2.5 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Bottom" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "Pathn" + } + } + ] + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/T1.auto b/src/main/deploy/pathplanner/autos/T1.auto index bc05e65..70a7122 100644 --- a/src/main/deploy/pathplanner/autos/T1.auto +++ b/src/main/deploy/pathplanner/autos/T1.auto @@ -36,7 +36,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T2.auto b/src/main/deploy/pathplanner/autos/T2.auto index 344e56c..96bfc65 100644 --- a/src/main/deploy/pathplanner/autos/T2.auto +++ b/src/main/deploy/pathplanner/autos/T2.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T3.auto b/src/main/deploy/pathplanner/autos/T3.auto index 9830fce..cf0320f 100644 --- a/src/main/deploy/pathplanner/autos/T3.auto +++ b/src/main/deploy/pathplanner/autos/T3.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T4.auto b/src/main/deploy/pathplanner/autos/T4.auto index 02511dc..80c2a58 100644 --- a/src/main/deploy/pathplanner/autos/T4.auto +++ b/src/main/deploy/pathplanner/autos/T4.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T5.auto b/src/main/deploy/pathplanner/autos/T5.auto index ec42a96..bc2c03a 100644 --- a/src/main/deploy/pathplanner/autos/T5.auto +++ b/src/main/deploy/pathplanner/autos/T5.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T6.auto b/src/main/deploy/pathplanner/autos/T6.auto index 234e882..b62a9d4 100644 --- a/src/main/deploy/pathplanner/autos/T6.auto +++ b/src/main/deploy/pathplanner/autos/T6.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -195,7 +195,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T7.auto b/src/main/deploy/pathplanner/autos/T7.auto index 2600d86..ffc16d2 100644 --- a/src/main/deploy/pathplanner/autos/T7.auto +++ b/src/main/deploy/pathplanner/autos/T7.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -195,7 +195,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T8.auto b/src/main/deploy/pathplanner/autos/T8.auto index 7f91d79..3eae6bb 100644 --- a/src/main/deploy/pathplanner/autos/T8.auto +++ b/src/main/deploy/pathplanner/autos/T8.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -195,7 +195,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -266,7 +266,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T9.auto b/src/main/deploy/pathplanner/autos/T9.auto index 152cf20..55ad0e5 100644 --- a/src/main/deploy/pathplanner/autos/T9.auto +++ b/src/main/deploy/pathplanner/autos/T9.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -195,7 +195,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -266,7 +266,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/T90.auto b/src/main/deploy/pathplanner/autos/T90.auto index e823f27..6821fa5 100644 --- a/src/main/deploy/pathplanner/autos/T90.auto +++ b/src/main/deploy/pathplanner/autos/T90.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -195,7 +195,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -266,7 +266,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -337,7 +337,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/autos/U1.auto b/src/main/deploy/pathplanner/autos/U1.auto index f2711f6..2f0d0f2 100644 --- a/src/main/deploy/pathplanner/autos/U1.auto +++ b/src/main/deploy/pathplanner/autos/U1.auto @@ -53,7 +53,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -124,7 +124,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -195,7 +195,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -266,7 +266,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] @@ -337,7 +337,7 @@ { "type": "named", "data": { - "name": "Ground" + "name": "L0" } } ] diff --git a/src/main/deploy/pathplanner/paths/Pathn.path b/src/main/deploy/pathplanner/paths/Pathn.path new file mode 100644 index 0000000..8f977e5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Pathn.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.782395471643518, + "y": 6.157399811921295 + }, + "prevControl": null, + "nextControl": { + "x": 8.532395471643518, + "y": 6.157399811921295 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.782395471643518, + "y": 6.157399811921295 + }, + "prevControl": { + "x": 9.03239547164352, + "y": 6.157399811921295 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 0.1, + "maxAcceleration": 0.1, + "maxAngularVelocity": 0.1, + "maxAngularAcceleration": 0.1, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e71d782..77b41d2 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -43,4 +43,4 @@ "{\"name\":\"Ground Intake\",\"type\":\"rounded_rect\",\"data\":{\"center\":{\"x\":0.34,\"y\":0.0},\"size\":{\"width\":0.8,\"length\":0.1},\"borderRadius\":0.02,\"strokeWidth\":0.01,\"filled\":false}}", "{\"name\":\"Circle\",\"type\":\"circle\",\"data\":{\"center\":{\"x\":-0.46,\"y\":0.0},\"radius\":0.032,\"strokeWidth\":0.01,\"filled\":true}}" ] -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 078eafa..f14d761 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -74,9 +74,9 @@ public final class Constants { // under strict caveat emptor -- and submit any error and bugfixes // via GitHub issues. private static SwerveType swerveType = SwerveType.PHOENIX6; // PHOENIX6, YAGSL - private static CTREPro phoenixPro = CTREPro.UNLICENSED; // LICENSED, UNLICENSED + private static CTREPro phoenixPro = CTREPro.LICENSED; // LICENSED, UNLICENSED private static AutoType autoType = AutoType.PATHPLANNER; // PATHPLANNER, CHOREO - private static VisionType visionType = VisionType.NONE; // PHOTON, LIMELIGHT, NONE + private static VisionType visionType = VisionType.PHOTON; // PHOTON, LIMELIGHT, NONE /** Enumerate the robot types (name your robots here) */ public static enum RobotType { @@ -192,6 +192,46 @@ public static final class FlywheelConstants { public static final PIDConstants pidSim = new PIDConstants(1.0, 0.0, 0.0); } + /** elevator subsystem constants ***************************************** */ + public static final class ElevatorConstants { + + // idle mode //6 + public static final MotorIdleMode kElevatorIdle = MotorIdleMode.BRAKE; + + // gear ratio + public static final double kElevatorGearRatio = 0.1; + + // mode real/replay + public static final double kStaticGainReal = 0.1; + public static final double kVelocityGainReal = 0.05; + // motor configs + public static final double kGReal = 0.3375; + public static final double kSReal = 0.075; + public static final double kVReal = 0.0018629; + public static final double kAReal = 0; // 0.000070378; + // ka kv values found from putting elevator at a perfect 90 degree and running sys id + public static final double kPReal = 17.983; + public static final double kIReal = 0; + public static final double kDReal = 0; + + // mode sim + public static final double kStaticGainSim = 0.1; + public static final double kVelocityGainSim = 0.05; + // motor configs + public static final double kGSim = 0; + public static final double kSSim = 0; + public static final double kVSim = 0; + public static final double kASim = 0; + public static final double kPSim = 0; + public static final double kISim = 0; + public static final double kDSim = 0; + + // Magic motion constants + public static final double kVelocity = 1.4; + public static final double kAcceleration = 2.8; + public static final double kJerk = 0; + } + /** Accelerometer Constants ********************************************** */ public static class AccelerometerConstants { @@ -316,16 +356,24 @@ public static class VisionConstants { /** Vision Camera Posses ************************************************* */ public static class Cameras { // Camera names, must match names configured on coprocessor - public static String camera0Name = "camera_0"; - public static String camera1Name = "camera_1"; + public static String camera0Name = "Photon_BW1"; + public static String camera1Name = "Photon_BW2"; // ... And more, if needed // Robot to camera transforms // (ONLY USED FOR PHOTONVISION -- Limelight: configure in web UI instead) public static Transform3d robotToCamera0 = - new Transform3d(0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, 0.0)); + new Transform3d( + Units.inchesToMeters(14), + 0.0, + Units.inchesToMeters(4.25), + new Rotation3d(0.0, Units.degreesToRadians(+30), 0.0)); public static Transform3d robotToCamera1 = - new Transform3d(-0.2, 0.0, 0.2, new Rotation3d(0.0, -0.4, Math.PI)); + new Transform3d( + Units.inchesToMeters(-14), + 0.0, + Units.inchesToMeters(4.25), + new Rotation3d(0.0, 0.0, Math.PI)); // Standard deviation multipliers for each camera // (Adjust to trust some cameras more than others) @@ -341,45 +389,49 @@ public static class CANandPowerPorts { /* DRIVETRAIN CAN DEVICE IDS */ // Input the correct Power Distribution Module port for each motor!!!! - // NOTE: The CAN ID and bus are set in the Swerve Generator (Phoenix Tuner or YAGSL) // Front Left public static final RobotDeviceId FL_DRIVE = - new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 18); + new RobotDeviceId(SwerveConstants.kFLDriveMotorId, SwerveConstants.kFLDriveCanbus, 7); public static final RobotDeviceId FL_ROTATION = - new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 19); + new RobotDeviceId(SwerveConstants.kFLSteerMotorId, SwerveConstants.kFLSteerCanbus, 6); public static final RobotDeviceId FL_CANCODER = - new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kFLEncoderId, SwerveConstants.kFLEncoderCanbus, 5); // Front Right public static final RobotDeviceId FR_DRIVE = - new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 17); + new RobotDeviceId(SwerveConstants.kFRDriveMotorId, SwerveConstants.kFRDriveCanbus, 2); public static final RobotDeviceId FR_ROTATION = - new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 16); + new RobotDeviceId(SwerveConstants.kFRSteerMotorId, SwerveConstants.kFRSteerCanbus, 3); public static final RobotDeviceId FR_CANCODER = - new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kFREncoderId, SwerveConstants.kFREncoderCanbus, 4); // Back Left public static final RobotDeviceId BL_DRIVE = - new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 1); + new RobotDeviceId(SwerveConstants.kBLDriveMotorId, SwerveConstants.kBLDriveCanbus, 12); public static final RobotDeviceId BL_ROTATION = - new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 0); + new RobotDeviceId(SwerveConstants.kBLSteerMotorId, SwerveConstants.kBLSteerCanbus, 13); public static final RobotDeviceId BL_CANCODER = - new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kBLEncoderId, SwerveConstants.kBLEncoderCanbus, 14); // Back Right public static final RobotDeviceId BR_DRIVE = - new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 2); + new RobotDeviceId(SwerveConstants.kBRDriveMotorId, SwerveConstants.kBRSteerCanbus, 17); public static final RobotDeviceId BR_ROTATION = - new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 3); + new RobotDeviceId(SwerveConstants.kBRSteerMotorId, SwerveConstants.kBRSteerCanbus, 16); public static final RobotDeviceId BR_CANCODER = - new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, null); + new RobotDeviceId(SwerveConstants.kBREncoderId, SwerveConstants.kBREncoderCanbus, 15); // Pigeon public static final RobotDeviceId PIGEON = new RobotDeviceId(SwerveConstants.kPigeonId, SwerveConstants.kCANbusName, null); /* SUBSYSTEM CAN DEVICE IDS */ - // This is where mechanism subsystem devices are defined (Including ID, bus, and power port) - // Example: - public static final RobotDeviceId FLYWHEEL_LEADER = new RobotDeviceId(3, "", 8); - public static final RobotDeviceId FLYWHEEL_FOLLOWER = new RobotDeviceId(4, "", 9); + public static final RobotDeviceId ELEVATOR = new RobotDeviceId(11, "", 18); + public static final RobotDeviceId CORAL_MECH = new RobotDeviceId(16, "", 19); + public static final RobotDeviceId INTAKE_PIVOT = new RobotDeviceId(21, "", 0); + public static final RobotDeviceId INTAKE_ROLLER = new RobotDeviceId(22, "", 1); + public static final RobotDeviceId INTAKE_ENCODER = new RobotDeviceId(23, "", null); + public static final RobotDeviceId ALGAE_PIVOT = new RobotDeviceId(26, "", 10); + public static final RobotDeviceId ALGAE_ROLLER = new RobotDeviceId(27, "", 11); + public static final RobotDeviceId CLIMB = new RobotDeviceId(31, "", 8); + public static final RobotDeviceId LED = new RobotDeviceId(36, "", null); /* BEAM BREAK and/or LIMIT SWITCH DIO CHANNELS */ // This is where digital I/O feedback devices are defined @@ -390,7 +442,7 @@ public static class CANandPowerPorts { // This is where PWM-controlled devices (actuators, servos, pneumatics, etc.) // are defined // Example: - // public static final int INTAKE_SERVO = 0; + public static final int CLIMB_SERVO = 4; } /** AprilTag Field Layout ************************************************ */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5e333b9..12d511b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,12 +26,14 @@ import choreo.auto.AutoRoutine; import choreo.auto.AutoTrajectory; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.path.GoalEndState; import com.pathplanner.lib.path.PathConstraints; import com.pathplanner.lib.path.PathPlannerPath; import com.pathplanner.lib.path.Waypoint; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -40,15 +42,24 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; +import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.AprilTagConstants.AprilTagLayoutType; import frc.robot.Constants.OperatorConstants; import frc.robot.commands.DriveCommands; +import frc.robot.commands.ElevatorCommand; +import frc.robot.commands.IntakeCommand; +import frc.robot.commands.LED.LEDCommand; +import frc.robot.subsystems.Controls.CoralControl; +import frc.robot.subsystems.Intake.Intake; +import frc.robot.subsystems.Intake.IntakeIOKraken; +import frc.robot.subsystems.LED.LED; +import frc.robot.subsystems.LED.LEDIOCandle; import frc.robot.subsystems.accelerometer.Accelerometer; import frc.robot.subsystems.drive.Drive; -import frc.robot.subsystems.flywheel_example.Flywheel; -import frc.robot.subsystems.flywheel_example.FlywheelIO; -import frc.robot.subsystems.flywheel_example.FlywheelIOSim; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorIO; +import frc.robot.subsystems.elevator.ElevatorIOTalonFX; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOLimelight; @@ -66,6 +77,9 @@ /** This is the location for defining robot hardware, commands, and controller button bindings. */ public class RobotContainer { + + private final DigitalInput elevatorStop = new DigitalInput(0); + private final Trigger elevatorTrigger = new Trigger(elevatorStop::get); // **** This is a Pathplanner On-the-Fly Command ****/ // Create a list of waypoints from poses. Each pose represents one waypoint. // The rotation component of the pose should be the direction of travel. Do not use @@ -102,6 +116,9 @@ public class RobotContainer { // Replace with ``CommandPS4Controller`` or ``CommandJoystick`` if needed final CommandXboxController driverController = new CommandXboxController(0); // Main Driver + private Trigger leftBumper = driverController.leftBumper(); + private Trigger rightBumper = driverController.rightBumper(); + final CommandXboxController operatorController = new CommandXboxController(1); // Second Operator final OverrideSwitches overrides = new OverrideSwitches(2); // Console toggle switches @@ -109,11 +126,15 @@ public class RobotContainer { // These are the "Active Subsystems" that the robot controlls private final Drive m_drivebase; - private final Flywheel m_flywheel; + private final Elevator m_elevator; // These are "Virtual Subsystems" that report information but have no motors private final Accelerometer m_accel; + private final CoralControl m_coralControl = new CoralControl(); private final Vision m_vision; private final PowerMonitoring m_power; + private final Intake m_intake = new Intake(new IntakeIOKraken()); + private final LED m_led = new LED(new LEDIOCandle()); + private final DigitalInput lightStop = new DigitalInput(5); /** Dashboard inputs ***************************************************** */ // AutoChoosers for both supported path planning types @@ -143,7 +164,7 @@ public RobotContainer() { // Real robot, instantiate hardware IO implementations // YAGSL drivebase, get config from deploy directory m_drivebase = new Drive(); - m_flywheel = new Flywheel(new FlywheelIOSim()); // new Flywheel(new FlywheelIOTalonFX()); + m_elevator = new Elevator(new ElevatorIOTalonFX()); m_vision = switch (Constants.getVisionType()) { case PHOTON -> @@ -162,12 +183,13 @@ public RobotContainer() { default -> null; }; m_accel = new Accelerometer(m_drivebase.getGyro()); + break; case SIM: // Sim robot, instantiate physics sim IO implementations m_drivebase = new Drive(); - m_flywheel = new Flywheel(new FlywheelIOSim() {}); + m_elevator = new Elevator(new ElevatorIO() {}); // make elevator Io sim m_vision = new Vision( m_drivebase::addVisionMeasurement, @@ -179,7 +201,7 @@ public RobotContainer() { default: // Replayed robot, disable IO implementations m_drivebase = new Drive(); - m_flywheel = new Flywheel(new FlywheelIO() {}); + m_elevator = new Elevator(new ElevatorIO() {}); m_vision = new Vision(m_drivebase::addVisionMeasurement, new VisionIO() {}, new VisionIO() {}); m_accel = new Accelerometer(m_drivebase.getGyro()); @@ -188,7 +210,21 @@ public RobotContainer() { // In addition to the initial battery capacity from the Dashbaord, ``PowerMonitoring`` takes all // the non-drivebase subsystems for which you wish to have power monitoring; DO NOT include // ``m_drivebase``, as that is automatically monitored. - m_power = new PowerMonitoring(batteryCapacity, m_flywheel); + m_power = new PowerMonitoring(batteryCapacity, m_elevator); + + // Idk where this is suppose to go. but I think this works, just setting up auto commands + NamedCommands.registerCommand("L4", new ElevatorCommand(72, 40, 40, m_elevator)); + + NamedCommands.registerCommand("L3", new ElevatorCommand(50, 40, 40, m_elevator)); + + NamedCommands.registerCommand("L2", new ElevatorCommand(32, 40, 40, m_elevator)); + + NamedCommands.registerCommand( + "Bottom", new ElevatorCommand(0, 10, 20, m_elevator).until(elevatorTrigger)); + + NamedCommands.registerCommand("CoralScorer", (Commands.print("CoralScorer"))); + + NamedCommands.registerCommand("CoralDetect", (Commands.print("CoralDetect"))); // Set up the SmartDashboard Auto Chooser based on auto type switch (Constants.getAutoType()) { @@ -284,6 +320,13 @@ private void configureBindings() { // ** Example Commands -- Remap, remove, or change as desired ** // Press B button while driving --> ROBOT-CENTRIC + + driverController + .rightBumper() + .onTrue( + new LEDCommand(m_led, lightStop::get) + .ignoringDisable(true) + .until(driverController.leftBumper())); driverController .b() .onTrue( @@ -295,21 +338,51 @@ private void configureBindings() { () -> -driveStickX.value(), () -> turnStickX.value()), m_drivebase)); + driverController.a().onTrue(Commands.run(() -> m_coralControl.indexL())); + driverController.y().onTrue(Commands.run(() -> m_coralControl.indexR())); // Press A button -> BRAKE driverController .a() .whileTrue(Commands.runOnce(() -> m_drivebase.setMotorBrake(true), m_drivebase)); - //setDefaultCommand(CommandConstants.FIELD_RELATIVE_DRIVE_COMMAND); - - driverController.rightBumper().onTrue(Commands.runOnce(() -> getPathPlannerPath(Squirtle(what, constraints)))); + driverController.rightBumper(); // Press X button --> Stop with wheels in X-Lock position driverController.x().onTrue(Commands.runOnce(m_drivebase::stopWithX, m_drivebase)); - driverController.leftBumper().whileTrue(Commands.run(() -> getAutonomousCommandPathPlanner())); + // driverController.a().whileTrue(new IntakeCommand(m_intake, 0)); + + // m_elevator.setDefaultCommand( + // Commands.run( + // () -> m_elevator.runVolts(driverController.getRightTriggerAxis()), m_elevator)); + + // the two driver controller bumpers below make it so when you let go of either button the + // intake pivot will go to a resting posistion + + m_intake.setDefaultCommand( + Commands.run( + () -> + m_intake.runPivotVolts( + driverController.getRightTriggerAxis() - driverController.getLeftTriggerAxis()), + m_intake)); + + driverController + .rightBumper() + .whileTrue(new IntakeCommand(m_intake, 0.25, -0.35, 0)) + .whileFalse(new IntakeCommand(m_intake, 0.9, 0, 0).until(leftBumper)); + + driverController + .leftBumper() + .whileTrue( + new IntakeCommand(m_intake, 0.75, 0, 0) + .withTimeout(0.075) + .andThen(new IntakeCommand(m_intake, 0.75, 0.7, 0))) + .whileFalse(new IntakeCommand(m_intake, 0.9, 0, 0).until(rightBumper)); + + driverController.a().whileTrue(new IntakeCommand(m_intake, 0.9, 0, 1)); // Press Y button --> Manually Re-Zero the Gyro + driverController .y() .onTrue( @@ -327,6 +400,7 @@ private void configureBindings() { // Commands.startEnd( // () -> m_flywheel.runVelocity(flywheelSpeedInput.get()), // m_flywheel::stop, + // m_flywheel)); } @@ -361,7 +435,7 @@ private void configureBindings() { public Command getAutonomousCommandPathPlanner() { // Use the ``autoChooser`` to define your auto path from the SmartDashboard // return autoChooserPathPlanner.get(); - // return new PathPlannerAuto("Consistancy Test"); + // return new PathPlannerAuto("Consistancy Test"); return AutoBuilder.followPath(Squirtle); } @@ -423,19 +497,20 @@ private void definesysIdRoutines() { "Drive SysId (Dynamic Reverse)", m_drivebase.sysIdDynamic(SysIdRoutine.Direction.kReverse)); - // Example Flywheel SysId Characterization - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Forward)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Quasistatic Reverse)", - m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Forward)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); - autoChooserPathPlanner.addOption( - "Flywheel SysId (Dynamic Reverse)", - m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + // // Example Flywheel SysId Characterization + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Quasistatic Forward)", + // m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Quasistatic Reverse)", + // m_flywheel.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Dynamic Forward)", + // m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kForward)); + // autoChooserPathPlanner.addOption( + // "Flywheel SysId (Dynamic Reverse)", + // m_flywheel.sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } } diff --git a/src/main/java/frc/robot/commands/Climb/ClimbExtend.java b/src/main/java/frc/robot/commands/Climb/ClimbExtend.java new file mode 100644 index 0000000..308bc2a --- /dev/null +++ b/src/main/java/frc/robot/commands/Climb/ClimbExtend.java @@ -0,0 +1,25 @@ +package frc.robot.commands.Climb; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climb.Climb; + +public class ClimbExtend extends Command { + private final Climb climb; + + public ClimbExtend(Climb climb) { + this.climb = climb; + } + + @Override + public void initialize() { + climb.rachetToggle(false); + } + + @Override + public void execute() { + climb.twistToPosition(180); + } + + @Override + public void end(boolean interrupted) {} +} diff --git a/src/main/java/frc/robot/commands/Climb/ClimbRetract.java b/src/main/java/frc/robot/commands/Climb/ClimbRetract.java new file mode 100644 index 0000000..e486920 --- /dev/null +++ b/src/main/java/frc/robot/commands/Climb/ClimbRetract.java @@ -0,0 +1,25 @@ +package frc.robot.commands.Climb; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climb.Climb; + +public class ClimbRetract extends Command { + private final Climb climb; + + public ClimbRetract(Climb climb) { + this.climb = climb; + } + + @Override + public void initialize() { + climb.rachetToggle(true); + } + + @Override + public void execute() { + climb.twistToPosition(0); + } + + @Override + public void end(boolean interrupted) {} +} diff --git a/src/main/java/frc/robot/commands/ElevatorCommand.java b/src/main/java/frc/robot/commands/ElevatorCommand.java new file mode 100644 index 0000000..04e2d84 --- /dev/null +++ b/src/main/java/frc/robot/commands/ElevatorCommand.java @@ -0,0 +1,37 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.elevator.Elevator; + +public class ElevatorCommand extends Command { + + private final double posistion; + private final double acceleration; + private final double velocity; + private final Elevator elevator; + + public ElevatorCommand( + double posistion, double acceleration, double velocity, Elevator elevator) { + this.posistion = posistion; + this.elevator = elevator; + this.acceleration = acceleration; + this.velocity = velocity; + } + + @Override + public void initialize() { + + elevator.configure(0.3375, 0.075, 0.18629, 0.01, 18, 0, 0.01, velocity, acceleration, 0); + } + + @Override + public void execute() { + double rotationsPosition = (35 / 54.75) * posistion - 10.86758; + elevator.setPosistion(rotationsPosition); + } + + @Override + public void end(boolean interrupted) { + elevator.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/IntakeCommand.java b/src/main/java/frc/robot/commands/IntakeCommand.java new file mode 100644 index 0000000..844648f --- /dev/null +++ b/src/main/java/frc/robot/commands/IntakeCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Intake.Intake; + +public class IntakeCommand extends Command { + + private final Intake intake; + private final double wantedPosistion; + private final double rollerSpeed; + private final double test; // get rid of this variable in future + + public IntakeCommand(Intake intake, double wantedPosistion, double rollerSpeed, double test) { + this.intake = intake; + this.wantedPosistion = wantedPosistion; + this.rollerSpeed = rollerSpeed; + this.test = test; + } + + @Override + public void initialize() { + intake.configure(1, 0, 0); + } + + @Override + public void execute() { + if (test == 0) { + intake.setPivotPosition(wantedPosistion); + intake.rollerSpeed(rollerSpeed); + } else if (test == 1) { + System.out.println(intake.getEncoder()); + } + } + + @Override + public void end(boolean interrupted) { + intake.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/LED/LEDCommand.java b/src/main/java/frc/robot/commands/LED/LEDCommand.java new file mode 100644 index 0000000..20ba259 --- /dev/null +++ b/src/main/java/frc/robot/commands/LED/LEDCommand.java @@ -0,0 +1,31 @@ +package frc.robot.commands.LED; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.LED.LED; +import java.util.function.BooleanSupplier; + +public class LEDCommand extends Command { + LED led; + BooleanSupplier lightstop; + + public LEDCommand(LED led, BooleanSupplier lightStop) { + this.led = led; + this.lightstop = lightStop; + } + + @Override + public void initialize() {} + + @Override + public void execute() { + led.scoreReady(lightstop.getAsBoolean()); + // led.rainbowTwinkle(); + // led.larson(); + // led.police(); + } + + @Override + public void end(boolean interrupted) { + led.off(); + } +} diff --git a/src/main/java/frc/robot/commands/commandTemplate.txt b/src/main/java/frc/robot/commands/commandTemplate.txt new file mode 100644 index 0000000..e06952b --- /dev/null +++ b/src/main/java/frc/robot/commands/commandTemplate.txt @@ -0,0 +1,16 @@ +package frc.robot.commands; + + +public class m_Command extends Command { + + public m_Command() {} + + @Override + public void initialize() {} + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) {} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Climb/Climb.java b/src/main/java/frc/robot/subsystems/Climb/Climb.java new file mode 100644 index 0000000..e85a370 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climb/Climb.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.Climb; + +public class Climb { + private ClimbIO io; + private boolean rachetToggle; + + public Climb(ClimbIO io) { + this.io = io; + io.configPID(1, 0, 0); + rachetToggle = false; + } + + public void twistToPosition(double position) { + io.twistMotorToPosition(position); + } + + public void manualControl(double controlVolts) { + io.twistMotorVolts(controlVolts * 3); + } + + public void rachetToggle(boolean toggleSwitch) { + if (rachetToggle) { + io.turnClimbServo(0); + } else { + io.turnClimbServo(1); + } + } +} 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..121dfb3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climb/ClimbIO.java @@ -0,0 +1,12 @@ +package frc.robot.subsystems.Climb; + +public interface ClimbIO { + + public default void turnClimbServo(double position) {} + + public default void twistMotorToPosition(double positionInRotations) {} + + public default void configPID(double kP, double kI, double kD) {} + + public default void twistMotorVolts(double volts) {} +} diff --git a/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java b/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java new file mode 100644 index 0000000..9d8bdf0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climb/ClimbIOTalonFX.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.Climb; + +import com.ctre.phoenix6.configs.Slot0Configs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.PositionDutyCycle; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.Servo; +import frc.robot.Constants.CANandPowerPorts; + +public class ClimbIOTalonFX implements ClimbIO { + private final Servo climbExtender = new Servo(CANandPowerPorts.CLIMB_SERVO); + private final TalonFX climber = new TalonFX(CANandPowerPorts.CLIMB.getDeviceNumber()); + + public ClimbIOTalonFX() { + var climbConfig = new TalonFXConfiguration(); + climbConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; + } + + @Override + public void turnClimbServo(double position) { + climbExtender.set(position); + } + + @Override + public void twistMotorToPosition(double position) { + climber.setControl(new PositionDutyCycle(position)); + } + + @Override + public void twistMotorVolts(double volts) { + climber.setVoltage(volts); + } + + @Override + public void configPID(double kP, double kI, double kD) { + Slot0Configs pid = new Slot0Configs(); + pid.withKP(kP); + pid.withKI(kI); + pid.withKD(kD); + climber.getConfigurator().apply(pid); + } +} diff --git a/src/main/java/frc/robot/subsystems/Controls/ControlMap.txt b/src/main/java/frc/robot/subsystems/Controls/ControlMap.txt new file mode 100644 index 0000000..ea38f2e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Controls/ControlMap.txt @@ -0,0 +1,39 @@ +A map of all controls + +Change these controls based on how back buttons are bound + + Driver Controls: +A: Drive to selected Position +B: Change score index Left +X: zero +Y: Change score index Right +D-Pad Up/Down: +D-Pad Left/right: +Right Bumper: Intake +Left Bumper: Intake Score +Right Trigger: Grab Algae +Left Trigger: +Start: +Stop: +Right Stick: Rotate +Left Stick: Strafe +Right Stick Button: +Left Stick Button: + + Co-Driver Controls: +A: Set up Score algae in net +B: Set up Score algae in processor +X: Score algae +Y: +D-Pad Up/Down: Change score index Up/Down +D-Pad Left/Right: +Right Bumper: Score coral +Left Bumper: +Right Trigger: +Left Trigger: +Start: +Stop: +Right Stick: +Left Stick: +Right Stick Button: +Left Stick Button: \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Controls/CoralControl.java b/src/main/java/frc/robot/subsystems/Controls/CoralControl.java new file mode 100644 index 0000000..79e65c0 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Controls/CoralControl.java @@ -0,0 +1,63 @@ +package frc.robot.subsystems.Controls; + +import frc.robot.util.VirtualSubsystem; + +public class CoralControl extends VirtualSubsystem { + // scoreStates[0] stores horizontal state + // scoreStates[1] stores vertical state + // scoreStates[2] stores left or right state + private int[] scoreStates; + + public CoralControl() { + scoreStates[0] = 0; + scoreStates[1] = 0; + scoreStates[2] = 0; + } + + public void indexHorizontal(boolean indexRight, boolean indexLeft) { + if (indexRight) { + scoreStates[0] = scoreStates[0] + 1; + } + if (indexLeft) { + scoreStates[0] = scoreStates[0] - 1; + } + + if (scoreStates[0] < 0) { + scoreStates[0] = 11; + } + if (scoreStates[0] > 11) { + scoreStates[0] = 0; + } + } + + public void indexUp(boolean indexUp, boolean indexDown) { + + scoreStates[1] = scoreStates[1] + 1; + if (scoreStates[1] > 3) { + scoreStates[1] = 3; + } + } + + public void indexDown() { + + scoreStates[1] = scoreStates[1] - 1; + if (scoreStates[1] < 0) { + scoreStates[1] = 0; + } + } + + public void indexR() { + scoreStates[2] = 1; + } + + public void indexL() { + scoreStates[2] = 0; + } + + @Override + public void periodic() {} + + public int[] getScoreState() { + return scoreStates; + } +} diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java new file mode 100644 index 0000000..45bc971 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorer.java @@ -0,0 +1,28 @@ +package frc.robot.subsystems.CoralScorer; + +import frc.robot.util.RBSISubsystem; + +public class CoralScorer extends RBSISubsystem { + private final CoralScorerIO io; + + public CoralScorer(CoralScorerIO io) { + this.io = io; + } + + public void runVolts(double volts) { + io.setVolts(volts); + } + + public void setVelocity(double velocity) { + io.setVelocity(velocity); + } + + public void stop() { + io.stop(); + } + + @Override + public int[] getPowerPorts() { + return io.powerPorts; + } +} diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java new file mode 100644 index 0000000..1580e61 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerIO.java @@ -0,0 +1,17 @@ +package frc.robot.subsystems.CoralScorer; + +import org.littletonrobotics.junction.AutoLog; + +public interface CoralScorerIO { + + @AutoLog + public static class IntakeIOInputs {} + + public final int[] powerPorts = {}; + + public default void setVolts(double volts) {} + + public default void stop() {} + + public default void setVelocity(double velocity) {} +} diff --git a/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerTalonFXIO.java b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerTalonFXIO.java new file mode 100644 index 0000000..c108c7e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/CoralScorer/CoralScorerTalonFXIO.java @@ -0,0 +1,3 @@ +package frc.robot.subsystems.CoralScorer; + +public class CoralScorerTalonFXIO {} diff --git a/src/main/java/frc/robot/subsystems/Intake/Intake.java b/src/main/java/frc/robot/subsystems/Intake/Intake.java new file mode 100644 index 0000000..0fbf3bb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/Intake.java @@ -0,0 +1,69 @@ +package frc.robot.subsystems.Intake; + +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; + +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.util.RBSISubsystem; +import org.littletonrobotics.junction.Logger; + +public class Intake extends RBSISubsystem { + private final IntakeIO io; + private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged(); + private final SysIdRoutine sysId; + + public Intake(IntakeIO io) { + this.io = io; + + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(1.0).div(Seconds.of(1.5)), // QuasiStatis + Volts.of(.75), // Dynamic + Seconds.of(1.9), + (state) -> Logger.recordOutput("Intake/SysIdState", state.toString())), + new SysIdRoutine.Mechanism( + (voltage) -> runPivotVolts(voltage.in(Units.Volts)), null, this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Intake", inputs); + } + + public void setPivotPosition(double position) { + io.setPivotPosition(position); + } + + public void rollerSpeed(double speed) { + io.rollerSpeed(speed); + } + + public void runPivotVolts(double volts) { + io.setPivotVolts(volts); + } + + public void setRollerVolts(double volts) { + io.setRollerVolts(volts); + } + + public void stop() { + io.stop(); + } + + public void configure(double kP, double kI, double kD) { + io.configure(kP, kI, kD); + } + ; + + public double getEncoder() { + return io.getEncoder(); + } + + @Override + public int[] getPowerPorts() { + return io.powerPorts; + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java new file mode 100644 index 0000000..fe86e87 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIO.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems.Intake; + +import org.littletonrobotics.junction.AutoLog; + +public interface IntakeIO { + + public final int[] powerPorts = {}; + + @AutoLog + public static class IntakeIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + public default void updateInputs(IntakeIOInputs inputs) {} + + public default void setPivotPosition(double position) {} + + public default void rollerSpeed(double speed) {} + + public default void stop() {} + + public default void setPivotVolts(double volts) {} + + public default void configure(double kP, double kI, double kD) {} + + public default void setRollerVolts(double volts) {} + + public default double getEncoder() { + return 0.0; + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java new file mode 100644 index 0000000..4aab918 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOKraken.java @@ -0,0 +1,87 @@ +package frc.robot.subsystems.Intake; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.CANcoder; +import com.ctre.phoenix6.hardware.TalonFX; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.CANandPowerPorts; + +public class IntakeIOKraken implements IntakeIO { + + private final TalonFX intakeRoller = + new TalonFX(CANandPowerPorts.INTAKE_ROLLER.getDeviceNumber()); + private final TalonFX intakePivot = new TalonFX(CANandPowerPorts.INTAKE_PIVOT.getDeviceNumber()); + + private final CANcoder encoderActual = new CANcoder(23); + + PIDController pivotPID = new PIDController(1.5, 0, 0); + + public final int[] powerPorts = { + CANandPowerPorts.INTAKE_PIVOT.getPowerPort(), CANandPowerPorts.INTAKE_ROLLER.getPowerPort() + }; + + private final StatusSignal pivotPosition = encoderActual.getAbsolutePosition(); + private final StatusSignal pivotVelocity = intakePivot.getVelocity(); + private final StatusSignal pivotAppliedVolts = intakePivot.getMotorVoltage(); + private final StatusSignal pivotCurrent = intakePivot.getSupplyCurrent(); + + public IntakeIOKraken() {} + + @Override + public void updateInputs(IntakeIOInputs inputs) { + BaseStatusSignal.refreshAll( + encoderActual.getAbsolutePosition(), pivotVelocity, pivotAppliedVolts, pivotCurrent); + inputs.positionRad = + Units.rotationsToRadians( + encoderActual.getAbsolutePosition().getValueAsDouble()); // 21.42857; // gear ratio + inputs.velocityRadPerSec = + Units.rotationsToRadians(pivotVelocity.getValueAsDouble()); // 21.42857; // gear ratio + inputs.appliedVolts = pivotAppliedVolts.getValueAsDouble(); + inputs.currentAmps = new double[] {pivotCurrent.getValueAsDouble()}; + } + + @Override + public void setPivotVolts(double volts) { + intakePivot.setVoltage(volts); + } + + @Override + public void setRollerVolts(double volts) { + intakeRoller.setVoltage(volts); + } + + @Override + public void stop() { + intakeRoller.stopMotor(); + intakePivot.stopMotor(); + } + + @Override + public void setPivotPosition(double position) { + intakePivot.set( + -pivotPID.calculate(encoderActual.getAbsolutePosition().getValueAsDouble(), position)); + } + + @Override + public void rollerSpeed(double speed) { + intakeRoller.set(speed); + } + + @Override + public void configure(double kP, double kI, double kD) { + pivotPID.setP(kP); + pivotPID.setI(kI); + pivotPID.setD(kD); + } + + @Override + public double getEncoder() { + return encoderActual.getAbsolutePosition().getValueAsDouble(); + } +} diff --git a/src/main/java/frc/robot/subsystems/Intake/IntakeIOSpark.java b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSpark.java new file mode 100644 index 0000000..3fa7d11 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Intake/IntakeIOSpark.java @@ -0,0 +1,33 @@ +package frc.robot.subsystems.Intake; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +public class IntakeIOSpark implements IntakeIO { + private final SparkMax intakeRoller = new SparkMax(21, MotorType.kBrushless); + + private final SparkMax intakePivot = new SparkMax(22, MotorType.kBrushless); + private final SparkClosedLoopController rollerPid = intakeRoller.getClosedLoopController(); + + private final AbsoluteEncoder encoder = intakePivot.getAbsoluteEncoder(); + + public IntakeIOSpark() {} + + @Override + public void setPivotVolts(double volts) { + intakePivot.setVoltage(volts); + } + + @Override + public void setRollerVolts(double volts) { + intakeRoller.setVoltage(volts); + } + + @Override + public void stop() { + intakeRoller.stopMotor(); + intakePivot.stopMotor(); + } +} diff --git a/src/main/java/frc/robot/subsystems/LED/LED.java b/src/main/java/frc/robot/subsystems/LED/LED.java new file mode 100644 index 0000000..7e7ccce --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LED/LED.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.LED; + +import frc.robot.util.RBSISubsystem; + +public class LED extends RBSISubsystem { + private LEDIO io; + + public LED(LEDIO io) { + this.io = io; + } + + public void rainbowTwinkle() { + io.rainbowTwinkle(); + } + + public void off() { + io.off(); + } + + public void scoreReady(boolean lightstop) { + if (lightstop) { + io.scoreReady(); + } else { + io.rainbowTwinkle(); + } + } +} diff --git a/src/main/java/frc/robot/subsystems/LED/LEDIO.java b/src/main/java/frc/robot/subsystems/LED/LEDIO.java new file mode 100644 index 0000000..67310f9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LED/LEDIO.java @@ -0,0 +1,11 @@ +package frc.robot.subsystems.LED; + +public interface LEDIO { + public default void rainbowTwinkle() {} + + public default void off() {} + + public default void scoreReady() {} + + public default void scoreNotReady() {} +} diff --git a/src/main/java/frc/robot/subsystems/LED/LEDIOCandle.java b/src/main/java/frc/robot/subsystems/LED/LEDIOCandle.java new file mode 100644 index 0000000..f3693b2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LED/LEDIOCandle.java @@ -0,0 +1,74 @@ +package frc.robot.subsystems.LED; + +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.TwinkleOffAnimation; +import com.ctre.phoenix.led.TwinkleOffAnimation.TwinkleOffPercent; + +public class LEDIOCandle implements LEDIO { + CANdle candle = new CANdle(1); + CANdleConfiguration config = new CANdleConfiguration(); + private boolean red; + private double i = 0; + + public LEDIOCandle() { + red = false; + config.stripType = LEDStripType.RGB; // set the strip type to RGB + config.brightnessScalar = 0.5; // dim the LEDs to half brightness + candle.configAllSettings(config); + } + + @Override + public void rainbowTwinkle() { + double rand = Math.random(); + + TwinkleOffAnimation twinkleR = + new TwinkleOffAnimation(255, 0, 0, 0, .01, 67, TwinkleOffPercent.Percent100); + TwinkleOffAnimation twinkleG = + new TwinkleOffAnimation(0, 255, 0, 0, .01, 67, TwinkleOffPercent.Percent100); + TwinkleOffAnimation twinkleB = + new TwinkleOffAnimation(0, 0, 255, 0, .01, 67, TwinkleOffPercent.Percent100); + TwinkleOffAnimation twinkleY = + new TwinkleOffAnimation(255, 255, 0, 0, .01, 67, TwinkleOffPercent.Percent100); + TwinkleOffAnimation twinkleO = + new TwinkleOffAnimation(255, 165, 0, 0, .01, 67, TwinkleOffPercent.Percent100); + TwinkleOffAnimation twinkleP = + new TwinkleOffAnimation(48, 25, 52, 0, .01, 67, TwinkleOffPercent.Percent100); + // FireAnimation fire = new FireAnimation(1, .5, 67, .9, .5); + // candle.setLEDs(0, 255, 0, 0, 0, 67); + // candle.setLEDs(255, 255, 255, 0, 8, 44); + // RainbowAnimation rainbowAnim = new RainbowAnimation(1, .5, 67); + if (rand < .166) { + candle.animate(twinkleR); + } else if (rand < .332) { + candle.animate(twinkleG); + } else if (rand < .498) { + candle.animate(twinkleB); + } else if (rand < .664) { + candle.animate(twinkleY); + } else if (rand < .830) { + candle.animate(twinkleO); + } else { + candle.animate(twinkleP); + } + } + + @Override + public void off() { + candle.clearAnimation(0); + candle.setLEDs(0, 0, 0, 0, 0, 67); + } + + @Override + public void scoreReady() { + candle.clearAnimation(0); + candle.setLEDs(255, 165, 0, 0, 0, 67); + } + + @Override + public void scoreNotReady() { + candle.clearAnimation(0); + candle.setLEDs(0, 0, 255, 0, 0, 67); + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/README b/src/main/java/frc/robot/subsystems/drive/README index 2b45714..36c39a1 100644 --- a/src/main/java/frc/robot/subsystems/drive/README +++ b/src/main/java/frc/robot/subsystems/drive/README @@ -1,3 +1,4 @@ + The hardware-specific IO files included here are a baseline for what we expect teams to use. If, however, you are using a different combination of hardware, you will need to create a new ``ModuleIO*.java`` or ``GyroIO*.java`` file. We diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java new file mode 100644 index 0000000..bfdbe0e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -0,0 +1,113 @@ +package frc.robot.subsystems.elevator; + +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; +import static frc.robot.Constants.ElevatorConstants.*; + +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.units.Units; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants; +import frc.robot.util.RBSISubsystem; +import org.littletonrobotics.junction.Logger; + +public class Elevator extends RBSISubsystem { + private final ElevatorFeedforward ffModel; + private final ElevatorIO io; + private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged(); + private final SysIdRoutine sysId; + + public Elevator(ElevatorIO io) { + this.io = io; + + switch (Constants.getMode()) { + case REAL: + case REPLAY: + ffModel = new ElevatorFeedforward(kSReal, kGReal, kVReal, kAReal); + io.configure( + kGReal, + kSReal, + kVReal, + kAReal, + kPReal, + kIReal, + kDReal, + kVelocity, + kAcceleration, + kJerk); + break; + case SIM: + ffModel = new ElevatorFeedforward(kSSim, kGSim, kVSim, kASim); + io.configure( + kGSim, kSSim, kVSim, kASim, kPSim, kISim, kDSim, kVelocity, kAcceleration, kJerk); + break; + default: + ffModel = new ElevatorFeedforward(0.0, 0.0, 0.0, 0.0); + io.configure(0, 0, 0, 0, 0, 0, 0, 0, 0, 0); + break; + } + + // Configure SysId + sysId = + new SysIdRoutine( + new SysIdRoutine.Config( + Volts.of(1.0).div(Seconds.of(1.5)), // QuasiStatis + Volts.of(1.5), // Dynamic + Seconds.of(2.0), + (state) -> Logger.recordOutput("Elevator/SysIdState", state.toString())), + new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Units.Volts)), null, this)); + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Elevator", inputs); + } + + public void setPosistion(double posistion) { + io.setPosistion(posistion); + } + + public void stop() { + io.stop(); + } + + public void configure( + double Kg, + double Ks, + double Kv, + double Ka, + double Kp, + double Ki, + double Kd, + double velocity, + double aceleration, + double jerk) { + io.configure(Kg, Ks, Kv, Ka, Kp, Ki, Kd, velocity, aceleration, jerk); + } + + // NOTE: This is how to measure the SysId for elevators that cannot hold + // themselves up under the force of gravity: + // https://www.chiefdelphi.com/t/running-backwards-sysid-for-elevator-that-can-t-hold-itself-up/427876/2 + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysId.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysId.dynamic(direction); + } + + /** Run open loop at the specified voltage. */ + public void runVolts(double volts) { + io.setVoltage(volts); + } + + public void setCoast() { + io.setCoast(); + } + + public void setBrake() { + io.setBrake(); + } +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java new file mode 100644 index 0000000..d5bf3b7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -0,0 +1,43 @@ +package frc.robot.subsystems.elevator; + +import java.util.function.BooleanSupplier; +import org.littletonrobotics.junction.AutoLog; + +public interface ElevatorIO { + + public final int[] powerPorts = {}; + + @AutoLog + public static class ElevatorIOInputs { + public double positionRad = 0.0; + public double velocityRadPerSec = 0.0; + public double appliedVolts = 0.0; + public double[] currentAmps = new double[] {}; + } + + public default void setPosistion(double posistion) {} + + public default void updateInputs(ElevatorIOInputs inputs) {} + + public default void stop() {} + + public default void configure( + double Kg, + double Ks, + double Kv, + double Ka, + double Kp, + double Ki, + double Kd, + double velocity, + double aceleration, + double jerk) {} + + public default void setVoltage(double volts) {} + + public default void limit(BooleanSupplier limitSwitch) {} + + public default void setCoast() {} + + public default void setBrake() {} +} diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java new file mode 100644 index 0000000..66d0fb5 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOTalonFX.java @@ -0,0 +1,111 @@ +package frc.robot.subsystems.elevator; + +import static frc.robot.Constants.ElevatorConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.MotionMagicVoltage; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Current; +import edu.wpi.first.units.measure.Voltage; +import frc.robot.Constants.CANandPowerPorts; +import java.util.function.BooleanSupplier; + +public class ElevatorIOTalonFX implements ElevatorIO { + + // motor called in + private final TalonFX elevatorMotor = + new TalonFX(CANandPowerPorts.ELEVATOR.getDeviceNumber(), CANandPowerPorts.ELEVATOR.getBus()); + + private final StatusSignal velocity = elevatorMotor.getVelocity(); + + public final int[] powerPorts = {CANandPowerPorts.ELEVATOR.getPowerPort()}; + + private final StatusSignal elevatorPosition = elevatorMotor.getPosition(); + private final StatusSignal elevatorVelocity = elevatorMotor.getVelocity(); + private final StatusSignal elevatorAppliedVolts = elevatorMotor.getMotorVoltage(); + private final StatusSignal elevatorCurrent = elevatorMotor.getSupplyCurrent(); + + public ElevatorIOTalonFX() {} + + @Override + public void updateInputs(ElevatorIOInputs inputs) { + BaseStatusSignal.refreshAll( + elevatorPosition, elevatorVelocity, elevatorAppliedVolts, elevatorCurrent); + inputs.positionRad = + Units.rotationsToRadians(elevatorPosition.getValueAsDouble()) / kElevatorGearRatio; + inputs.velocityRadPerSec = + Units.rotationsToRadians(elevatorVelocity.getValueAsDouble()) / kElevatorGearRatio; + inputs.appliedVolts = elevatorAppliedVolts.getValueAsDouble(); + inputs.currentAmps = new double[] {elevatorCurrent.getValueAsDouble()}; + } + + @Override + public void setPosistion(double posistion) { + final MotionMagicVoltage motionMagic = new MotionMagicVoltage(0); + + elevatorMotor.setControl(motionMagic.withPosition(posistion)); + } + + @Override + public void stop() { + elevatorMotor.stopMotor(); + } + + @Override + public void configure( + double Kg, + double Ks, + double Kv, + double Ka, + double Kp, + double Ki, + double Kd, + double velocity, + double aceleration, + double jerk) { + var talonFXConfigs = new TalonFXConfiguration(); + var talonSlot0Configs = talonFXConfigs.Slot0; + var motionMagicConfigs = talonFXConfigs.MotionMagic; + + talonSlot0Configs.kG = Kg; + talonSlot0Configs.kS = Ks; + talonSlot0Configs.kV = Kv; + talonSlot0Configs.kA = Ka; + talonSlot0Configs.kP = Kp; + talonSlot0Configs.kI = Ki; + talonSlot0Configs.kD = Kd; + + motionMagicConfigs.MotionMagicCruiseVelocity = velocity; + motionMagicConfigs.MotionMagicAcceleration = aceleration; + motionMagicConfigs.MotionMagicJerk = jerk; + + elevatorMotor.getConfigurator().apply(talonFXConfigs); + } + + @Override + public void setVoltage(double volts) { + elevatorMotor.setControl(new VoltageOut(volts)); + } + + @Override + public void limit(BooleanSupplier limitSwitch) { + if (limitSwitch.getAsBoolean()) {} + } + + @Override + public void setCoast() { + elevatorMotor.setNeutralMode(NeutralModeValue.Coast); + } + + @Override + public void setBrake() { + elevatorMotor.setNeutralMode(NeutralModeValue.Brake); + } +} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java b/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java deleted file mode 100644 index 53e3b0d..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel_example/Flywheel.java +++ /dev/null @@ -1,118 +0,0 @@ -// Copyright (c) 2024-2025 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel_example; - -import static edu.wpi.first.units.Units.*; -import static frc.robot.Constants.FlywheelConstants.*; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; -import frc.robot.Constants; -import frc.robot.util.RBSISubsystem; -import org.littletonrobotics.junction.AutoLogOutput; -import org.littletonrobotics.junction.Logger; - -public class Flywheel extends RBSISubsystem { - private final FlywheelIO io; - private final FlywheelIOInputsAutoLogged inputs = new FlywheelIOInputsAutoLogged(); - private final SimpleMotorFeedforward ffModel; - private final SysIdRoutine sysId; - - /** Creates a new Flywheel. */ - public Flywheel(FlywheelIO io) { - this.io = io; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.getMode()) { - case REAL: - case REPLAY: - ffModel = new SimpleMotorFeedforward(kStaticGainReal, kVelocityGainReal); - io.configurePID(pidReal.kP, pidReal.kI, pidReal.kD); - break; - case SIM: - ffModel = new SimpleMotorFeedforward(kStaticGainSim, kVelocityGainSim); - io.configurePID(pidSim.kP, pidSim.kI, pidSim.kD); - break; - default: - ffModel = new SimpleMotorFeedforward(0.0, 0.0); - break; - } - - // Configure SysId - sysId = - new SysIdRoutine( - new SysIdRoutine.Config( - null, - null, - null, - (state) -> Logger.recordOutput("Flywheel/SysIdState", state.toString())), - new SysIdRoutine.Mechanism((voltage) -> runVolts(voltage.in(Volts)), null, this)); - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Flywheel", inputs); - } - - /** Run open loop at the specified voltage. */ - public void runVolts(double volts) { - io.setVoltage(volts); - } - - /** Run closed loop at the specified velocity. */ - public void runVelocity(double velocityRPM) { - var velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(velocityRPM); - io.setVelocity(velocityRadPerSec, ffModel.calculate(velocityRadPerSec)); - - // Log flywheel setpoint - Logger.recordOutput("Flywheel/SetpointRPM", velocityRPM); - } - - /** Stops the flywheel. */ - public void stop() { - io.stop(); - } - - /** Returns a command to run a quasistatic test in the specified direction. */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysId.quasistatic(direction); - } - - /** Returns a command to run a dynamic test in the specified direction. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysId.dynamic(direction); - } - - /** Returns the current velocity in RPM. */ - @AutoLogOutput(key = "Mechanism/Flywheel") - public double getVelocityRPM() { - return Units.radiansPerSecondToRotationsPerMinute(inputs.velocityRadPerSec); - } - - /** Returns the current velocity in radians per second. */ - public double getCharacterizationVelocity() { - return inputs.velocityRadPerSec; - } - - @Override - public int[] getPowerPorts() { - return io.powerPorts; - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java deleted file mode 100644 index 5ffbf39..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIO.java +++ /dev/null @@ -1,47 +0,0 @@ -// Copyright (c) 2024-2025 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel_example; - -import org.littletonrobotics.junction.AutoLog; - -public interface FlywheelIO { - - // IMPORTANT: Include here all devices that are part of this mechanism! - public final int[] powerPorts = {}; - - @AutoLog - public static class FlywheelIOInputs { - public double positionRad = 0.0; - public double velocityRadPerSec = 0.0; - public double appliedVolts = 0.0; - public double[] currentAmps = new double[] {}; - } - - /** Updates the set of loggable inputs. */ - public default void updateInputs(FlywheelIOInputs inputs) {} - - /** Run open loop at the specified voltage. */ - public default void setVoltage(double volts) {} - - /** Run closed loop at the specified velocity. */ - public default void setVelocity(double velocityRadPerSec, double ffVolts) {} - - /** Stop in open loop. */ - public default void stop() {} - - /** Set velocity PID constants. */ - public default void configurePID(double kP, double kI, double kD) {} -} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java deleted file mode 100644 index a4a82cd..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSim.java +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright (c) 2024-2025 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel_example; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.numbers.N1; -import edu.wpi.first.math.system.LinearSystem; -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.FlywheelSim; - -public class FlywheelIOSim implements FlywheelIO { - // Reduction between motors and encoder, as output over input. If the flywheel - // spins slower than the motors, this number should be greater than one. - private static final double kFlywheelGearing = 1.0; - - // 1/2 MR^2 - private static final double kFlywheelMomentOfInertia = - 0.5 * Units.lbsToKilograms(1.5) * Math.pow(Units.inchesToMeters(4), 2); - - private final DCMotor m_gearbox = DCMotor.getNEO(1); - private final LinearSystem m_plant = - LinearSystemId.createFlywheelSystem(m_gearbox, kFlywheelGearing, kFlywheelMomentOfInertia); - - private final FlywheelSim sim = new FlywheelSim(m_plant, m_gearbox); - private PIDController pid = new PIDController(0.0, 0.0, 0.0); - - private boolean closedLoop = false; - private double ffVolts = 0.0; - private double appliedVolts = 0.0; - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - if (closedLoop) { - appliedVolts = - MathUtil.clamp(pid.calculate(sim.getAngularVelocityRadPerSec()) + ffVolts, -12.0, 12.0); - sim.setInputVoltage(appliedVolts); - } - - sim.update(0.02); - - inputs.positionRad = 0.0; - inputs.velocityRadPerSec = sim.getAngularVelocityRadPerSec(); - inputs.appliedVolts = appliedVolts; - inputs.currentAmps = new double[] {sim.getCurrentDrawAmps()}; - } - - @Override - public void setVoltage(double volts) { - closedLoop = false; - appliedVolts = volts; - sim.setInputVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - closedLoop = true; - pid.setSetpoint(velocityRadPerSec); - this.ffVolts = ffVolts; - } - - @Override - public void stop() { - setVoltage(0.0); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - pid.setPID(kP, kI, kD); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java deleted file mode 100644 index 871a9de..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOSpark.java +++ /dev/null @@ -1,137 +0,0 @@ -// Copyright (c) 2024-2025 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel_example; - -import static frc.robot.Constants.FlywheelConstants.*; -import static frc.robot.util.SparkUtil.*; - -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkBase.ControlType; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkClosedLoopController; -import com.revrobotics.spark.SparkClosedLoopController.ArbFFUnits; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants.CANandPowerPorts; -import frc.robot.subsystems.drive.SwerveConstants; - -/** - * NOTE: To use the Spark Flex / NEO Vortex, replace all instances of "CANSparkMax" with - * "CANSparkFlex". - */ -public class FlywheelIOSpark implements FlywheelIO { - - // Define the leader / follower motors from the Ports section of RobotContainer - private final SparkBase leader = - new SparkMax(CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); - private final SparkBase follower = - new SparkMax(CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), MotorType.kBrushless); - private final RelativeEncoder encoder = leader.getEncoder(); - private final SparkClosedLoopController pid = leader.getClosedLoopController(); - // IMPORTANT: Include here all devices listed above that are part of this mechanism! - public final int[] powerPorts = { - CANandPowerPorts.FLYWHEEL_LEADER.getPowerPort(), - CANandPowerPorts.FLYWHEEL_FOLLOWER.getPowerPort() - }; - - public FlywheelIOSpark() { - - // Configure leader motor - var leaderConfig = new SparkFlexConfig(); - leaderConfig - .idleMode( - switch (kFlywheelIdleMode) { - case COAST -> IdleMode.kCoast; - case BRAKE -> IdleMode.kBrake; - }) - .smartCurrentLimit((int) SwerveConstants.kDriveCurrentLimit) - .voltageCompensation(12.0); - leaderConfig.encoder.uvwMeasurementPeriod(10).uvwAverageDepth(2); - leaderConfig - .closedLoop - .feedbackSensor(FeedbackSensor.kPrimaryEncoder) - .pidf( - 0.0, 0.0, - 0.0, 0.0); - leaderConfig - .signals - .primaryEncoderPositionAlwaysOn(true) - .primaryEncoderPositionPeriodMs((int) (1000.0 / SwerveConstants.kOdometryFrequency)) - .primaryEncoderVelocityAlwaysOn(true) - .primaryEncoderVelocityPeriodMs(20) - .appliedOutputPeriodMs(20) - .busVoltagePeriodMs(20) - .outputCurrentPeriodMs(20); - tryUntilOk( - leader, - 5, - () -> - leader.configure( - leaderConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - tryUntilOk(leader, 5, () -> encoder.setPosition(0.0)); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - inputs.positionRad = Units.rotationsToRadians(encoder.getPosition() / kFlywheelGearRatio); - inputs.velocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / kFlywheelGearRatio); - inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.currentAmps = new double[] {leader.getOutputCurrent(), follower.getOutputCurrent()}; - } - - @Override - public void setVoltage(double volts) { - leader.setVoltage(volts); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - pid.setReference( - Units.radiansPerSecondToRotationsPerMinute(velocityRadPerSec) * kFlywheelGearRatio, - ControlType.kVelocity, - ClosedLoopSlot.kSlot0, - ffVolts, - ArbFFUnits.kVoltage); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - /** - * Configure the closed-loop PID - * - *

TODO: This functionality is no longer supported by the REVLib SparkClosedLoopController - * class. In order to keep control of the flywheel's underlying funtionality, shift everything to - * SmartMotion control. - */ - @Override - public void configurePID(double kP, double kI, double kD) { - // pid.setP(kP, 0); - // pid.setI(kI, 0); - // pid.setD(kD, 0); - // pid.setFF(0, 0); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java b/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java deleted file mode 100644 index 9364d32..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel_example/FlywheelIOTalonFX.java +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright (c) 2024-2025 Az-FIRST -// http://github.com/AZ-First -// Copyright (c) 2021-2025 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.flywheel_example; - -import static frc.robot.Constants.FlywheelConstants.*; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.Follower; -import com.ctre.phoenix6.controls.VelocityVoltage; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; -import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.Voltage; -import frc.robot.Constants.CANandPowerPorts; - -public class FlywheelIOTalonFX implements FlywheelIO { - - // Define the leader / follower motors from the Ports section of RobotContainer - private final TalonFX leader = - new TalonFX( - CANandPowerPorts.FLYWHEEL_LEADER.getDeviceNumber(), - CANandPowerPorts.FLYWHEEL_LEADER.getBus()); - private final TalonFX follower = - new TalonFX( - CANandPowerPorts.FLYWHEEL_FOLLOWER.getDeviceNumber(), - CANandPowerPorts.FLYWHEEL_FOLLOWER.getBus()); - // IMPORTANT: Include here all devices listed above that are part of this mechanism! - public final int[] powerPorts = { - CANandPowerPorts.FLYWHEEL_LEADER.getPowerPort(), - CANandPowerPorts.FLYWHEEL_FOLLOWER.getPowerPort() - }; - - private final StatusSignal leaderPosition = leader.getPosition(); - private final StatusSignal leaderVelocity = leader.getVelocity(); - private final StatusSignal leaderAppliedVolts = leader.getMotorVoltage(); - private final StatusSignal leaderCurrent = leader.getSupplyCurrent(); - private final StatusSignal followerCurrent = follower.getSupplyCurrent(); - - public FlywheelIOTalonFX() { - var config = new TalonFXConfiguration(); - config.CurrentLimits.SupplyCurrentLimit = 30.0; - config.CurrentLimits.SupplyCurrentLimitEnable = true; - config.MotorOutput.NeutralMode = - switch (kFlywheelIdleMode) { - case COAST -> NeutralModeValue.Coast; - case BRAKE -> NeutralModeValue.Brake; - }; - leader.getConfigurator().apply(config); - follower.getConfigurator().apply(config); - // If follower rotates in the opposite direction, set "OpposeMasterDirection" to true - follower.setControl(new Follower(leader.getDeviceID(), false)); - - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - leader.optimizeBusUtilization(); - follower.optimizeBusUtilization(); - } - - @Override - public void updateInputs(FlywheelIOInputs inputs) { - BaseStatusSignal.refreshAll( - leaderPosition, leaderVelocity, leaderAppliedVolts, leaderCurrent, followerCurrent); - inputs.positionRad = - Units.rotationsToRadians(leaderPosition.getValueAsDouble()) / kFlywheelGearRatio; - inputs.velocityRadPerSec = - Units.rotationsToRadians(leaderVelocity.getValueAsDouble()) / kFlywheelGearRatio; - inputs.appliedVolts = leaderAppliedVolts.getValueAsDouble(); - inputs.currentAmps = - new double[] {leaderCurrent.getValueAsDouble(), followerCurrent.getValueAsDouble()}; - } - - @Override - public void setVoltage(double volts) { - leader.setControl(new VoltageOut(volts)); - } - - @Override - public void setVelocity(double velocityRadPerSec, double ffVolts) { - leader.setControl(new VelocityVoltage(Units.radiansToRotations(velocityRadPerSec))); - } - - @Override - public void stop() { - leader.stopMotor(); - } - - @Override - public void configurePID(double kP, double kI, double kD) { - var config = new Slot0Configs(); - config.kP = kP; - config.kI = kI; - config.kD = kD; - leader.getConfigurator().apply(config); - } -} diff --git a/src/main/java/frc/robot/subsystems/flywheel_example/README b/src/main/java/frc/robot/subsystems/flywheel_example/README deleted file mode 100644 index b796c05..0000000 --- a/src/main/java/frc/robot/subsystems/flywheel_example/README +++ /dev/null @@ -1,33 +0,0 @@ -This directory contains an example generic flywheel subsystem, as distributed -with AdvantageKit. The structure of the subsystem directory is: - - * Flywheel.java - The base subsystem definitions and outward-facing (public) hardware- - agnostic functions for interacting with the subsystem. The idea is - that any of the hardware-specific (i.e. motor controller API calls) - functionality is insulated from the rest of the robot code within - the library-specific modules in this directory. - - * FlywheelIO.java - The base subsystem I/O (input/output) interface that contains the - structure and class variables needed by the library-specific modules. - - * FlywheelIOSim.java - Simulated flywheel module, for use with robot simulations, does not - control actual hardware. - - * FlywheelIOSparkMax.java - An example implementation of a flywheel using Rev SparkMax motor - controllers (and their associated library APIs). - - * FlywheelIOTalonFX.java - An example implementation of a flywheel using CTRE TalonFX motor - controllers (and their associated library APIs). - --------------------- -When creating a new mechanism subsystem, consider using this generic framework -for public vs. private functions and API calls to make your code more modular -and reusable from year to year. Additionally, the I/O framework given here -interfaces with AdvantageKit's logging to allow for replay of inputs and -testing of code changes in simulation. - \ No newline at end of file diff --git a/vendordeps/ChoreoLib2025.json b/vendordeps/ChoreoLib2025.json index 7bb0933..2c2d3c3 100644 --- a/vendordeps/ChoreoLib2025.json +++ b/vendordeps/ChoreoLib2025.json @@ -1,7 +1,7 @@ { "fileName": "ChoreoLib2025.json", "name": "ChoreoLib", - "version": "2025.0.2", + "version": "2025.0.3", "uuid": "b5e23f0a-dac9-4ad2-8dd6-02767c520aca", "frcYear": "2025", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-java", - "version": "2025.0.2" + "version": "2025.0.3" }, { "groupId": "com.google.code.gson", @@ -26,7 +26,7 @@ { "groupId": "choreo", "artifactId": "ChoreoLib-cpp", - "version": "2025.0.2", + "version": "2025.0.3", "libName": "ChoreoLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index c0f7c11..81144af 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2025.2.2", + "version": "2025.2.3", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2025", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2025.2.2" + "version": "2025.2.3" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2025.2.2", + "version": "2025.2.3", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/Phoenix6-frc2025-latest.json b/vendordeps/Phoenix6-frc2025-latest.json index 820c61a..78c7b40 100644 --- a/vendordeps/Phoenix6-frc2025-latest.json +++ b/vendordeps/Phoenix6-frc2025-latest.json @@ -1,7 +1,7 @@ { "fileName": "Phoenix6-frc2025-latest.json", "name": "CTRE-Phoenix (v6)", - "version": "25.2.1", + "version": "25.2.2", "frcYear": "2025", "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ @@ -19,14 +19,14 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "25.2.1" + "version": "25.2.2" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "api-cpp", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -40,7 +40,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -54,7 +54,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "api-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -68,7 +68,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -82,7 +82,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -96,7 +96,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +110,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -124,7 +124,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -138,7 +138,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -152,7 +152,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -166,7 +166,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -180,7 +180,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -194,7 +194,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -210,7 +210,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +226,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -242,7 +242,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -258,7 +258,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -274,7 +274,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -290,7 +290,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -306,7 +306,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -322,7 +322,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -338,7 +338,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -354,7 +354,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFXS", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProTalonFXS", "headerClassifier": "headers", "sharedLibrary": true, @@ -370,7 +370,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -386,7 +386,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true, @@ -402,7 +402,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANrange", - "version": "25.2.1", + "version": "25.2.2", "libName": "CTRE_SimProCANrange", "headerClassifier": "headers", "sharedLibrary": true, diff --git a/vendordeps/maple-sim.json b/vendordeps/maple-sim.json index 8c0771c..68ae20d 100644 --- a/vendordeps/maple-sim.json +++ b/vendordeps/maple-sim.json @@ -1,7 +1,7 @@ { "fileName": "maple-sim.json", "name": "maplesim", - "version": "0.3.7", + "version": "0.3.8", "frcYear": "2025", "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b", "mavenUrls": [ @@ -13,7 +13,7 @@ { "groupId": "org.ironmaple", "artifactId": "maplesim-java", - "version": "0.3.7" + "version": "0.3.8" }, { "groupId": "org.dyn4j",