diff --git a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path index e120e78..85711b0 100644 --- a/src/main/deploy/pathplanner/paths/From amp to midline acquire.path +++ b/src/main/deploy/pathplanner/paths/From amp to midline acquire.path @@ -48,12 +48,12 @@ }, { "anchor": { - "x": 8.54, - "y": 7.34 + "x": 8.515833783804318, + "y": 7.4608705342766815 }, "prevControl": { - "x": 7.904677120863775, - "y": 7.5503162432548745 + "x": 7.825878671700363, + "y": 7.3556231442947215 }, "nextControl": null, "isLocked": false, @@ -82,7 +82,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0.0, + "rotation": 8.62, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/From midline note to amp.path b/src/main/deploy/pathplanner/paths/From midline note to amp.path index 2a3eab6..091217b 100644 --- a/src/main/deploy/pathplanner/paths/From midline note to amp.path +++ b/src/main/deploy/pathplanner/paths/From midline note to amp.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 8.540000000000001, - "y": 7.343809471653955 + "x": 8.515833783804318, + "y": 7.4608705342766815 }, "prevControl": null, "nextControl": { - "x": 8.683718987153075, - "y": 7.280092322567236 + "x": 8.659552770957392, + "y": 7.397153385189962 }, "isLocked": false, "linkedName": "MidAcquireTopNote" @@ -48,11 +48,11 @@ }, { "anchor": { - "x": 1.95, + "x": 2.0, "y": 8.1 }, "prevControl": { - "x": 2.514708547080911, + "x": 2.5647085470809112, "y": 8.05967115979816 }, "nextControl": null, @@ -83,7 +83,7 @@ "reversed": false, "folder": null, "previewStartingState": { - "rotation": 0.0, + "rotation": 8.62, "velocity": 0 }, "useDefaultConstraints": true diff --git a/src/main/java/frc/robot/commands/ShootPasserCommand.java b/src/main/java/frc/robot/commands/ShootPasserCommand.java index a864e03..e6f8ae5 100644 --- a/src/main/java/frc/robot/commands/ShootPasserCommand.java +++ b/src/main/java/frc/robot/commands/ShootPasserCommand.java @@ -25,15 +25,13 @@ public ShootPasserCommand() { @Override public void initialize() { timer.start(); + passer.shoot(); } @Override public void execute() { - scoring.feedForPasser(); - passer.shoot(); - if (!storage.noteExitingStorage()) { - storage.stopStorage(); - } else { + if (timer.get() > 0.25) { + scoring.feedForPasser(); storage.runStorage(); } } diff --git a/src/main/java/frc/robot/constants/DrawbridgeConstants.java b/src/main/java/frc/robot/constants/DrawbridgeConstants.java index 53bae28..1467eba 100644 --- a/src/main/java/frc/robot/constants/DrawbridgeConstants.java +++ b/src/main/java/frc/robot/constants/DrawbridgeConstants.java @@ -6,7 +6,7 @@ public class DrawbridgeConstants { public static final int rightServoPort = 3; public static final double minLeftDrawbridgeExtension = 0.0; - public static final double maxLeftDrawbridgeExtension = 0.3; - public static final double minRightDrawbridgeExtension = 0.08; - public static final double maxRightDrawbridgeExtension = 0.45; + public static final double maxLeftDrawbridgeExtension = 0.65; + public static final double minRightDrawbridgeExtension = 0.0; + public static final double maxRightDrawbridgeExtension = 0.6; } diff --git a/src/main/java/frc/robot/constants/NeoMotorConstants.java b/src/main/java/frc/robot/constants/NeoMotorConstants.java index f26b1a5..8234b6f 100644 --- a/src/main/java/frc/robot/constants/NeoMotorConstants.java +++ b/src/main/java/frc/robot/constants/NeoMotorConstants.java @@ -5,7 +5,7 @@ public final class NeoMotorConstants { public static final double kVortexFreeSpeedRpm = 6784; public static final double kNeo550FreeSpeedRpm = 11000; public static final int kMaxNeoCurrent = 50; // amps - public static final int kMaxVortexCurrent = 50; // amps + public static final int kMaxVortexCurrent = 70; // amps public static final int kMaxNeo550Current = 20; // amps public static final double kMinPower = 0.05; public static final double kMaxPower = 1.0; diff --git a/src/main/java/frc/robot/constants/ScoringElevatorConstants.java b/src/main/java/frc/robot/constants/ScoringElevatorConstants.java index c6fbe02..9bc2a28 100644 --- a/src/main/java/frc/robot/constants/ScoringElevatorConstants.java +++ b/src/main/java/frc/robot/constants/ScoringElevatorConstants.java @@ -9,8 +9,8 @@ public class ScoringElevatorConstants { public static final double elevatorEncoderConversionFactor = 1 / (maxElevatorEncoderCounts / maxElevatorInches); public static final double groundSetpoint = 0; - public static final double ampSetpoint = 4.0; - public static final double passerSetpoint = 6.0; + public static final double ampSetpoint = 4.5; + public static final double passerSetpoint = 6.75; public static final double trapSetpoint = 15.0; public static final double maxDistance = 0;