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/constants/BuildConstants.java b/src/main/java/frc4388/robot/constants/BuildConstants.java index 93a48fb..c6f0611 100644 --- a/src/main/java/frc4388/robot/constants/BuildConstants.java +++ b/src/main/java/frc4388/robot/constants/BuildConstants.java @@ -7,9 +7,9 @@ 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 = 189; + public static final String GIT_SHA = "65c76aca95de81f342df7ff26a77e18856f22a83"; + public static final String GIT_DATE = "2026-03-28 15:56:54 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; diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index 9cc56af..1d889d7 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -37,6 +37,7 @@ public class Intake extends SubsystemBase { Retracting, Idle, + RectractTorque, Bouncing } @@ -85,6 +86,8 @@ public class Intake extends SubsystemBase { // return FieldZone.InShootZone; // } + + @Override public void periodic() { // FaultReporter.register(this); // TODO Implement fault reporter @@ -94,6 +97,7 @@ public class Intake extends SubsystemBase { Logger.processInputs("Intake", state); Logger.recordOutput("Intake/IntakeState", this.mode); + io.updateInputs(state); // getCurrentTime @@ -154,6 +158,19 @@ public class Intake extends SubsystemBase { io.setRollerOutput(state, 0); } break; + case RectractTorque: + io.setRollerOutput(state, IntakeConstants.ROLLER_RETRACT_PERCENT_OUTPUT.get()); + if (state.armMotorCurrent.in(Amps) < IntakeConstants.INTAKE_SQUEEZE_CURRENT_LIMIT.get()){ + io.armOutput(IntakeConstants.ARM_SQUEEZE_PERCENT_OUTPUT.get()); + } else { + 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 bc95154..1d1cb03 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -25,6 +25,11 @@ 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_LIMIT = new ConfigurableDouble("Intake Squeeze Current Limit", 20); + 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 squeeze % output", -0.02); + //IDs