From 564d94f2f1b37bf1e0bbb3b9876f5681c455317e Mon Sep 17 00:00:00 2001 From: Andrew Chen Date: Sat, 9 Mar 2024 19:45:53 -0500 Subject: [PATCH] Various adjustments during competition - adjust speaker and amp positions - speed up index - make autos drive further, make another auto to drive event further --- networktables.json | 112 ++++++++++++++++++ networktables.json.bck | 88 ++------------ simgui.json | 5 + .../deploy/pathplanner/paths/Leave Side.path | 52 ++++++++ src/main/java/frc/robot/RobotContainer.java | 4 +- src/main/java/frc/robot/commands/Autos.java | 12 +- .../frc/robot/commands/auto/SpeakerBack.java | 6 +- .../robot/commands/auto/SpeakerBackFar.java | 40 +++++++ .../frc/robot/subsystems/shooter/Shooter.java | 15 +-- .../subsystems/shooter/ShooterPosition.java | 4 +- 10 files changed, 239 insertions(+), 99 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/Leave Side.path create mode 100644 src/main/java/frc/robot/commands/auto/SpeakerBackFar.java diff --git a/networktables.json b/networktables.json index 2440d3b..1069985 100644 --- a/networktables.json +++ b/networktables.json @@ -1,4 +1,116 @@ [ + { + "name": "/Preferences/kSwerveTestTurn", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveTestDrive", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/0 Front LeftEnabled", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleDriveP", + "type": "double", + "value": 0.01, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleDriveI", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleDriveD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleDriveV", + "type": "double", + "value": 0.145, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleTurnP", + "type": "double", + "value": 0.3, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleTurnI", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleTurnD", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/kSwerveModuleTurnV", + "type": "double", + "value": 0.0, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/1 Front RightEnabled", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/2 Back LeftEnabled", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, + { + "name": "/Preferences/3 Back RightEnabled", + "type": "boolean", + "value": true, + "properties": { + "persistent": true + } + }, { "name": "/Preferences/testwrist", "type": "double", diff --git a/networktables.json.bck b/networktables.json.bck index 2b777c0..2440d3b 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -1,6 +1,6 @@ [ { - "name": "/Preferences/kSwerveModuleDriveI", + "name": "/Preferences/testwrist", "type": "double", "value": 0.0, "properties": { @@ -8,55 +8,23 @@ } }, { - "name": "/Preferences/0 Front LeftEnabled", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/kSwerveModuleTurnI", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/kSwerveModuleDriveV", - "type": "double", - "value": 0.145, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/kSwerveModuleDriveP", - "type": "double", - "value": 0.01, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/kSwerveModuleTurnP", + "name": "/Preferences/kShooterWristP", "type": "double", - "value": 0.3, + "value": 0.16, "properties": { "persistent": true } }, { - "name": "/Preferences/kSwerveModuleDriveD", + "name": "/Preferences/kShooterWristI", "type": "double", - "value": 0.0, + "value": 0.007, "properties": { "persistent": true } }, { - "name": "/Preferences/kSwerveTestTurn", + "name": "/Preferences/kShooterWristD", "type": "double", "value": 0.0, "properties": { @@ -64,49 +32,9 @@ } }, { - "name": "/Preferences/kSwerveTestDrive", + "name": "/Preferences/kShooterWristV", "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/kSwerveModuleTurnD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/kSwerveModuleTurnV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/1 Front RightEnabled", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/2 Back LeftEnabled", - "type": "boolean", - "value": true, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/3 Back RightEnabled", - "type": "boolean", - "value": true, + "value": 0.5, "properties": { "persistent": true } diff --git a/simgui.json b/simgui.json index 51ba4a6..932161f 100644 --- a/simgui.json +++ b/simgui.json @@ -18,6 +18,11 @@ "/SmartDashboard/Trajectory Field": "Field2d" }, "windows": { + "/Shuffleboard/Autos/Auto": { + "window": { + "visible": true + } + }, "/SmartDashboard/Auto Visualization": { "bottom": 1476, "height": 8.210550308227539, diff --git a/src/main/deploy/pathplanner/paths/Leave Side.path b/src/main/deploy/pathplanner/paths/Leave Side.path new file mode 100644 index 0000000..e954d6f --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Leave Side.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.9861162090367046, + "y": 4.3330095219390365 + }, + "prevControl": null, + "nextControl": { + "x": 2.2625973925268887, + "y": 3.536672636825895 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 4.311993788038652, + "y": 2.318745636064618 + }, + "prevControl": { + "x": 3.0940667872773764, + "y": 3.1033716846319783 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 144.46232220802563, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d7cb6e..6ee50aa 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,9 +78,9 @@ private void configureBindings() { altController.b().onTrue(shooter.runSetWristPosition(ShooterPosition.FLAT)); altController.rightStick().onTrue(shooter.runShooter(20)); altController.rightStick().onFalse(shooter.runShooter(0)); - altController.leftBumper().onTrue(shooter.runIndex(-10)); + altController.leftBumper().onTrue(shooter.runIndex(-12)); altController.leftBumper().onFalse(shooter.runIndex(0)); - altController.leftTrigger().onTrue(shooter.runIndex(10)); + altController.leftTrigger().onTrue(shooter.runIndex(12)); altController.leftTrigger().onFalse(shooter.runIndex(0)); } diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 2f5e975..b7d6631 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/src/main/java/frc/robot/commands/Autos.java @@ -22,6 +22,7 @@ import frc.robot.commands.auto.NothingAuto; import frc.robot.commands.auto.ShootOnly; import frc.robot.commands.auto.SpeakerBack; +import frc.robot.commands.auto.SpeakerBackFar; import frc.robot.subsystems.shooter.Shooter; import frc.robot.subsystems.shooter.ShooterPosition; import frc.robot.subsystems.swerve.SwerveDrive; @@ -31,8 +32,9 @@ public class Autos { public static final Field2d autoField = new Field2d(); public static final void initAutos(SwerveDrive swerve, Shooter shooter) { - autoChooser.addOption("Speaker Back", new SpeakerBack(swerve, shooter)); - autoChooser.setDefaultOption("Shoot Only", new ShootOnly(swerve, shooter)); + autoChooser.setDefaultOption("Speaker Back", new SpeakerBack(swerve, shooter)); + autoChooser.addOption("Speaker Back Far", new SpeakerBackFar(swerve, shooter)); + autoChooser.addOption("Shoot Only", new ShootOnly(swerve, shooter)); // autoChooser.addOption("Shoot Only", new SpeakerOnly(shooter)); autoChooser.addOption("Do Nothing", new NothingAuto(swerve)); // autoChooser.addOption("Mid Double Auto", new MidDoubleAuto(swerveDrive)); @@ -73,10 +75,10 @@ public static Trajectory getPathPlannerTrajectory(String filename, Rotation2d st public static Command runSpeakerShot(Shooter shooter) { return Commands.sequence( shooter.runSetWristPosition(ShooterPosition.SPEAKER), - shooter.runShooter(20), - new WaitCommand(1.2), - shooter.runIndex(-10), + shooter.runShooter(23), new WaitCommand(1.5), + shooter.runIndex(-12), + new WaitCommand(1.7), shooter.runIndex(0), shooter.runShooter(0), shooter.runSetWristPosition(ShooterPosition.FLAT) diff --git a/src/main/java/frc/robot/commands/auto/SpeakerBack.java b/src/main/java/frc/robot/commands/auto/SpeakerBack.java index 2b59fb7..a3b5df1 100644 --- a/src/main/java/frc/robot/commands/auto/SpeakerBack.java +++ b/src/main/java/frc/robot/commands/auto/SpeakerBack.java @@ -18,10 +18,10 @@ public SpeakerBack(SwerveDrive swerve, Shooter shooter) { Commands.runOnce(() -> { swerve.rawDriveInputs(0, 2, 0, false, false); }, swerve), - new WaitCommand(0.7), + new WaitCommand(1.6), Commands.run(() -> { swerve.rawDriveInputs(0, 0, -1.5, false, false); - System.out.println(Math.abs(swerve.getRotation2d().getDegrees() % 360)); + // System.out.println(Math.abs(swerve.getRotation2d().getDegrees() % 360)); }, swerve) .until(() -> (Math.abs(swerve.getRotation2d().getDegrees() % 360) < 5) @@ -35,6 +35,6 @@ public SpeakerBack(SwerveDrive swerve, Shooter shooter) { @Override public Pose2d getStartPose() { - return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(180)).getInitialPose(); + return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(0)).getInitialPose(); } } diff --git a/src/main/java/frc/robot/commands/auto/SpeakerBackFar.java b/src/main/java/frc/robot/commands/auto/SpeakerBackFar.java new file mode 100644 index 0000000..ad3cf86 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/SpeakerBackFar.java @@ -0,0 +1,40 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.commands.AutoCommand; +import frc.robot.commands.Autos; +import frc.robot.subsystems.shooter.Shooter; +import frc.robot.subsystems.swerve.SwerveDrive; + +public class SpeakerBackFar extends AutoCommand { + public SpeakerBackFar(SwerveDrive swerve, Shooter shooter) { + addCommands( + Autos.runSpeakerShot(shooter), + swerve.runSetGyroAngle(Rotation2d.fromDegrees(180)), + new WaitCommand(1), + Commands.runOnce(() -> { + swerve.rawDriveInputs(0, 2, 0, false, false); + }, swerve), + new WaitCommand(2.4), + Commands.run(() -> { + swerve.rawDriveInputs(0, 0, -1.5, false, false); + // System.out.println(Math.abs(swerve.getRotation2d().getDegrees() % 360)); + }, swerve) + .until(() -> + (Math.abs(swerve.getRotation2d().getDegrees() % 360) < 5) + ) + .andThen(() -> { + swerve.rawDriveInputs(0, 0, 0, false, false); + }, swerve) + ); + addRequirements(swerve, shooter); + } + + @Override + public Pose2d getStartPose() { + return Autos.getPathPlannerTrajectory("Leave Side", Rotation2d.fromDegrees(0)).getInitialPose(); + } +} diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java index f4dfdb7..1b307c1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java @@ -109,7 +109,7 @@ public Shooter() { ShooterConstants.kWristPIDConstants.kV(), // actually kG 0 ); - setWristPosition(ShooterPosition.SPEAKER); + setWristPosition(ShooterPosition.FLAT); toSetpoint = false; toTest = false; speedFactor = 0; @@ -207,14 +207,15 @@ public void periodic() { wristPID.setD(ShooterConstants.kWristPIDConstants.kD()); wristFF.setG(ShooterConstants.kWristPIDConstants.kV()); - if(!lowerLimit.get()) { - wristMotor.setVoltage(MathUtil.clamp(wristPIDOutput + wristFFOutput, 0, 6)); - } else { - wristMotor.setVoltage(wristPIDOutput + wristFFOutput); - } + // with limit switch + // if(!lowerLimit.get()) { + // wristMotor.setVoltage(MathUtil.clamp(wristPIDOutput + wristFFOutput, 0, 6)); + // } else { + // wristMotor.setVoltage(wristPIDOutput + wristFFOutput); + // } // no limit switch - // wristMotor.setVoltage(wristPIDOutput + wristFFOutput); + wristMotor.setVoltage(wristPIDOutput + wristFFOutput); // use test preference as input // if(toTest) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterPosition.java b/src/main/java/frc/robot/subsystems/shooter/ShooterPosition.java index b23649b..68e7a7a 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterPosition.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterPosition.java @@ -2,8 +2,8 @@ public enum ShooterPosition { FLAT("FLAT", 19.5), - AMP("AMP", 90), // prev 108.7 - SPEAKER("SPEAKER", 64), + AMP("AMP", 95), // prev 108.7 prev prev 90 + SPEAKER("SPEAKER", 66), // 62.9 -> 64 -> 66 SOURCE("SOURCE", 49); // amp or speaker shooting method adjusted if needed could work for trap