From 6ac4b62089debe3f1faed7de0b4f6aaac2156336 Mon Sep 17 00:00:00 2001 From: C4llSiqn <83995467+C4llSqin@users.noreply.github.com> Date: Tue, 18 Mar 2025 15:44:51 -0600 Subject: [PATCH] end of day 2 --- .../autos/1 Coral Center Barge Bottom.auto | 31 +++++++++++++++++++ .../autos/1 Coral Center Barge Top.auto | 31 +++++++++++++++++++ .../paths/Bottom Feed to Reef 2.path | 12 +++---- .../pathplanner/paths/Cage 1 to Reef.path | 12 +++---- .../pathplanner/paths/Cage 2 to Reef 5.path | 12 +++---- .../pathplanner/paths/Cage 3 to Reef.path | 8 ++--- .../pathplanner/paths/Cage 4 to Reef.path | 8 ++--- .../pathplanner/paths/Cage 5 to Reef.path | 8 ++--- .../pathplanner/paths/Cage 6 to Reef.path | 8 ++--- .../paths/Reef 2 to Bottom Feed.path | 16 +++++----- .../paths/Reef 3 to Bottom Feed.path | 18 +++++------ .../paths/Reef 4 to Bottom Feed.path | 20 ++++++------ .../pathplanner/paths/Reef 4 to Top Feed.path | 16 +++++----- .../pathplanner/paths/Reef 5 to Top Feed.path | 22 ++++++------- .../pathplanner/paths/Reef 6 to Top Feed.path | 8 ++--- src/main/deploy/pathplanner/settings.json | 6 ++-- .../java/frc4388/robot/RobotContainer.java | 14 +++++---- 17 files changed, 157 insertions(+), 93 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto create mode 100644 src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto new file mode 100644 index 0000000..7adeb88 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Bottom.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Left" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-left-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto new file mode 100644 index 0000000..6d81c5b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/1 Coral Center Barge Top.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "prepare-l4" + } + }, + { + "type": "path", + "data": { + "pathName": "Center Barge to Reef 4 Right" + } + }, + { + "type": "named", + "data": { + "name": "place-coral-right-l4" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path index 521f2b1..943d856 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef 2.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.590135691664867, - "y": 2.114027889765366 + "x": 2.892340424329296, + "y": 2.067688275398428 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 2.3675 }, "prevControl": { - "x": 2.583188410703272, - "y": 1.782750079374932 + "x": 2.589485733053583, + "y": 1.921600370835535 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, diff --git a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path index b9271b0..710c3b1 100644 --- a/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 1 to Reef.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 5.594062500000001, - "y": 6.7671875 + "x": 6.292598684210526, + "y": 6.699835526315789 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.2528125, - "y": 5.7434375 + "x": 5.590213815789473, + "y": 5.622203947368421 }, "prevControl": { - "x": 5.50875, - "y": 6.145625 + "x": 5.846151315789473, + "y": 6.024391447368421 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path index eb7439a..1247110 100644 --- a/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path +++ b/src/main/deploy/pathplanner/paths/Cage 2 to Reef 5.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.3984375, - "y": 6.291875 + "x": 6.4946546052631575, + "y": 5.9878289473684205 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.265, - "y": 5.73125 + "x": 5.590213815789473, + "y": 5.593338815789473 }, "prevControl": { - "x": 5.8621875, - "y": 6.194375 + "x": 6.331085526315788, + "y": 5.958963815789473 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path index fdebcb4..5e34982 100644 --- a/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 3 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.2528125, - "y": 5.7434375 + "x": 5.619078947368421, + "y": 5.593338815789473 }, "prevControl": { - "x": 5.728125, - "y": 5.6703125 + "x": 6.094391447368421, + "y": 5.5202138157894725 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path index c439a80..c5ef9d6 100644 --- a/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 4 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.2771875, - "y": 2.27 + "x": 5.55172697368421, + "y": 2.4470394736842103 }, "prevControl": { - "x": 6.1059375, - "y": 2.5625000000000004 + "x": 6.3804769736842095, + "y": 2.7395394736842107 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path index 87801e6..452dec9 100644 --- a/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 5 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.754837328767123, - "y": 2.492380136986301 + "x": 5.5805921052631575, + "y": 2.4374177631578946 }, "prevControl": { - "x": 6.077263063386753, - "y": 2.432264238502225 + "x": 5.9030178398827875, + "y": 2.377301864673819 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path index 37daaac..27ace9e 100644 --- a/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path +++ b/src/main/deploy/pathplanner/paths/Cage 6 to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.2771875, - "y": 2.294375 + "x": 5.561348684210526, + "y": 2.456661184210526 }, "prevControl": { - "x": 5.599741161616161, - "y": 1.876556186868687 + "x": 5.883902345826687, + "y": 2.038842371079213 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path index 92cd109..dbd1a43 100644 --- a/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 2 to Bottom Feed.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 2.866114267676767, - "y": 1.8226988636363637 + "x": 2.9564891755960816, + "y": 2.1114702488693777 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.3803125 }, "prevControl": { - "x": 1.8063036616161614, - "y": 2.0447316919191922 + "x": 2.0502196323692035, + "y": 1.7292616486680126 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": 55.37584492005105 }, "reversed": false, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": 58.10920819815426 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path index 0d5a745..46f8cb1 100644 --- a/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 3 to Bottom Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.3015625, - "y": 2.2821875 + "x": 5.426644736842105, + "y": 2.851151315789474 }, "prevControl": null, "nextControl": { - "x": 4.106435503617635, - "y": 1.8391383607824139 + "x": 4.23151774045974, + "y": 2.4081021765718877 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.31625, - "y": 1.3559375 + "x": 1.7415296052631577, + "y": 1.5618421052631584 }, "prevControl": { - "x": 2.9441094150144833, - "y": 1.668218251495801 + "x": 3.369389020277641, + "y": 1.8741228567589594 }, "nextControl": null, "isLocked": false, @@ -41,7 +41,7 @@ "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": 55.05578949900953 }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path index dab21be..7fa944f 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Bottom Feed.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 6.3984375, - "y": 3.854374999999999 + "x": 5.9750822368421055, + "y": 3.9865131578947364 }, "prevControl": null, "nextControl": { - "x": 7.557357954545456, - "y": 2.073892045454545 + "x": 7.0046052631578934, + "y": 1.6388157894736843 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 1.368125 }, "prevControl": { - "x": 2.1450000000000005, - "y": 2.4650000000000007 + "x": 1.7992598684210526, + "y": 2.302713815789474 }, "nextControl": null, "isLocked": false, @@ -33,15 +33,15 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.0, + "maxAcceleration": 5.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, "unlimited": false }, "goalEndState": { - "velocity": 1.0, + "velocity": 0.0, "rotation": 52.857102599919905 }, "reversed": false, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -179.46454101443553 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path index 705c06b..ef11e1e 100644 --- a/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 4 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 6.4228125, - "y": 4.195625 + "x": 5.946217105263157, + "y": 4.178947368421053 }, "prevControl": null, "nextControl": { - "x": 7.0809375, - "y": 6.535625 + "x": 6.604342105263157, + "y": 6.518947368421053 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.291875, - "y": 6.7428125 + "x": 1.452878289473684, + "y": 6.565131578947369 }, "prevControl": { - "x": 1.8889109848484849, - "y": 5.739094065656566 + "x": 2.0499142743221688, + "y": 5.561413144603935 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path index 2accf7f..2301370 100644 --- a/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 5 to Top Feed.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.3015625, - "y": 5.755625 + "x": 5.41702302631579, + "y": 5.256578947368421 }, "prevControl": null, "nextControl": { - "x": 5.013494318181819, - "y": 7.207045454545455 + "x": 4.012253289473684, + "y": 5.958963815789473 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.2796875, - "y": 6.730625 + "x": 1.3951480263157894, + "y": 6.5747532894736835 }, "prevControl": { - "x": 2.3966445707070707, - "y": 5.275969065656566 + "x": 2.5121050970228604, + "y": 5.12009735513025 }, "nextControl": null, "isLocked": false, @@ -33,8 +33,8 @@ "pointTowardsZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, + "maxVelocity": 5.5, + "maxAcceleration": 5.5, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0, "nominalVoltage": 12.0, @@ -50,5 +50,5 @@ "velocity": 0, "rotation": -121.0370223454415 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path index b01e723..04ae5d9 100644 --- a/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path +++ b/src/main/deploy/pathplanner/paths/Reef 6 to Top Feed.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.31625, - "y": 6.730625 + "x": 1.4143914473684207, + "y": 6.584375 }, "prevControl": { - "x": 1.9132859848484847, - "y": 5.726906565656566 + "x": 2.0114274322169052, + "y": 5.580656565656565 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index e658ac9..cf4f1eb 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -15,11 +15,11 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 74.088, + "robotMass": 58.18, "robotMOI": 6.883, "robotTrackwidth": 0.546, - "driveWheelRadius": 0.048, - "driveGearing": 5.143, + "driveWheelRadius": 0.0508, + "driveGearing": 6.12, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", "driveCurrentLimit": 60.0, diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c5086ea..a67903a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -39,6 +39,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -90,6 +91,7 @@ import frc4388.utility.configurable.ConfigurableString; */ public class RobotContainer { /* RobotMap */ + public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ @@ -154,7 +156,7 @@ public class RobotContainer { waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), waitDebuger.asProxy(), - new ParallelDeadlineGroup( + new ParallelRaceGroup( new WaitCommand(1), new MoveUntilSuply( m_robotSwerveDrive, @@ -271,7 +273,7 @@ public class RobotContainer { waitDebuger.asProxy(), new LidarAlign(m_robotSwerveDrive, m_reefLidar), waitDebuger.asProxy(), - new ParallelDeadlineGroup( + new ParallelRaceGroup( new WaitCommand(1), new MoveUntilSuply( m_robotSwerveDrive, @@ -635,10 +637,10 @@ public class RobotContainer { ), m_robotSwerveDrive)) .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, - new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); - // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); + // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + // .onTrue( new DriveUntilLiDAR(m_robotSwerveDrive, + // new Translation2d(-1,0), new Translation2d(), m_reverseLidar, LiDARConstants.HUMAN_PLAYER_STATION_DISTANCE, true)); + // // .onTrue(new GotoLastApril(m_robotSwerveDrive, m_vision, AutoConstants.HALF_ROBOT_SIZE, Side.CENTER, true)); new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) .onTrue(thrustIntake.asProxy());