diff --git a/src/main/deploy/pathplanner/autos/Left.auto b/src/main/deploy/pathplanner/autos/Left.auto index 13c5621..735637c 100644 --- a/src/main/deploy/pathplanner/autos/Left.auto +++ b/src/main/deploy/pathplanner/autos/Left.auto @@ -21,7 +21,7 @@ { "type": "wait", "data": { - "waitTime": 0.85 + "waitTime": 0.62 } }, { @@ -39,7 +39,7 @@ { "type": "wait", "data": { - "waitTime": 0.1 + "waitTime": 0.2 } }, { @@ -103,7 +103,7 @@ { "type": "wait", "data": { - "waitTime": 0.95 + "waitTime": 1.25 } }, { @@ -121,7 +121,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 0.25 } }, { @@ -185,7 +185,7 @@ { "type": "wait", "data": { - "waitTime": 0.95 + "waitTime": 1.25 } }, { @@ -203,7 +203,7 @@ { "type": "wait", "data": { - "waitTime": 0.23 + "waitTime": 0.35 } }, { diff --git a/src/main/deploy/pathplanner/autos/LeftBLUE.auto b/src/main/deploy/pathplanner/autos/LeftBLUE.auto new file mode 100644 index 0000000..87c4523 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/LeftBLUE.auto @@ -0,0 +1,221 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftBLUE-1" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.62 + } + }, + { + "type": "named", + "data": { + "name": "ArmL4" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.2 + } + }, + { + "type": "named", + "data": { + "name": "ExtakeCoral" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.85 + } + }, + { + "type": "path", + "data": { + "pathName": "LeftBLUE-2" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ArmIntakeCoral" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeCoral" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftBLUE-3" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.25 + } + }, + { + "type": "named", + "data": { + "name": "ArmL4" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + }, + { + "type": "named", + "data": { + "name": "ExtakeCoral" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.85 + } + }, + { + "type": "path", + "data": { + "pathName": "LeftBLUE-4" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "ArmIntakeCoral" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "IntakeCoral" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "LeftBLUE-5" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 1.25 + } + }, + { + "type": "named", + "data": { + "name": "ArmL4" + } + } + ] + } + } + ] + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.35 + } + }, + { + "type": "named", + "data": { + "name": "ExtakeCoral" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Processor.auto b/src/main/deploy/pathplanner/autos/Processor.auto index 6c11f59..b7b76c4 100644 --- a/src/main/deploy/pathplanner/autos/Processor.auto +++ b/src/main/deploy/pathplanner/autos/Processor.auto @@ -21,7 +21,7 @@ { "type": "wait", "data": { - "waitTime": 0.92 + "waitTime": 0.62 } }, { @@ -39,7 +39,7 @@ { "type": "wait", "data": { - "waitTime": 0.15 + "waitTime": 0.2 } }, { @@ -203,7 +203,7 @@ { "type": "wait", "data": { - "waitTime": 0.27 + "waitTime": 0.35 } }, { diff --git a/src/main/deploy/pathplanner/autos/ProcessorBLUE.auto b/src/main/deploy/pathplanner/autos/ProcessorBLUE.auto index 8b2001e..cbc6026 100644 --- a/src/main/deploy/pathplanner/autos/ProcessorBLUE.auto +++ b/src/main/deploy/pathplanner/autos/ProcessorBLUE.auto @@ -21,7 +21,7 @@ { "type": "wait", "data": { - "waitTime": 0.82 + "waitTime": 0.72 } }, { diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path deleted file mode 100644 index 0f9275d..0000000 --- a/src/main/deploy/pathplanner/paths/Example Path.path +++ /dev/null @@ -1,54 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 2.0, - "y": 7.0 - }, - "prevControl": null, - "nextControl": { - "x": 3.0, - "y": 7.0 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 7.508607954545455, - "y": 6.133991477272727 - }, - "prevControl": { - "x": 6.508607954545455, - "y": 6.133991477272727 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "pointTowardsZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 5.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": 0.0 - }, - "reversed": false, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": 0.0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-1.path b/src/main/deploy/pathplanner/paths/LeftBLUE-1.path new file mode 100644 index 0000000..fadad37 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-1.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.568493824274356, + "y": 5.627 + }, + "prevControl": null, + "nextControl": { + "x": 5.9654605263157885, + "y": 5.381661184210526 + }, + "isLocked": true, + "linkedName": null + }, + { + "anchor": { + "x": 4.941, + "y": 5.17 + }, + "prevControl": { + "x": 5.399694206208194, + "y": 5.252906356202587 + }, + "nextControl": null, + "isLocked": true, + "linkedName": "Left-ReefJ" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6016042780748664, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": "LeftBLUE", + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-2.path b/src/main/deploy/pathplanner/paths/LeftBLUE-2.path new file mode 100644 index 0000000..0561ec5 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-2.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.941, + "y": 5.17 + }, + "prevControl": null, + "nextControl": { + "x": 4.878207236842105, + "y": 6.15139802631579 + }, + "isLocked": false, + "linkedName": "Left-ReefJ" + }, + { + "anchor": { + "x": 1.17635, + "y": 7.075 + }, + "prevControl": { + "x": 2.8865131578947363, + "y": 5.391282894736841 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Station-Left" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "station appr", + "minWaypointRelativePos": 0.9425133689839575, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.244 + }, + "reversed": false, + "folder": "LeftBLUE", + "idealStartingState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-3.path b/src/main/deploy/pathplanner/paths/LeftBLUE-3.path new file mode 100644 index 0000000..3659566 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-3.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.17635, + "y": 7.075 + }, + "prevControl": null, + "nextControl": { + "x": 2.6555921052631577, + "y": 6.0359375 + }, + "isLocked": false, + "linkedName": "Station-Left" + }, + { + "anchor": { + "x": 4.0043, + "y": 5.157 + }, + "prevControl": { + "x": 2.9538651315789473, + "y": 5.805016447368421 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left-ReefK" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "reef appr", + "minWaypointRelativePos": 0.8232421874999997, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "LeftBLUE", + "idealStartingState": { + "velocity": 0, + "rotation": -53.244 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-4.path b/src/main/deploy/pathplanner/paths/LeftBLUE-4.path new file mode 100644 index 0000000..0399713 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-4.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 4.0043, + "y": 5.157 + }, + "prevControl": null, + "nextControl": { + "x": 2.944243421052631, + "y": 5.862746710526316 + }, + "isLocked": false, + "linkedName": "Left-ReefK" + }, + { + "anchor": { + "x": 1.17635, + "y": 7.075 + }, + "prevControl": { + "x": 2.6074835526315785, + "y": 6.0551809210526315 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Station-Left" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "station appr", + "minWaypointRelativePos": 0.914438502673797, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -53.244 + }, + "reversed": false, + "folder": "LeftBLUE", + "idealStartingState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-5.path b/src/main/deploy/pathplanner/paths/LeftBLUE-5.path new file mode 100644 index 0000000..c675eac --- /dev/null +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-5.path @@ -0,0 +1,68 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.17635, + "y": 7.075 + }, + "prevControl": null, + "nextControl": { + "x": 2.3092105263157894, + "y": 6.15139802631579 + }, + "isLocked": false, + "linkedName": "Station-Left" + }, + { + "anchor": { + "x": 3.704971589432794, + "y": 4.996 + }, + "prevControl": { + "x": 3.0308388157894735, + "y": 5.7280427631578945 + }, + "nextControl": null, + "isLocked": false, + "linkedName": "Left-ReefL" + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "reef appr", + "minWaypointRelativePos": 0.838878542877907, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.2, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.99999999999999 + }, + "reversed": false, + "folder": "LeftBLUE", + "idealStartingState": { + "velocity": 0, + "rotation": -53.244 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Processor-1.path b/src/main/deploy/pathplanner/paths/Processor-1.path index a40ed2d..95465a4 100644 --- a/src/main/deploy/pathplanner/paths/Processor-1.path +++ b/src/main/deploy/pathplanner/paths/Processor-1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.95, - "y": 2.865 + "x": 4.975, + "y": 2.89 }, "prevControl": { - "x": 5.451026965738442, - "y": 2.7850796377032374 + "x": 5.476026965738441, + "y": 2.8100796377032373 }, "nextControl": null, "isLocked": true, diff --git a/src/main/deploy/pathplanner/paths/Processor-2.path b/src/main/deploy/pathplanner/paths/Processor-2.path index 1559960..60f1f5c 100644 --- a/src/main/deploy/pathplanner/paths/Processor-2.path +++ b/src/main/deploy/pathplanner/paths/Processor-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.95, - "y": 2.865 + "x": 4.975, + "y": 2.89 }, "prevControl": null, "nextControl": { - "x": 4.478062138878911, - "y": 1.6422191695918489 + "x": 4.50306213887891, + "y": 1.6672191695918488 }, "isLocked": true, "linkedName": "Processor-ReefE" diff --git a/src/main/deploy/pathplanner/paths/Processor-5.path b/src/main/deploy/pathplanner/paths/Processor-5.path index 9c6ae96..e861582 100644 --- a/src/main/deploy/pathplanner/paths/Processor-5.path +++ b/src/main/deploy/pathplanner/paths/Processor-5.path @@ -9,19 +9,19 @@ "prevControl": null, "nextControl": { "x": 2.308132596123537, - "y": 1.773069669586734 + "y": 1.7730696695867336 }, "isLocked": true, "linkedName": "Station-Processor" }, { "anchor": { - "x": 3.73, - "y": 3.02 + "x": 3.715, + "y": 3.005 }, "prevControl": { - "x": 2.9610293262395833, - "y": 2.504248924299277 + "x": 2.946029326239583, + "y": 2.489248924299277 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ProcessorBLUE-5.path b/src/main/deploy/pathplanner/paths/ProcessorBLUE-5.path index ca6a886..f5cf695 100644 --- a/src/main/deploy/pathplanner/paths/ProcessorBLUE-5.path +++ b/src/main/deploy/pathplanner/paths/ProcessorBLUE-5.path @@ -9,7 +9,7 @@ "prevControl": null, "nextControl": { "x": 2.308132596123537, - "y": 1.773069669586734 + "y": 1.7730696695867338 }, "isLocked": true, "linkedName": "Station-Processor" diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 8540b3f..3823ab2 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -3,10 +3,11 @@ "robotLength": 0.758, "holonomicMode": true, "pathFolders": [ + "LEAVE", + "Left", + "LeftBLUE", "Processor", "ProcessorBLUE", - "Left", - "LEAVE", "Tuning" ], "autoFolders": [], diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index ed0e4ca..ff7cebe 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -72,8 +72,8 @@ public static class CANdle { public static final double CLIMB_JOINT_THRESHOLD = 0.125; - public static final double ARM_JOINT_LOWER_BOUND = 0.1754; - public static final double ARM_JOINT_UPPER_BOUND = 0.1805; + public static final double ARM_JOINT_LOWER_BOUND = 0.183; + public static final double ARM_JOINT_UPPER_BOUND = 0.188; } public static class Climb { @@ -86,7 +86,7 @@ public static class Joint { public static final double KV = 1.2; public static final double KA = 0.1; - public static final double KP = 15.0; + public static final double KP = 20.0; public static final double KI = 0.0; public static final double KD = 0.0; @@ -95,8 +95,8 @@ public static class Joint { public static final GravityTypeValue GRAVITY_TYPE = GravityTypeValue.Arm_Cosine; - public static final double CURRENT_LIMIT = 20.0; - public static final double SUPPLY_CURRENT_LIMIT = 20.0; + public static final double CURRENT_LIMIT = 80.0; + public static final double SUPPLY_CURRENT_LIMIT = 80.0; public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; public static final double FORWARD_SOFT_LIMIT = 0.5; @@ -154,7 +154,9 @@ public static class Intake { public static final int FOLLOW_ID = 13; public static final double INTAKE_SPEED = 0.25; - public static final double INTAKE_LOWER_SPEED = 0.05; + public static final double INTAKE_LOWER_SPEED = 0.04; + + public static final double SCOOCH_SPEED = 0.06; public static final double EXTAKE_SPEED = 0.1; public static final double EXTAKE_TIMEOUT_S = 1.0; @@ -178,7 +180,7 @@ public static class Joint { public static final int CANCODER_ID = 11; public static final double CANCODER_GEAR_RATIO = 28.0 / 80.0; - public static final double CANCODER_OFFSET = -(0.358398 - (0.25 / CANCODER_GEAR_RATIO)); + public static final double CANCODER_OFFSET = -(0.352783 - (0.25 / CANCODER_GEAR_RATIO)); public static final double CANCODER_DISCONTINUITY_POINT = 0.5; public static final double CONVERSION_FACTOR = (120.0 / 12.0) * 20.0; @@ -204,17 +206,17 @@ public static class Joint { } public static class Positions { - public static double[] L4_SCORING_POS_M = { -0.24, 2.72 }; + public static double[] L4_SCORING_POS_M = { -0.24, 2.65 }; public static double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 }; public static double[] L4_CHECK_POSITION = { -0.25, 2.62 }; public static double[] L4_GRAND_SLAM = { -0.2, 1.57 }; - public static double[] L3_SCORING_POS_M = { -0.25, 1.57 }; + public static double[] L3_SCORING_POS_M = { -0.20, 1.57 }; public static double[] L2_SCORING_POS_M = { -0.2, 0.9 }; public static double[] L1_SCORING_POS_M = { 0.0, 0.0 }; - public static double[] INTAKE_POS_M = { 0.5, 0.835 }; - public static double[] INTAKE_POS_M_BLUE = { 0.5, 0.81 }; + public static double[] INTAKE_POS_M = { 0.5, 0.8425 }; + public static double[] INTAKE_POS_M_BLUE = { 0.5, 0.8425 }; public static double[] HOME_POS_M = { 0.0, 0.0 }; } diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index b286240..bb3ed0a 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -4,7 +4,9 @@ package raidzero.robot; -import static edu.wpi.first.units.Units.*; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RotationsPerSecond; import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType; import com.ctre.phoenix6.swerve.SwerveRequest; @@ -25,7 +27,8 @@ import raidzero.robot.subsystems.drivetrain.Limelight; import raidzero.robot.subsystems.drivetrain.Swerve; import raidzero.robot.subsystems.drivetrain.TunerConstants; -import raidzero.robot.subsystems.telescopingarm.*; +import raidzero.robot.subsystems.telescopingarm.Arm; +import raidzero.robot.subsystems.telescopingarm.CoralIntake; public class RobotContainer { private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed @@ -160,25 +163,16 @@ private void configureBindings() { .onlyIf(swerve.isArmDeployable()) ); - operator.button(Constants.Bindings.L4).and(operator.button(Constants.Bindings.ALGAE_INTAKE).negate()).whileTrue( + operator.button(Constants.Bindings.L4).whileTrue( arm.moveToL4() .onlyIf(swerve.isArmDeployable()) ); - operator.button(Constants.Bindings.L4).and(operator.button(Constants.Bindings.ALGAE_INTAKE)) - .whileTrue(arm.moveWithoutDelay(Constants.TelescopingArm.Positions.L4_CHECK_POSITION).onlyIf(() -> arm.isUp())); - operator.button(Constants.Bindings.L4).and(operator.button(Constants.Bindings.ALGAE_EXTAKE)) - .onTrue( - arm.moveWithoutDelay(Constants.TelescopingArm.Positions.L4_GRAND_SLAM).onlyIf(() -> arm.isUp()) - .until( - () -> arm.getJointPosition() >= arm.calculateJointAngle(Constants.TelescopingArm.Positions.L4_GRAND_SLAM) && - arm.getTelescopePosition() <= arm.calculateTelescopeHeight(Constants.TelescopingArm.Positions.L4_GRAND_SLAM) - ).withTimeout(0.5) - ); + operator.button(Constants.Bindings.ALGAE_INTAKE).onTrue(coralIntake.contingencyIntake()); operator.button(Constants.Bindings.CORAL_EXTAKE).whileTrue(coralIntake.extake()); operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); - operator.button(Constants.Bindings.CORAL_SCOOCH).whileTrue(coralIntake.run(-Constants.TelescopingArm.Intake.EXTAKE_SPEED)); + operator.button(Constants.Bindings.CORAL_SCOOCH).whileTrue(coralIntake.run(-Constants.TelescopingArm.Intake.SCOOCH_SPEED)); operator.button(Constants.Bindings.BOTTOM_RIGHT).onTrue(coralIntake.unstuckServo()); @@ -200,11 +194,14 @@ private void configureBindings() { // operator.button(Constants.Bindings.CLIMB_UP) // .whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED).onlyIf(climbJoint.isDeployed())); - operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED)); + operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3)); operator.button(Constants.Bindings.CLIMB_UP).onTrue(climbJoint.retract()); operator.button(Constants.Bindings.CLIMB_DOWN) - .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED).onlyIf(climbJoint.isDeployed())); + .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3).onlyIf(climbJoint.isDeployed())); + + // operator.axisGreaterThan(0, 0.6).whileTrue(climbJoint.run(0.125)); + // operator.axisGreaterThan(1, 0.6).whileTrue(climbJoint.run(0.28)); swerve.registerTelemetry(logger::telemeterize); } diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index 548ff28..39d3ad3 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -112,7 +112,8 @@ private void loopEstopped() { * The disabled loop of the CANdle LED strip */ private void loopDisabled() { - if (DriverStation.isAutonomous() && DriverStation.getMatchTime() > 0.0) { + if (DriverStation.isAutonomous() && DriverStation.getMatchTime() < 14.5 && + DriverStation.getMatchTime() > 0.0) { if (animation2Applied || animation3Applied) { resetAnimation(); } diff --git a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java index 306f5a8..a3f738e 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java +++ b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java @@ -151,4 +151,5 @@ public static ClimbJoint system() { return system; } + } diff --git a/src/main/java/raidzero/robot/subsystems/climb/Winch.java b/src/main/java/raidzero/robot/subsystems/climb/Winch.java index ef2eee1..2c584ec 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/Winch.java +++ b/src/main/java/raidzero/robot/subsystems/climb/Winch.java @@ -1,8 +1,10 @@ package raidzero.robot.subsystems.climb; +import com.ctre.phoenix6.configs.OpenLoopRampsConfigs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.robot.Constants; @@ -27,7 +29,7 @@ private Winch() { * @param speed The speed to run the winch at * @return A {@link Command} that runs the winch at the specified speed */ - public Command run(double speed) { + public Command run(double speed, boolean ramp) { return run(() -> winch.set(speed)); } @@ -46,7 +48,11 @@ public Command stop() { * @return A {@link TalonFXConfiguration} for the winch */ private TalonFXConfiguration winchConfiguration() { - TalonFXConfiguration configuration = new TalonFXConfiguration(); + TalonFXConfiguration configuration = new TalonFXConfiguration() + .withOpenLoopRamps( + new OpenLoopRampsConfigs() + .withDutyCycleOpenLoopRampPeriod(0.2) + ); return configuration; } diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index bc96b90..72eac00 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -2,6 +2,7 @@ import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; +import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFXS; @@ -17,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.lib.LazyCan; import raidzero.robot.Constants; +import raidzero.robot.Constants.TelescopingArm.Intake; public class CoralIntake extends SubsystemBase { private TalonFXS roller, follow; @@ -41,12 +43,12 @@ private CoralIntake() { bottomLaser = new LazyCan(Constants.TelescopingArm.Intake.BOTTOM_LASERCAN_ID) .withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 4, 6, 8) + .withRegionOfInterest(8, 8, 4, 4) .withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); topLaser = new LazyCan(Constants.TelescopingArm.Intake.TOP_LASERCAN_ID) .withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 4, 6, 8) + .withRegionOfInterest(8, 8, 4, 4) .withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); servoHub = new ServoHub(Constants.TelescopingArm.Intake.SERVO_HUB_ID); @@ -76,6 +78,23 @@ public Command intake() { .until(() -> getBottomLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); } + /** + * Creates a {@link Command} to intake the coral slower + * + * @return A {@link Command} to intake the coral slower + */ + public Command contingencyIntake() { + return startRun( + () -> roller.getConfigurator().apply( + rollerConfiguration().withCurrentLimits( + new CurrentLimitsConfigs().withSupplyCurrentLimit(3).withSupplyCurrentLowerLimit(1.5).withSupplyCurrentLowerTime(0.05) + ) + ), + () -> roller.set(Constants.TelescopingArm.Intake.INTAKE_SPEED - 0.05) + ).until(() -> getBottomLaserDistance() <= Intake.LASERCAN_DISTANCE_THRESHOLD_MM) + .finallyDo(() -> roller.getConfigurator().apply(rollerConfiguration())); + } + /** * Creates a {@link Command} to move the coral upwards to unstuck the servo block *