diff --git a/build.gradle b/build.gradle index aed9e2a8..f6b7e51a 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" } java { 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 e120e786..85711b0b 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 2a3eab67..091217b0 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 a864e03c..e6f8ae5a 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 53bae286..1467ebab 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 f26b1a59..8234b6f0 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/PneumaticsConstants.java b/src/main/java/frc/robot/constants/PneumaticsConstants.java deleted file mode 100644 index 90a4e3ed..00000000 --- a/src/main/java/frc/robot/constants/PneumaticsConstants.java +++ /dev/null @@ -1,10 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.wpilibj.PneumaticsModuleType; - -public final class PneumaticsConstants { - public static final int kPressureSensorPort = 1; - public static final PneumaticsModuleType kModuleType = PneumaticsModuleType.REVPH; - public static final int kMinPressure = 110; - public static final int kMaxPressure = 120; -} diff --git a/src/main/java/frc/robot/constants/ScoringElevatorConstants.java b/src/main/java/frc/robot/constants/ScoringElevatorConstants.java index c6fbe02b..9bc2a28a 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; diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index 41f51097..148184f5 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -5,11 +5,12 @@ public final class VisionConstants { public static final String kValuesTopic = "values"; public static final String kRecordingTopic = "recording"; public static final String kStreamCam0 = "shouldStream0"; - public static final String kEnabled0Topic = "on0"; - public static final String kEnabled1Topic = "on1"; + public static final String kEnabled0Topic = "enable0"; + public static final String kEnabled1Topic = "enable1"; - public static final double width = 640; - public static final double height = 480; + public static final double width = 1280; + public static final double height = 720; + public static final double fov = 100; public static final double driveP = 0.005; public static final double driveI = 0; diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index c904212e..8c932c66 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/src/main/java/frc/robot/subsystems/Vision.java @@ -21,24 +21,23 @@ public class Vision extends SubsystemBase { // Enum for different things vision can find public enum VisionTarget { - APR1(0), - APT2(1), - APT3(2), - APT4(3), - APT5(4), - APT6(5), - APT7(6), - APT8(7), - APT9(8), - APT10(9), - APT11(10), - APT12(11), - APT13(12), - APT14(13), - APT15(14), - APT16(15), - GAVIN(16), - NOTE(17); + NOTE(0), + APR1(1), + APT2(2), + APT3(3), + APT4(4), + APT5(5), + APT6(6), + APT7(7), + APT8(8), + APT9(9), + APT10(10), + APT11(11), + APT12(12), + APT13(13), + APT14(14), + APT15(15), + APT16(16); public final int id; @@ -57,7 +56,7 @@ public enum CameraStream { private boolean recording = false; private boolean enabled0 = false; private boolean enabled1 = false; - private List visionData; + private List visionData = new ArrayList(); // Network Tables Setup NetworkTableInstance inst = NetworkTableInstance.getDefault(); @@ -84,6 +83,10 @@ public static Vision getInstance() { } private Vision() { + streamCam0Publisher.set(true); + enabled0Publisher.set(false); + enabled1Publisher.set(false); + recordingPublisher.set(false); } @Override @@ -91,11 +94,10 @@ public void periodic() { String value = valuesSubscriber.get(); try { JSONArray unparsedData = (JSONArray) jsonParser.parse(value); - visionData = new ArrayList(); + visionData.clear(); for (Object data : unparsedData) { JSONObject jsonObject = (JSONObject) data; VisionData mappedData = new VisionData((String) jsonObject.get("x"), (String) jsonObject.get("y"), - (String) jsonObject.get("area"), (String) jsonObject.get("conf"), (String) jsonObject.get("id")); visionData.add(mappedData); } @@ -166,46 +168,15 @@ public double getY(VisionTarget target) { return -1; } - public double getArea(VisionTarget target) { - for (int i = 0; i < visionData.size(); i++) { - if (visionData.get(i).getTarget() == target) { - return visionData.get(i).getArea(); - } - } - return -1; - } - public double getAngle(VisionTarget target) { double x = getX(target); - double y = getY(target); - if (x < VisionConstants.width / 2) { - x = -getX(target); - } - if (y < VisionConstants.height / 2) { - y = -y; + + if (x == -1) { + return 0; } - return Math.toDegrees(Math.atan(x / y)); - } - public double getDistance(VisionTarget target) { - double y = getY(target); - double oldY = 0.0; + x = x - (VisionConstants.width / 2); - if (target == VisionTarget.NOTE) { - if (y > oldY) { - oldY = y; - return 0; - } else { - oldY = 0.0; - return -1; - } - - } else if (target.id >= VisionTarget.APR1.id && target.id <= VisionTarget.APT16.id) { - double area = getArea(target); - return area; - } else { - return 1; - } + return x / (VisionConstants.width / (VisionConstants.fov / 2)); } - } diff --git a/src/main/java/frc/robot/utils/VisionData.java b/src/main/java/frc/robot/utils/VisionData.java index 6cd06e8c..0d441abe 100644 --- a/src/main/java/frc/robot/utils/VisionData.java +++ b/src/main/java/frc/robot/utils/VisionData.java @@ -5,22 +5,18 @@ public class VisionData { private double x; private double y; - private double area; - private double confidence; private VisionTarget target; - public VisionData(String x, String y, String area, String confidence, String target) { + public VisionData(String x, String y, String target) { + System.out.println("x " + x + " y " + y); this.x = Double.parseDouble(x); this.y = Double.parseDouble(y); - this.area = Double.parseDouble(area); - this.confidence = Double.parseDouble(confidence); this.target = VisionTarget.values()[Integer.parseInt(target)]; } @Override public String toString() { - return "x: " + this.x + " y: " + this.y + " area: " + this.area + " conf: " + this.confidence + " tgt: " - + this.target; + return "x: " + this.x + " y: " + this.y + " area: " + " tgt: " + this.target; } public double getX() { @@ -34,8 +30,4 @@ public double getY() { public VisionTarget getTarget() { return target; } - - public double getArea() { - return area; - } } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index 787450f4..6dc648db 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.2.4", + "version": "2024.2.8", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.2.4" + "version": "2024.2.8" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.2.4", + "version": "2024.2.8", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json index 60eacf84..f85acd40 100644 --- a/vendordeps/REVLib.json +++ b/vendordeps/REVLib.json @@ -1,7 +1,7 @@ { "fileName": "REVLib.json", "name": "REVLib", - "version": "2024.2.3", + "version": "2024.2.4", "frcYear": "2024", "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", "mavenUrls": [ @@ -12,14 +12,14 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-java", - "version": "2024.2.3" + "version": "2024.2.4" } ], "jniDependencies": [ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "skipInvalidPlatforms": true, "isJar": false, "validPlatforms": [ @@ -37,7 +37,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-cpp", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLib", "headerClassifier": "headers", "sharedLibrary": false, @@ -55,7 +55,7 @@ { "groupId": "com.revrobotics.frc", "artifactId": "REVLib-driver", - "version": "2024.2.3", + "version": "2024.2.4", "libName": "REVLibDriver", "headerClassifier": "headers", "sharedLibrary": false,