diff --git a/elastic-layout.json b/elastic-layout.json index 8a56808..61649a7 100644 --- a/elastic-layout.json +++ b/elastic-layout.json @@ -8,10 +8,10 @@ "layouts": [ { "title": "Tag Processed", - "x": 896.0, + "x": 384.0, "y": 0.0, "width": 256.0, - "height": 384.0, + "height": 256.0, "type": "List Layout", "properties": { "label_position": "TOP" @@ -56,10 +56,26 @@ ], "containers": [ { - "title": "RetractedLimit", - "x": 512.0, + "title": "MatchTime", + "x": 0.0, "y": 0.0, - "width": 256.0, + "width": 384.0, + "height": 256.0, + "type": "Match Time", + "properties": { + "topic": "/AdvantageKit/DriverStation/MatchTime", + "period": 0.06, + "data_type": "double", + "time_display_mode": "Minutes and Seconds", + "red_start_time": 15, + "yellow_start_time": 30 + } + }, + { + "title": "RetractedLimit", + "x": 0.0, + "y": 384.0, + "width": 128.0, "height": 128.0, "type": "Boolean Box", "properties": { @@ -74,9 +90,9 @@ }, { "title": "Auto Chooser", - "x": 896.0, - "y": 384.0, - "width": 256.0, + "x": 1024.0, + "y": 256.0, + "width": 384.0, "height": 128.0, "type": "ComboBox Chooser", "properties": { @@ -87,9 +103,9 @@ }, { "title": "Roller Percent Output", - "x": 512.0, + "x": 0.0, "y": 256.0, - "width": 384.0, + "width": 256.0, "height": 128.0, "type": "Number Slider", "properties": { @@ -104,8 +120,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 768.0, - "y": 0.0, + "x": 128.0, + "y": 384.0, "width": 128.0, "height": 128.0, "type": "Text Display", @@ -118,8 +134,8 @@ }, { "title": "TRIM SHOOTER SPEED", - "x": 512.0, - "y": 384.0, + "x": 256.0, + "y": 256.0, "width": 384.0, "height": 128.0, "type": "Number Slider", @@ -135,8 +151,8 @@ }, { "title": "Mode", - "x": 512.0, - "y": 128.0, + "x": 256.0, + "y": 384.0, "width": 384.0, "height": 128.0, "type": "Text Display", @@ -148,49 +164,58 @@ } }, { - "title": "IsActive", - "x": 0.0, - "y": 0.0, - "width": 384.0, - "height": 256.0, - "type": "Boolean Box", - "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive", - "period": 0.06, - "data_type": "boolean", - "true_color": 4283215696, - "false_color": 4294198070, - "true_icon": "None", - "false_icon": "None" - } - }, - { - "title": "RemainingInShift", - "x": 0.0, - "y": 256.0, - "width": 384.0, - "height": 128.0, - "type": "Match Time", - "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift", - "period": 0.06, - "data_type": "double", - "time_display_mode": "Minutes and Seconds", - "red_start_time": 15, - "yellow_start_time": 30 - } - }, - { - "title": "Phase", - "x": 0.0, - "y": 384.0, - "width": 384.0, + "title": "Time to rev", + "x": 768.0, + "y": 128.0, + "width": 256.0, "height": 128.0, "type": "Large Text Display", "properties": { - "topic": "/AdvantageKit/RealOutputs/HubShift/Phase", + "topic": "/AdvantageKit/RealOutputs/Time to rev", "period": 0.06, - "data_type": "string" + "data_type": "double" + } + }, + { + "title": "Arm angle extended", + "x": 1152.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm angle extended", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Current", + "x": 640.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Current", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Position", + "x": 768.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Position", + "period": 0.06, + "data_type": "double", + "show_submit_button": false } } ] @@ -530,8 +555,8 @@ }, { "title": "Shooter Idle max current", - "x": 1024.0, - "y": 384.0, + "x": 384.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -558,8 +583,8 @@ }, { "title": "Shooter OVERRIDE Velocity", - "x": 1024.0, - "y": 256.0, + "x": 640.0, + "y": 512.0, "width": 256.0, "height": 128.0, "type": "Text Display", @@ -578,40 +603,12 @@ "grid_layout": { "layouts": [], "containers": [ - { - "title": "Stalled Motor: ", - "x": 512.0, - "y": 128.0, - "width": 384.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ", - "period": 0.06, - "data_type": "string", - "show_submit_button": false - } - }, - { - "title": "Shooter idle % output", - "x": 128.0, - "y": 256.0, - "width": 256.0, - "height": 128.0, - "type": "Text Display", - "properties": { - "topic": "/SmartDashboard/Shooter idle % output", - "period": 0.06, - "data_type": "double", - "show_submit_button": true - } - }, { "title": "Auto Chooser", - "x": 512.0, - "y": 256.0, - "width": 384.0, - "height": 128.0, + "x": 0.0, + "y": 0.0, + "width": 640.0, + "height": 768.0, "type": "ComboBox Chooser", "properties": { "topic": "/SmartDashboard/Auto Chooser", @@ -621,6 +618,275 @@ } ] } + }, + { + "name": "ARM", + "grid_layout": { + "layouts": [], + "containers": [ + { + "title": "IntakeState", + "x": 640.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/RealOutputs/Intake/IntakeState", + "period": 0.06, + "data_type": "string", + "show_submit_button": false + } + }, + { + "title": "ArmAngle", + "x": 256.0, + "y": 128.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/ArmAngle", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "IntakeEncoder", + "x": 256.0, + "y": 0.0, + "width": 128.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/AdvantageKit/Intake/IntakeEncoder", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "RetractedLimitSwitch", + "x": 0.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Intake/RetractedLimitSwitch", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Roller Retract Output", + "x": 384.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Roller Retract Output", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Roller Percent Output", + "x": 384.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Roller Percent Output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Arm extend % output", + "x": 384.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm extend % output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Arm retract % output", + "x": 384.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm retract % output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "RetractedSoftLimit", + "x": 0.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Intake/RetractedSoftLimit", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "ExtendedSoftLimit", + "x": 0.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Boolean Box", + "properties": { + "topic": "/AdvantageKit/Intake/ExtendedSoftLimit", + "period": 0.06, + "data_type": "boolean", + "true_color": 4283215696, + "false_color": 4294198070, + "true_icon": "None", + "false_icon": "None" + } + }, + { + "title": "Arm reverse roller range", + "x": 640.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm reverse roller range", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Arm angle extended", + "x": 640.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm angle extended", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Arm Encoder Offset", + "x": 0.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Arm Encoder Offset", + "period": 0.06, + "data_type": "double", + "show_submit_button": false + } + }, + { + "title": "Bounce Half Period", + "x": 896.0, + "y": 0.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Bounce Half Period", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Bounce Max Output", + "x": 896.0, + "y": 128.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Bounce Max Output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Bounce Output", + "x": 896.0, + "y": 256.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Bounce Output", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Intake Bounce Current Limit", + "x": 896.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake Bounce Current Limit", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + }, + { + "title": "Intake Bounce Velocity Limit", + "x": 640.0, + "y": 384.0, + "width": 256.0, + "height": 128.0, + "type": "Text Display", + "properties": { + "topic": "/SmartDashboard/Intake Bounce Velocity Limit", + "period": 0.06, + "data_type": "double", + "show_submit_button": true + } + } + ] + } } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/New Path.path b/src/main/deploy/pathplanner/paths/New Path.path new file mode 100644 index 0000000..c93706b --- /dev/null +++ b/src/main/deploy/pathplanner/paths/New Path.path @@ -0,0 +1,155 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.61204761904762, + "y": 1.918666666666668 + }, + "prevControl": null, + "nextControl": { + "x": 2.5040714285714296, + "y": 1.4003809523809525 + }, + "isLocked": false, + "linkedName": "end of RIght trench first swipe" + }, + { + "anchor": { + "x": 3.648619047619049, + "y": 0.6121547619047625 + }, + "prevControl": { + "x": 3.4190610119047635, + "y": 0.7741190476190478 + }, + "nextControl": { + "x": 3.878177083333335, + "y": 0.45019047619047714 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.333047619047619, + "y": 0.6121547619047625 + }, + "prevControl": { + "x": 4.4368452380952395, + "y": 0.6229523809523821 + }, + "nextControl": { + "x": 6.519422842432679, + "y": 0.5978610845145801 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.391214285714287, + "y": 3.7434642857142855 + }, + "prevControl": { + "x": 5.376238095238096, + "y": 2.7932738095238094 + }, + "nextControl": { + "x": 6.987633102739957, + "y": 4.301813816546828 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.589750000000002, + "y": 1.2492142857142858 + }, + "prevControl": { + "x": 7.488215383486641, + "y": 1.4776671728693485 + }, + "nextControl": { + "x": 7.8057023809523844, + "y": 0.7633214285714278 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.648619047619049, + "y": 0.4717857142857145 + }, + "prevControl": { + "x": 4.501280949513, + "y": 0.5293980049542248 + }, + "nextControl": { + "x": 2.8495952380952403, + "y": 0.41779761904761914 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.7374404761904771, + "y": 0.9792738095238098 + }, + "prevControl": { + "x": 2.4068928571428576, + "y": 1.3032023809523823 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0.5, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 0.7377593360995809, + "rotationDegrees": -1.8097734110142742 + }, + { + "waypointRelativePos": 1.8273858921161648, + "rotationDegrees": 0.0 + }, + { + "waypointRelativePos": 2.364315352697095, + "rotationDegrees": 87.9267492938311 + }, + { + "waypointRelativePos": 3.5701244813277846, + "rotationDegrees": -59.49633403888164 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 10.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -132.7688253919687 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -133.33166255170664 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path b/src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path new file mode 100644 index 0000000..262e1f3 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/RightTrench-FirstSwipe.path @@ -0,0 +1,127 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 3.8969642857142865, + "y": 0.6757407407407425 + }, + "prevControl": null, + "nextControl": { + "x": 5.1494880952380955, + "y": 0.6013571428571436 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.237607142857144, + "y": 1.4759642857142854 + }, + "prevControl": { + "x": 7.558649163266191, + "y": 0.2980245529994656 + }, + "nextControl": { + "x": 8.584042208300819, + "y": 2.0770024068622486 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.237607142857144, + "y": 3.8190476190476197 + }, + "prevControl": { + "x": 8.310074297364315, + "y": 3.5787466468913385 + }, + "nextControl": { + "x": 8.165425863946698, + "y": 4.058400628167732 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 8.237607142857144, + "y": 1.0872500000000003 + }, + "prevControl": { + "x": 9.520930827264253, + "y": 2.0312275985936155 + }, + "nextControl": { + "x": 7.304441714310592, + "y": 0.40083918172158384 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.4434642857142865, + "y": 0.6757407407407425 + }, + "prevControl": { + "x": 4.9917133086412, + "y": 0.5083004760390194 + }, + "nextControl": { + "x": 2.6336428571428576, + "y": 0.7633214285714289 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.61204761904762, + "y": 1.918666666666668 + }, + "prevControl": { + "x": 2.502511133859423, + "y": 1.6751404175218187 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "end of RIght trench first swipe" + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1.9709543568464838, + "rotationDegrees": 90.0 + }, + { + "waypointRelativePos": 3.4315352697095514, + "rotationDegrees": 0.0 + } + ], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 600.0, + "maxAngularAcceleration": 750.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -133.33166255170664 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 90.7627685672325 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6bae83b..6eaa430 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -404,7 +404,7 @@ public class RobotContainer { new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0) .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Bouncing); + m_robotIntake.setMode(IntakeMode.RectractTorque); })) .onFalse(new InstantCommand(() -> { m_robotIntake.setMode(IntakeMode.Idle); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index fe771bf..21cf606 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -101,7 +101,7 @@ public class RobotMap { //Configure Intake 20,21 SparkMax arm = new SparkMax(IntakeConstants.ARM_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); - SparkMax roller = new SparkMax(IntakeConstants.ROLLER_ID.id, com.revrobotics.spark.SparkLowLevel.MotorType.kBrushless); + TalonFX roller = new TalonFX(IntakeConstants.ROLLER_ID.id, Constants.CANIVORE_CANBUS); // DigitalInput armLimitSwitch = new DigitalInput(IntakeConstants.ARM_LIMIT_SWITCH_CHANNEL); // DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH); // DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH); @@ -120,7 +120,7 @@ public class RobotMap { FaultTalonFX.addDevice(shooter2, "Shooter2"); FaultTalonFX.addDevice(indexer, "Indexer"); FaultSparkMax.addDevice(arm, "Arm"); - FaultSparkMax.addDevice(roller, "Roller"); + FaultTalonFX.addDevice(roller, "Roller"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); diff --git a/src/main/java/frc4388/robot/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 93a48fb..59403f0 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,12 +7,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "2026KPopRobotHunters"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 191; - public static final String GIT_SHA = "9021f480beaf35912eb54a845ec87522fef5337a"; - public static final String GIT_DATE = "2026-03-30 18:57:14 MDT"; + public static final int GIT_REVISION = 198; + public static final String GIT_SHA = "958bdc46fd90b8388750f8bf1a95b62ff91b2092"; + public static final String GIT_DATE = "2026-03-31 20:51:47 MDT"; public static final String GIT_BRANCH = "New-Intake"; - public static final String BUILD_DATE = "2026-03-30 19:50:05 MDT"; - public static final long BUILD_UNIX_TIME = 1774921805228L; + public static final String BUILD_DATE = "2026-04-01 17:29:06 MDT"; + public static final long BUILD_UNIX_TIME = 1775086146983L; public static final int DIRTY = 1; private BuildConstants(){} diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 9cc56af..5ff8b7e 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -37,8 +37,10 @@ public class Intake extends SubsystemBase { Retracting, Idle, + RectractTorque, Bouncing } + private boolean overCompressed = false; private IntakeMode mode = IntakeMode.Idle; @@ -85,6 +87,7 @@ public class Intake extends SubsystemBase { // return FieldZone.InShootZone; // } + @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter @@ -94,8 +97,17 @@ public class Intake extends SubsystemBase { Logger.processInputs("Intake", state); Logger.recordOutput("Intake/IntakeState", this.mode); + io.updateInputs(state); + if (state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_SQUEEZE_CURRENT_LOWER_THRESHOLD.get()){ + overCompressed = false; + } else if (state.armMotorCurrent.in(Amps) > IntakeConstants.INTAKE_SQUEEZE_CURRENT_UPPER_THRESHOLD.get()) { + overCompressed = true; + } + + Logger.recordOutput("overCompressed", overCompressed); + // getCurrentTime switch (mode) { @@ -154,6 +166,19 @@ public class Intake extends SubsystemBase { io.setRollerOutput(state, 0); } break; + case RectractTorque: + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + if (!overCompressed){ + io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get()); + } else if (overCompressed) { + io.armOutput(IntakeConstants.ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT.get()); + } + if(state.intakeEncoder.in(Rotations) > IntakeConstants.ARM_REVERSE_ROLLER_RANGE.get()) { + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + } else { + io.setRollerOutput(state, 0); + } + break; case Idle: io.armOutput(0); io.setRollerOutput(state, 0); diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index cc5216c..a5f7c02 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -1,5 +1,9 @@ package frc4388.robot.subsystems.intake; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.spark.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.LimitSwitchConfig.Behavior; @@ -25,6 +29,12 @@ public class IntakeConstants { public static final ConfigurableDouble INTAKE_BOUNCE_VELOCITY_LIMIT = new ConfigurableDouble("Intake Bounce Velocity Limit", 4); + //squeeze constants + public static final ConfigurableDouble INTAKE_SQUEEZE_CURRENT_LOWER_THRESHOLD = new ConfigurableDouble("Intake Squeeze Current LOWER THRESHOLD", 20); + public static final ConfigurableDouble INTAKE_SQUEEZE_CURRENT_UPPER_THRESHOLD = new ConfigurableDouble("Intake Squeeze Current UPPER THRESHOLD", 25); + public static final ConfigurableDouble ARM_SQUEEZE_PERCENT_OUTPUT = new ConfigurableDouble("Arm squeeze % output", -0.1); + public static final ConfigurableDouble ARM_REDUCED_SQUEEZE_PERCENT_OUTPUT = new ConfigurableDouble("Arm reduce squeeze % output", -0.02); + //IDs @@ -43,7 +53,7 @@ public class IntakeConstants { // public static final Angle ARM_LIMIT_UPPER = Degrees.of(-90); public static final ConfigurableDouble ARM_LIMIT_RETRACTED = new ConfigurableDouble("Arm angle retracted", 0.); - public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.5); + public static final ConfigurableDouble ARM_LIMIT_EXTENDED = new ConfigurableDouble("Arm angle extended", 1.75); public static final ConfigurableDouble ARM_EXTEND_PERCENT_OUTPUT = new ConfigurableDouble("Arm extend % output", 0.2); public static final ConfigurableDouble ARM_RETRACT_PERCENT_OUTPUT = new ConfigurableDouble("Arm retract % output", -0.2); @@ -79,7 +89,7 @@ public class IntakeConstants { // Motor configs public static final SparkMaxConfig ARM_MOTOR_CONFIG = new SparkMaxConfig(); - public static final SparkMaxConfig ROLELR_MOTOR_CONFIG = new SparkMaxConfig(); + public static final CanDevice ROLLER_MOTOR_ID = new CanDevice("INTAKE_ROLLER", 21); static { ARM_MOTOR_CONFIG.limitSwitch @@ -95,7 +105,7 @@ public class IntakeConstants { ARM_MOTOR_CONFIG.idleMode(IdleMode.kBrake); - ROLELR_MOTOR_CONFIG.idleMode(IdleMode.kCoast); + // ROLLER_MOTOR_CONFIG.idleMode(IdleMode.kCoast); } // public static final TalonFXConfiguration ARM_MOTOR_CONFIG = new TalonFXConfiguration() @@ -109,14 +119,14 @@ public class IntakeConstants { // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means // ); - // public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() - // .withCurrentLimits( - // new CurrentLimitsConfigs() - // .withStatorCurrentLimit(40) // TODO: tune??? - // .withStatorCurrentLimitEnable(true) - // ).withMotorOutput( - // new MotorOutputConfigs() - // .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny - // .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means - // ); + public static final TalonFXConfiguration ROLLER_MOTOR_CONFIG = new TalonFXConfiguration() + .withCurrentLimits( + new CurrentLimitsConfigs() + .withStatorCurrentLimit(40) // TODO: tune??? + .withStatorCurrentLimitEnable(true) + ).withMotorOutput( + new MotorOutputConfigs() + .withNeutralMode(NeutralModeValue.Coast) // Must be coast because this is spinny spinny + .withDutyCycleNeutralDeadband(0.04) // TODO: Figure out what this means + ); } diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 1ccfd72..4d294be 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -21,17 +21,18 @@ import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DutyCycleEncoder; +import frc4388.robot.subsystems.shooter.ShooterConstants; import frc4388.utility.compute.JankCoder; public class IntakeReal implements IntakeIO { SparkMax m_armMotor; - SparkMax m_rollerMotor; + TalonFX m_rollerMotor; JankCoder m_encoder; public IntakeReal( SparkMax armMotor, - SparkMax rollerMotor, + TalonFX rollerMotor, JankCoder jankCoder ) { // m_angleMotor = angleMotor; @@ -41,7 +42,7 @@ public class IntakeReal implements IntakeIO { m_encoder = jankCoder; m_armMotor.configure(IntakeConstants.ARM_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - m_rollerMotor.configure(IntakeConstants.ROLELR_MOTOR_CONFIG, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + m_rollerMotor.getConfigurator().apply(IntakeConstants.ROLLER_MOTOR_CONFIG); } @@ -113,7 +114,7 @@ public class IntakeReal implements IntakeIO { state.armMotorCurrent = Amps.of(m_armMotor.getOutputCurrent()); state.rollerOutput = m_rollerMotor.get(); - state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); + // state.rollerMotorCurrent = Amps.of(m_rollerMotor.getOutputCurrent()); state.retractedSoftLimit = retractedLimit(); state.extendedSoftLimit = extendedLimit(); diff --git a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java index 597b7a3..9e502de 100644 --- a/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java +++ b/src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java @@ -67,16 +67,16 @@ public final class SwerveDriveConstants { // Operation public static final double FORWARD_OFFSET = 90; // 0, 90, 180, 270 + public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // degrees to add to the angle when we are far away, to account for camera misalignment. TODO: find value. public static final boolean DRIFT_CORRECTION_ENABLED = true; public static final boolean INVERT_X = false; public static final boolean INVERT_Y = true; public static final boolean INVERT_ROTATION = false; - public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05); // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0); - public static final class ModuleSpecificConstants { //2026 + private static final class ModuleSpecificConstants { //2025 //Front Left private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043); private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false; @@ -166,6 +166,7 @@ public final class SwerveDriveConstants { public static final ConfigurableDouble AIM_kP = new ConfigurableDouble("Aim kP", 12); public static final ConfigurableDouble AIM_kI = new ConfigurableDouble("Aim kI", 0); public static final ConfigurableDouble AIM_kD = new ConfigurableDouble("Aim kD", 0.1); + public static final ConfigurableDouble HOLD_POSITION_kP = new ConfigurableDouble("Hold Position kP", 15); // public static final Gains AIM_GAINS = new Gains(2.5, 0, 0.1); }