diff --git a/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto b/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto index 87a18024..35bf4808 100644 --- a/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto +++ b/src/main/deploy/pathplanner/autos/5mForwardRotate180.auto @@ -2,7 +2,7 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.4, + "x": 2.3, "y": 7.0 }, "rotation": 0 @@ -14,7 +14,7 @@ { "type": "path", "data": { - "pathName": "5mForwardRotate" + "pathName": "bonkRotateForward" } } ] diff --git a/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto b/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto index 0aef4b66..b8715fbe 100644 --- a/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto +++ b/src/main/deploy/pathplanner/autos/PresetSourceSide1ScorePassAutoline.auto @@ -11,12 +11,6 @@ "type": "sequential", "data": { "commands": [ - { - "type": "named", - "data": { - "name": "SubwooferLaunch" - } - }, { "type": "path", "data": { diff --git a/src/main/deploy/pathplanner/autos/TuneTranslationPID back.auto b/src/main/deploy/pathplanner/autos/TuneTranslationPID back.auto new file mode 100644 index 00000000..c35f1799 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/TuneTranslationPID back.auto @@ -0,0 +1,25 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 6.3, + "y": 7.0 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "TuneRotationPID back" + } + } + ] + } + }, + "folder": "Test", + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/VisionAmpSideFarAutoline3Score.auto b/src/main/deploy/pathplanner/autos/VisionAmpSideFarAutoline3Score.auto index 430b21d8..62149fa6 100644 --- a/src/main/deploy/pathplanner/autos/VisionAmpSideFarAutoline3Score.auto +++ b/src/main/deploy/pathplanner/autos/VisionAmpSideFarAutoline3Score.auto @@ -23,18 +23,6 @@ "pathName": "HighSpeakerToHighNoteLaunch2" } }, - { - "type": "named", - "data": { - "name": "VisionLaunch" - } - }, - { - "type": "path", - "data": { - "pathName": "HighNoteLaunch2ToFarHighNoteToLaunch" - } - }, { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path b/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path index 6a28feb8..17b5d536 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerLaunchHighNote.path @@ -31,7 +31,7 @@ "rotationTargets": [ { "waypointRelativePos": 0.35, - "rotationDegrees": -169.16565991676583, + "rotationDegrees": -167.0, "rotateFast": false }, { diff --git a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path index 18cfec5e..f7ab1a7f 100644 --- a/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path +++ b/src/main/deploy/pathplanner/paths/HighSpeakerToHighNoteLaunch2.path @@ -71,6 +71,12 @@ "data": { "name": "Intake" } + }, + { + "type": "named", + "data": { + "name": "RetractPivot" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/TuneRotationPID back.path b/src/main/deploy/pathplanner/paths/TuneRotationPID back.path new file mode 100644 index 00000000..f94f7563 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/TuneRotationPID back.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 6.3, + "y": 7.0 + }, + "prevControl": null, + "nextControl": { + "x": 5.3, + "y": 7.0 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": { + "x": 3.218814272122443, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 4.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path b/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path index 7dbd0f72..30d0b1a4 100644 --- a/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path +++ b/src/main/deploy/pathplanner/paths/TuneTranslationalPID.path @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 2.0, "maxAcceleration": 3.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 720.0 @@ -48,5 +48,5 @@ "rotation": 0, "velocity": 0 }, - "useDefaultConstraints": true + "useDefaultConstraints": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/bonkRotateForward.path b/src/main/deploy/pathplanner/paths/bonkRotateForward.path index aeeaae62..357b3d03 100644 --- a/src/main/deploy/pathplanner/paths/bonkRotateForward.path +++ b/src/main/deploy/pathplanner/paths/bonkRotateForward.path @@ -3,12 +3,12 @@ "waypoints": [ { "anchor": { - "x": 0.4, + "x": 2.3, "y": 7.0 }, "prevControl": null, "nextControl": { - "x": 1.4, + "x": 2.1999999999999997, "y": 7.0 }, "isLocked": false, @@ -16,11 +16,27 @@ }, { "anchor": { - "x": 5.5, + "x": 5.335023775460655, "y": 7.0 }, "prevControl": { - "x": 4.5, + "x": 4.835024537004013, + "y": 6.999127335817051 + }, + "nextControl": { + "x": 5.835023013917296, + "y": 7.000872664182949 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.3, + "y": 7.0 + }, + "prevControl": { + "x": 2.5999999999999996, "y": 7.0 }, "nextControl": null, @@ -30,8 +46,13 @@ ], "rotationTargets": [ { - "waypointRelativePos": 0.5, - "rotationDegrees": 45.0, + "waypointRelativePos": 1.0, + "rotationDegrees": 1.8407261107289572, + "rotateFast": false + }, + { + "waypointRelativePos": 0.55, + "rotationDegrees": 89.63503006133968, "rotateFast": false } ], @@ -39,7 +60,7 @@ "eventMarkers": [ { "name": "New Event Marker", - "waypointRelativePos": 0.25, + "waypointRelativePos": 0.5, "command": { "type": "parallel", "data": { diff --git a/src/main/deploy/swerve/controllerproperties.json b/src/main/deploy/swerve/controllerproperties.json index ffb1caf4..782e1ee0 100644 --- a/src/main/deploy/swerve/controllerproperties.json +++ b/src/main/deploy/swerve/controllerproperties.json @@ -1,8 +1,8 @@ { "angleJoystickRadiusDeadband": 0.5, "heading": { - "p": 0.4, + "p": 0.25, "i": 0, - "d": 0.15 + "d": 0 } } \ No newline at end of file diff --git a/src/main/deploy/swerve/modules/physicalproperties.json b/src/main/deploy/swerve/modules/physicalproperties.json index 8b88ec01..26fd831e 100644 --- a/src/main/deploy/swerve/modules/physicalproperties.json +++ b/src/main/deploy/swerve/modules/physicalproperties.json @@ -11,7 +11,7 @@ }, "conversionFactor": { "angle": 19.2, - "drive": 0.0696793458626792 + "drive": 0.06681204078043 }, "rampRate": { "drive": 0.25, diff --git a/src/main/java/frc/team2412/robot/Controls.java b/src/main/java/frc/team2412/robot/Controls.java index c137e3d9..bc75f89b 100644 --- a/src/main/java/frc/team2412/robot/Controls.java +++ b/src/main/java/frc/team2412/robot/Controls.java @@ -30,6 +30,7 @@ import frc.team2412.robot.commands.launcher.SetPivotCommand; import frc.team2412.robot.subsystems.LauncherSubsystem; import frc.team2412.robot.util.AmpAlign; +import frc.team2412.robot.util.TrapAlign; public class Controls { public static class ControlConstants { @@ -58,7 +59,7 @@ public static class ControlConstants { private final Trigger launcherSubwooferPresetButton; private final Trigger launcherLowerPresetButton; // private final Trigger launcherPodiumPresetButton; - // private final Trigger launcherTrapPresetButton; + private final Trigger launcherTrapPresetButton; private final Trigger launcherAmpAlignPresetButton; private final Trigger launcherLaunchButton; @@ -76,7 +77,7 @@ public Controls(Subsystems s) { launcherSubwooferPresetButton = codriveController.a(); launcherLowerPresetButton = codriveController.y(); // launcherPodiumPresetButton = codriveController.povLeft(); - // launcherTrapPresetButton = codriveController.start(); + launcherTrapPresetButton = codriveController.start(); launcherAmpAlignPresetButton = driveController.y(); launcherLaunchButton = codriveController.rightBumper(); // intake controls (confirmed with driveteam) @@ -228,8 +229,8 @@ private void bindLauncherControls() { LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, LauncherSubsystem.AMP_AIM_ANGLE)); - // launcherTrapPresetButton.onTrue( - // TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); + launcherTrapPresetButton.whileTrue( + TrapAlign.trapPreset(s.drivebaseSubsystem, s.launcherSubsystem)); launcherAmpAlignPresetButton.onTrue( Commands.either( AmpAlign.ampPreset(s.drivebaseSubsystem, s.launcherSubsystem), diff --git a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java index 9c54c813..688b263a 100644 --- a/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/DrivebaseSubsystem.java @@ -78,7 +78,8 @@ public class DrivebaseSubsystem extends SubsystemBase { : Robot.getInstance().getRobotType() == RobotType.CRANE ? new PIDConstants(3.9, 0, 0.2) // crane : new PIDConstants(0.1, 0, 0.1); // bobot TODO: tune - private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(5.5, 0, 0); + private static final PIDConstants AUTO_ROTATION_PID = new PIDConstants(5.5, 0, 1); + // 7 0 0.2 private static final double MAX_AUTO_SPEED = 500.0; // this seems to only affect rotation for some reason diff --git a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java index 8b55a939..a1215a77 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LauncherSubsystem.java @@ -38,9 +38,9 @@ public class LauncherSubsystem extends SubsystemBase { // HARDWARE private static final double PIVOT_GEARING_RATIO = 1.0 / 180.0; private static final double PIVOT_TO_ENCODER_GEARING_RATIO = 1.0 / 2.0; - private static final float PIVOT_SOFTSTOP_FORWARD = 0.93f; + private static final float PIVOT_SOFTSTOP_FORWARD = 0.99f; private static final float PIVOT_SOFTSTOP_BACKWARD = 0.635f; - private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.93f; + private static final float PIVOT_SOFTSTOP_FORWARD_THROUGHBORE = 0.99f; private static final float PIVOT_SOFTSTOP_BACKWARD_THROUGHBORE = 0.38f; private static final float PIVOT_DISABLE_OFFSET = 0.04f; private static final int PIVOT_OFFSET = USE_THROUGHBORE ? 40 : 36; @@ -51,9 +51,9 @@ public class LauncherSubsystem extends SubsystemBase { // ANGLE VALUES public static final int AMP_AIM_ANGLE = 290 + PIVOT_OFFSET; - public static final int SUBWOOFER_AIM_ANGLE = 252 + PIVOT_OFFSET; + public static final double SUBWOOFER_AIM_ANGLE = 253.5 + PIVOT_OFFSET; public static final int PODIUM_AIM_ANGLE = 238 + PIVOT_OFFSET; - public static final int TRAP_AIM_ANGLE = 317 + PIVOT_OFFSET; + public static final double TRAP_AIM_ANGLE = 311.879 + PIVOT_OFFSET; public static final double MANUAL_MODIFIER = 0.02; public static final double RETRACTED_ANGLE = 242 + PIVOT_OFFSET; // offset for FF so parallel to floor is 0 @@ -65,7 +65,7 @@ public class LauncherSubsystem extends SubsystemBase { public static final double ANGLE_TOLERANCE = 5; public static final double RPM_TOLERANCE = 500; // RPM - public static final int SPEAKER_SHOOT_SPEED_RPM = 4500; + public static final int SPEAKER_SHOOT_SPEED_RPM = 3800; public static final int TRAP_SHOOT_SPEED_RPM = 4500; public static final int LOBBING_RPM = 4700; public static final double ANGLE_MAX_SPEED = 0.3; // percent output diff --git a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java index a3109e6a..5f7992c7 100644 --- a/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java +++ b/src/main/java/frc/team2412/robot/subsystems/LimelightSubsystem.java @@ -13,6 +13,7 @@ public class LimelightSubsystem extends SubsystemBase { // POTENTIAL CONSTANTS private GenericEntry GOAL_DISTANCE_FROM_NOTE; + private final double AUTO_DETECTION_DISTANCE_FROM_NOTE = 3.5; // meters // MEMBERS @@ -69,6 +70,10 @@ private void setPipeline() { // METHODS + public boolean isNoteInFront() { + return (hasTargets() && getDistanceFromTargetInches() <= AUTO_DETECTION_DISTANCE_FROM_NOTE); + } + public boolean hasTargets() { return (networkTable.getEntry("tv").getDouble(0) != 0); } diff --git a/src/main/java/frc/team2412/robot/util/TrapAlign.java b/src/main/java/frc/team2412/robot/util/TrapAlign.java index eeaa138d..d58969d9 100644 --- a/src/main/java/frc/team2412/robot/util/TrapAlign.java +++ b/src/main/java/frc/team2412/robot/util/TrapAlign.java @@ -30,7 +30,7 @@ public class TrapAlign { // trap that faces amp new Pose2d(new Translation2d(12.3, 5.14), Rotation2d.fromDegrees(-120)), // trap that faces source - new Pose2d(new Translation2d(12.3, 3.09), Rotation2d.fromDegrees(300)), + new Pose2d(new Translation2d(12.15, 3.45), Rotation2d.fromDegrees(120)), // trap that faces mid // DO THIS ONE FIRST // brute force the X lol diff --git a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java index 9df1141e..3f1fcb5f 100644 --- a/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java +++ b/src/main/java/frc/team2412/robot/util/auto/AutoLogic.java @@ -48,7 +48,7 @@ public class AutoLogic { public static final double FEEDER_DELAY = 0.4; // rpm to rev up launcher before launching - public static final double REV_RPM = 2500; + public static final double REV_RPM = 3400; public static final double STAGE_ANGLE = 262; public static enum StartPosition { @@ -88,6 +88,7 @@ public static enum StartPosition { new AutoPath("Test Path", "DiameterTest"), new AutoPath("Master PID Test", "MasterPIDTest"), new AutoPath("Tune Translational PID", "TuneTranslationalPID"), + new AutoPath("Tune Translational PID nack", "TuneTranslationPID back"), new AutoPath("Tune Rotational PID", "TuneRotationalPID"), new AutoPath("Stand Still", "PresetSourceSide1Score"), new AutoPath("Stand Still", "PresetMid1Score"), @@ -436,7 +437,10 @@ public static Command visionLaunch() { public static Command revFlyWheels() { return (LAUNCHER_ENABLED - ? new SetLaunchSpeedCommand(s.launcherSubsystem, REV_RPM) + ? Commands.either( + new SetLaunchSpeedCommand(s.launcherSubsystem, REV_RPM), + Commands.none(), + () -> s.launcherSubsystem.getLauncherSpeed() < REV_RPM) : Commands.none()) .withName("Auto - RevFlyWheelsCommand"); } @@ -465,6 +469,16 @@ public static Command setAngleSubwoofer() { .withName("Auto - SetPivotSubwooferCommand"); } + public static Command setAngleIndex() { + return (LAUNCHER_ENABLED + ? new SetAngleLaunchCommand( + s.launcherSubsystem, + LauncherSubsystem.SPEAKER_SHOOT_SPEED_RPM, + LauncherSubsystem.RETRACTED_ANGLE) + : Commands.none()) + .withName("Auto - SetPivotIndexCommand"); + } + public static Command feedUntilNoteLaunched() { return (INTAKE_ENABLED && LAUNCHER_ENABLED ? Commands.waitUntil(isReadyToLaunch()) diff --git a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java index 4c48ffa4..20e0f532 100644 --- a/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java +++ b/src/main/java/frc/team2412/robot/util/auto/ComplexAutoPaths.java @@ -173,6 +173,6 @@ public static final Command SubwooferLaunchCommand() { } public static BooleanSupplier checkForTargets() { - return (LIMELIGHT_ENABLED ? s.limelightSubsystem::hasTargets : () -> true); + return (LIMELIGHT_ENABLED ? s.limelightSubsystem::isNoteInFront : () -> true); } }