From 1b9b7ed00cdc3bb4073de23e2f807faeb90cd6b9 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Sat, 25 Jan 2025 14:47:23 -0700 Subject: [PATCH 1/2] Debug autos --- .../deploy/pathplanner/autos/Stupid auto.auto | 31 +++++++++++ .../paths/Bottom Blue Cage to Reef.path | 4 +- .../paths/Bottom Feed to Reef.path | 4 +- .../paths/Bottom Red Cage to Reef.path | 4 +- .../paths/Bottom Reef to Feed.path | 4 +- .../paths/Center Barge to Reef.path | 4 +- .../paths/Center Blue Cage to Reef.path | 12 ++--- .../paths/Center Red Cage to Reef.path | 4 +- .../paths/Center Reef to Bottom Feed.path | 4 +- .../paths/Center Reef to Feed.path | 6 +-- .../pathplanner/paths/Stupid auto 1.path | 54 +++++++++++++++++++ .../pathplanner/paths/Stupid auto 2.path | 54 +++++++++++++++++++ .../pathplanner/paths/Stupid auto 3.path | 54 +++++++++++++++++++ src/main/deploy/pathplanner/paths/Taxi.path | 4 +- .../paths/Top Blue Cage to Reef.path | 4 +- .../pathplanner/paths/Top Feed to Reef.path | 20 +++---- .../paths/Top Red Cage to Reef.path | 4 +- .../pathplanner/paths/Top Reef to Feed.path | 12 ++--- src/main/deploy/pathplanner/settings.json | 2 +- src/main/java/frc4388/robot/Constants.java | 15 +++--- src/main/java/frc4388/robot/Robot.java | 7 +++ .../java/frc4388/robot/RobotContainer.java | 19 ++++--- .../robot/commands/GotoPositionCommand.java | 8 +-- 23 files changed, 272 insertions(+), 62 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Stupid auto.auto create mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 1.path create mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 2.path create mode 100644 src/main/deploy/pathplanner/paths/Stupid auto 3.path diff --git a/src/main/deploy/pathplanner/autos/Stupid auto.auto b/src/main/deploy/pathplanner/autos/Stupid auto.auto new file mode 100644 index 0000000..06f26cf --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Stupid auto.auto @@ -0,0 +1,31 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Stupid auto 1" + } + }, + { + "type": "path", + "data": { + "pathName": "Stupid auto 2" + } + }, + { + "type": "path", + "data": { + "pathName": "Stupid auto 3" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path index a5ccc16..0670521 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Blue Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -122.12499844038754 + "rotation": 60.35013649242443 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 178.97696981133208 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path index 1d37d81..cadc03f 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Feed to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.82647997035557 + "rotation": -121.27770286686648 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 54.32359177813805 + "rotation": -126.57303097851931 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path index 6b2c3d3..226a1a8 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Bottom Red Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 121.75948008481289 + "rotation": -57.4259428654274 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 179.30130561701642 + "rotation": -0.9548412538721401 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path index 117ae7d..0e53fb5 100644 --- a/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Bottom Reef to Feed.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 54.0394828033551 + "rotation": -123.50343698241421 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 119.16761337957772 + "rotation": -59.036243467926624 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path index 9e36d9d..ebcc6cf 100644 --- a/src/main/deploy/pathplanner/paths/Center Barge to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Barge to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.65844947344297 + "rotation": 0.5456575934157175 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path index 05a8393..2e99234 100644 --- a/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Blue Cage to Reef.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.197222222222222, - "y": 5.52929292929293 + "x": 5.268371212121212, + "y": 5.417487373737374 }, "prevControl": { - "x": 6.234912180385907, - "y": 5.781923450986739 + "x": 6.306061170284897, + "y": 5.670117895431184 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -121.60750224624891 + "rotation": 60.851928154286895 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 179.44374777291927 + "rotation": 1.1457628387483283 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path index 6a9c102..9597e5f 100644 --- a/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Center Red Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 118.92642583525368 + "rotation": -62.35402463626126 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 180.0 + "rotation": 1.97493401088202 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path index ad0027f..07e870b 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path +++ b/src/main/deploy/pathplanner/paths/Center Reef to Bottom Feed.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 50.964487101253106 + "rotation": -124.91183021661 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -178.99491399474581 + "rotation": -1.6365770416167795 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path index ce674ff..891990b 100644 --- a/src/main/deploy/pathplanner/paths/Center Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Center Reef to Feed.path @@ -25,7 +25,7 @@ }, "nextControl": { "x": 3.388005050505051, - "y": 6.11881313131313 + "y": 6.1188131313131295 }, "isLocked": false, "linkedName": null @@ -58,13 +58,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -52.30575953331084 + "rotation": 126.3843518158359 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -177.95459151111288 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Stupid auto 1.path b/src/main/deploy/pathplanner/paths/Stupid auto 1.path new file mode 100644 index 0000000..afb7b0c --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Stupid auto 1.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.081502525252525, + "y": 7.267361111111111 + }, + "prevControl": null, + "nextControl": { + "x": 4.871969696969696, + "y": 8.070328282828283 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.081502525252525, + "y": 5.10239898989899 + }, + "prevControl": { + "x": 4.821148989898989, + "y": 5.10239898989899 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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/Stupid auto 2.path b/src/main/deploy/pathplanner/paths/Stupid auto 2.path new file mode 100644 index 0000000..a849084 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Stupid auto 2.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 6.101830808080808, + "y": 5.041414141414141 + }, + "prevControl": null, + "nextControl": { + "x": 7.10183080808081, + "y": 5.041414141414141 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.585795454545455, + "y": 6.149305555555555 + }, + "prevControl": { + "x": 8.175315656565658, + "y": 5.295517676767678 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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/Stupid auto 3.path b/src/main/deploy/pathplanner/paths/Stupid auto 3.path new file mode 100644 index 0000000..c87eab2 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Stupid auto 3.path @@ -0,0 +1,54 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.585795454545455, + "y": 6.200126262626262 + }, + "prevControl": null, + "nextControl": { + "x": 7.778914141414141, + "y": 7.033585858585859 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.081502525252525, + "y": 7.32834595959596 + }, + "prevControl": { + "x": 7.595959595959596, + "y": 7.724747474747475 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.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/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index 8ff7750..325abd3 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 179.47479945510887 + "rotation": -1.9251837083231407 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 177.6140559696112 + "rotation": -1.1306909512379957 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path index 2a1b6ba..88c93bc 100644 --- a/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Blue Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -120.0183674276091 + "rotation": 60.255118703057796 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 179.30972280213487 + "rotation": -2.223961240385466 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path index 3c7f204..be71b4a 100644 --- a/src/main/deploy/pathplanner/paths/Top Feed to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Feed to Reef.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.263699494949495, - "y": 7.135227272727272 + "x": 1.0807449494949497, + "y": 6.972601010101011 }, "prevControl": null, "nextControl": { - "x": 1.6862557056350642, - "y": 6.8478211564289 + "x": 2.0869949494949496, + "y": 6.291603535353536 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.6217803030303033, - "y": 5.417487373737374 + "x": 3.764078282828282, + "y": 5.24469696969697 }, "prevControl": { - "x": 3.111746968518841, - "y": 5.776730048021274 + "x": 2.9836492542425197, + "y": 5.685899280282051 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -60.228308784269046 + "rotation": 121.34521414171566 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -55.124671655397854 + "rotation": 124.23611780915067 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path index fe0d749..cfabb8b 100644 --- a/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path +++ b/src/main/deploy/pathplanner/paths/Top Red Cage to Reef.path @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 120.25643716352924 + "rotation": -54.659893078442295 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -179.06080905426438 + "rotation": 0.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path index 32f6914..e9543c9 100644 --- a/src/main/deploy/pathplanner/paths/Top Reef to Feed.path +++ b/src/main/deploy/pathplanner/paths/Top Reef to Feed.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.192550505050505, - "y": 7.0234217171717175 + "x": 1.2230429292929292, + "y": 7.003093434343434 }, "prevControl": { - "x": 0.9300747161944047, - "y": 7.106516384157039 + "x": 0.960567140436829, + "y": 7.086188101328755 }, "nextControl": null, "isLocked": false, @@ -42,13 +42,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -52.56142842766699 + "rotation": 126.86989764584399 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -121.75948008481281 + "rotation": 57.21571913413089 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index 16b0598..f3e6372 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -16,7 +16,7 @@ "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", - "driveCurrentLimit": 60.0, + "driveCurrentLimit": 40.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5be92c8..4b09b12 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -230,7 +230,7 @@ public final class Constants { } public static final class AutoConstants { - public static final Gains XY_GAINS = new Gains(3,0,0); + public static final Gains XY_GAINS = new Gains(3,0.01,0); public static final Gains ROT_GAINS = new Gains(0.05,0,0.0); public static final Trim X_OFFSET_TRIM = new Trim("X Offset Trim", Double.MAX_VALUE, -Double.MAX_VALUE, 0.05, 0); @@ -240,7 +240,7 @@ public final class Constants { public static final int LIDAR_MICROS_TO_CM = 10; public static final int SECONDS_TO_MICROS = 1000000; - public static final double XY_TOLERANCE = 0.05; + public static final double XY_TOLERANCE = 0.1; public static final double ROT_TOLERANCE = 1; // public static final Pose2d targetpos = new Pose2d(new Translation2d(0.3,0), new Rotation2d()); @@ -249,8 +249,8 @@ public final class Constants { public static final class Configurations { - public static final double OPEN_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help. - public static final double CLOSED_LOOP_RAMP_RATE = 0.0; // Todo: Test. think this will help. + public static final double OPEN_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help. + public static final double CLOSED_LOOP_RAMP_RATE = 0.2; // Todo: Test. think this will help. public static final double NEUTRAL_DEADBAND = 0.04; // POWER! (limiting) @@ -282,7 +282,7 @@ public final class Constants { // new ClosedLoopRampsConfigs() // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); - private static final double SLIP_CURRENT = 100; // TODO: Tune??? + private static final double SLIP_CURRENT = 20; // TODO: Tune??? } // No mans land @@ -343,7 +343,7 @@ public final class Constants { public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); - public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters + public static final double MIN_ESTIMATION_DISTANCE = 0; // Meters // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate @@ -354,7 +354,8 @@ public final class Constants { public static final class FieldConstants { public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape); - public static final double HORISONTAL_SCORING_POSITION_OFFSET = 0; + public static final double HORISONTAL_SCORING_POSITION_OFFSET = .2794 + ; // public static final double HORISONTAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(6.5); // Positive is Right public static final double VERTICAL_SCORING_POSITION_OFFSET = Units.inchesToMeters(20); diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 0281e90..2181062 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -143,13 +143,20 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { + m_robotContainer.stop(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. + if (m_autonomousCommand != null) { CommandScheduler.getInstance().cancel(m_autonomousCommand); m_autonomousCommand.cancel(); + m_autonomousCommand.end(true); + System.out.println("NOT Null!!"); + + } else { + System.out.println("Null!!"); } m_robotTime.startMatchTime(); } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 7735d99..98e92e6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,6 +92,10 @@ public class RobotContainer { // public List subsystems = new ArrayList<>(); // ! Teleop Commands + public void stop() { + new InstantCommand(()->{}, m_robotSwerveDrive).schedule();;;;; + m_robotSwerveDrive.stopModules(); + } // ! /* Autos */ private String lastAutoName = "defualt.auto"; @@ -104,8 +108,10 @@ public class RobotContainer { private Command AutoGotoPosition = new GotoPositionCommand(m_robotSwerveDrive, m_vision); private Command placeCoral = new SequentialCommandGroup( + new InstantCommand(() -> m_robotSwerveDrive.stopModules()), new InstantCommand(() -> System.out.println("Placing Some Coral")), - new WaitCommand(3) + new WaitCommand(3), + new InstantCommand(() -> System.out.println("Done placing Some Coral")) ); private Command aprilAlign = new SequentialCommandGroup( @@ -115,7 +121,8 @@ public class RobotContainer { private Command grabCoral = new SequentialCommandGroup( new InstantCommand(() -> System.out.println("Yoinking some coral...")), - new WaitCommand(2) + new WaitCommand(2), + new InstantCommand(() -> System.out.println("Done yoinking some coral...")) ); /** @@ -210,10 +217,10 @@ public class RobotContainer { // ? /* Operator Buttons */ new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - .onTrue(new GotoPositionCommand(m_robotSwerveDrive, m_vision)); + .onTrue(AutoGotoPosition); new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - .onTrue(new InstantCommand(() -> {}, m_robotSwerveDrive)); + .onTrue(new InstantCommand(()->{}, m_robotSwerveDrive)); // creates an empty command & requires the swerve drive, subsystems can run only 1 command at a time // ? /* Programer Buttons (Controller 3)*/ @@ -300,10 +307,10 @@ public class RobotContainer { //return autoPlayback; //return new GotoPositionCommand(m_robotSwerveDrive, m_vision) - return autoChooser.getSelected(); + return autoChooser.getSelected().andThen(new InstantCommand(() -> System.err.println("Auto Ded!"))); // try{ // // Load the path you want to follow using its name in the GUI - // return new PathPlannerAuto("New Auto"); + // return new PathPlannerAuto("New Auto"); // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); // return Commands.none(); diff --git a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java index 1d5755b..4b4b115 100644 --- a/src/main/java/frc4388/robot/commands/GotoPositionCommand.java +++ b/src/main/java/frc4388/robot/commands/GotoPositionCommand.java @@ -37,14 +37,14 @@ public class GotoPositionCommand extends Command { public GotoPositionCommand(SwerveDrive swerveDrive, Vision vision) { this.swerveDrive = swerveDrive; this.vision = vision; - addRequirements(swerveDrive); + // addRequirements(swerveDrive); } @Override public void initialize() { xPID.initialize(); yPID.initialize(); - this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.RIGHT, AutoConstants.X_OFFSET_TRIM.get()); + this.targetpos = ReefPositionHelper.getNearestPosition(this.vision.getPose2d(), Side.LEFT, AutoConstants.X_OFFSET_TRIM.get()); } double xerr; @@ -84,7 +84,9 @@ public class GotoPositionCommand extends Command { @Override public final boolean isFinished() { - return (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); + boolean finished = (Math.abs(xerr) < AutoConstants.XY_TOLERANCE && Math.abs(yerr) < AutoConstants.XY_TOLERANCE && Math.abs(roterr) < AutoConstants.ROT_TOLERANCE); + // System.out.println(finished); + return finished; // this statement is a boolean in and of itself } From 526cea9e642c2ace8d27cc2149526fe2e40713f0 Mon Sep 17 00:00:00 2001 From: Michael Mikovsky <77305074+Astatin3@users.noreply.github.com> Date: Tue, 28 Jan 2025 17:05:10 -0700 Subject: [PATCH 2/2] remove autochooser --- .../deploy/pathplanner/autos/Test Auto.auto | 19 +++++++++++++++++++ src/main/deploy/pathplanner/paths/Taxi.path | 16 ++++++++++++++++ src/main/deploy/pathplanner/settings.json | 4 ++-- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/Robot.java | 2 ++ .../java/frc4388/robot/RobotContainer.java | 6 +++--- 6 files changed, 44 insertions(+), 7 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Test Auto.auto diff --git a/src/main/deploy/pathplanner/autos/Test Auto.auto b/src/main/deploy/pathplanner/autos/Test Auto.auto new file mode 100644 index 0000000..2e09e4c --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Test Auto.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Stupid auto 1" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Taxi.path b/src/main/deploy/pathplanner/paths/Taxi.path index 325abd3..7b32a44 100644 --- a/src/main/deploy/pathplanner/paths/Taxi.path +++ b/src/main/deploy/pathplanner/paths/Taxi.path @@ -23,6 +23,22 @@ "x": 5.928529040722133, "y": 7.353679406601922 }, + "nextControl": { + "x": 6.681698232005139, + "y": 7.343669078246562 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.8889204545454543, + "y": 7.348674242424242 + }, + "prevControl": { + "x": 4.138853417354843, + "y": 7.3544633789160185 + }, "nextControl": null, "isLocked": false, "linkedName": null diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json index f3e6372..5bf438c 100644 --- a/src/main/deploy/pathplanner/settings.json +++ b/src/main/deploy/pathplanner/settings.json @@ -9,14 +9,14 @@ "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "defaultNominalVoltage": 12.0, - "robotMass": 22.6796, + "robotMass": 74.088, "robotMOI": 6.883, "robotTrackwidth": 0.546, "driveWheelRadius": 0.048, "driveGearing": 5.143, "maxDriveSpeed": 5.45, "driveMotorType": "krakenX60", - "driveCurrentLimit": 40.0, + "driveCurrentLimit": 60.0, "wheelCOF": 1.2, "flModuleX": 0.273, "flModuleY": 0.273, diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4b09b12..b85592d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -282,7 +282,7 @@ public final class Constants { // new ClosedLoopRampsConfigs() // .withDutyCycleClosedLoopRampPeriod(SwerveDriveConstants.Configurations.CLOSED_LOOP_RAMP_RATE) ); - private static final double SLIP_CURRENT = 20; // TODO: Tune??? + private static final double SLIP_CURRENT = 100; // TODO: Tune??? } // No mans land @@ -343,7 +343,7 @@ public final class Constants { public static final Transform3d CAMERA_POS = new Transform3d(new Translation3d(-.3048, 0.2413, .2794), new Rotation3d(0,0.52333,Math.PI)); - public static final double MIN_ESTIMATION_DISTANCE = 0; // Meters + public static final double MIN_ESTIMATION_DISTANCE = 1; // Meters // Photonvision thing // The standard deviations of our vision estimated poses, which affect correction rate diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2181062..828ed46 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -17,6 +17,7 @@ import com.ctre.phoenix6.CANBus.CANBusStatus; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; @@ -85,6 +86,7 @@ public class Robot extends TimedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); + SmartDashboard.putNumber("Time", System.currentTimeMillis()); //System.out.println(m_robotContainer.limelight.isNearSpeaker()); //mled.updateLED(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 98e92e6..ea43541 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -131,7 +131,7 @@ public class RobotContainer { public RobotContainer() { NamedCommands.registerCommand("AutoGotoPosition", AutoGotoPosition); - NamedCommands.registerCommand("april-allign", aprilAlign); + NamedCommands.registerCommand("april-allign", AutoGotoPosition); NamedCommands.registerCommand("place-coral", placeCoral); NamedCommands.registerCommand("grab-coral", grabCoral); @@ -307,10 +307,10 @@ public class RobotContainer { //return autoPlayback; //return new GotoPositionCommand(m_robotSwerveDrive, m_vision) - return autoChooser.getSelected().andThen(new InstantCommand(() -> System.err.println("Auto Ded!"))); + //return autoChooser.getSelected(); // try{ // // Load the path you want to follow using its name in the GUI - // return new PathPlannerAuto("New Auto"); + return new PathPlannerAuto("Red Center Cage"); // } catch (Exception e) { // DriverStation.reportError("Path planner error: " + e.getMessage(), e.getStackTrace()); // return Commands.none();