From 2dd0ab12130e56091c713f15d034890887f57b92 Mon Sep 17 00:00:00 2001 From: D0LPH1NB01 Date: Fri, 14 Mar 2025 22:28:16 -0700 Subject: [PATCH 01/14] Added LED stuff --- build.gradle | 3 - .../java/org/team5924/frc2025/Constants.java | 4 + src/main/java/org/team5924/frc2025/Robot.java | 6 +- .../frc2025/subsystems/Lights/Lights.java | 172 ++++++++++++++ .../frc2025/subsystems/Lights/LightsIO.java | 21 ++ .../frc2025/util/PolynomialRegression.java | 221 ------------------ vendordeps/Phoenix5-5.35.1.json | 171 ++++++++++++++ 7 files changed, 373 insertions(+), 225 deletions(-) create mode 100644 src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java create mode 100644 src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java delete mode 100644 src/main/java/org/team5924/frc2025/util/PolynomialRegression.java create mode 100644 vendordeps/Phoenix5-5.35.1.json diff --git a/build.gradle b/build.gradle index 2f2a0b7..9148a58 100755 --- a/build.gradle +++ b/build.gradle @@ -76,7 +76,6 @@ dependencies { annotationProcessor wpi.java.deps.wpilibAnnotations() implementation wpi.java.deps.wpilib() implementation wpi.java.vendor.java() - implementation files('libs/Jama-1.0.3.jar') roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio) roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio) @@ -95,8 +94,6 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "gov.nist.math:jama:1.0.3" - def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" } diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index e127175..26ac566 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -116,6 +116,10 @@ public static enum Mode { public static final Distance SPROCKET_RADIUS = Inches.of(.6405); public static final double ELEVATOR_CANCODER_OFFSET = 0.00; // TODO: Check and change if needed + /* # Lights # */ + + public static final int CANDLE_ID = 50; // TODO: Verify this!! + /* # Vision # */ public static String APRIL_TAG_LIMELIGHT_NAME_FRONT = "limelight-front"; public static String APRIL_TAG_LIMELIGHT_NAME_BACK = "limelight-back"; diff --git a/src/main/java/org/team5924/frc2025/Robot.java b/src/main/java/org/team5924/frc2025/Robot.java index b057fcc..366d5b1 100644 --- a/src/main/java/org/team5924/frc2025/Robot.java +++ b/src/main/java/org/team5924/frc2025/Robot.java @@ -33,6 +33,8 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team5924.frc2025.generated.TunerConstants; +import org.team5924.frc2025.subsystems.Lights.Lights; +import org.team5924.frc2025.subsystems.Lights.Lights.LEDSegment; import org.team5924.frc2025.util.Elastic; /** @@ -142,7 +144,9 @@ public void disabledInit() {} /** This function is called periodically when disabled. */ @Override - public void disabledPeriodic() {} + public void disabledPeriodic() { + LEDSegment.MainStrip.setFadeAnimation(Lights.green, 0.5); + } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ @Override diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java new file mode 100644 index 0000000..882578e --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java @@ -0,0 +1,172 @@ +/* + * Lights.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.Lights; + +import com.ctre.phoenix.led.Animation; +import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdle.LEDStripType; +import com.ctre.phoenix.led.CANdle.VBatOutputMode; +import com.ctre.phoenix.led.CANdleConfiguration; +import com.ctre.phoenix.led.ColorFlowAnimation; +import com.ctre.phoenix.led.ColorFlowAnimation.Direction; +import com.ctre.phoenix.led.LarsonAnimation; +import com.ctre.phoenix.led.LarsonAnimation.BounceMode; +import com.ctre.phoenix.led.RainbowAnimation; +import com.ctre.phoenix.led.SingleFadeAnimation; +import com.ctre.phoenix.led.StrobeAnimation; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import org.team5924.frc2025.Constants; + +public class Lights extends SubsystemBase { + private static final CANdle candle = new CANdle(Constants.CANDLE_ID); + + /* Creating a Color class to read in some colors */ + public static class Color { + public int red; + public int green; + public int blue; + + public Color(int red, int green, int blue) { + this.red = red; + this.green = green; + this.blue = blue; + } + } + + /* Standard Colors */ + public static final Color white = new Color(255, 230, 220); + public static final Color red = new Color(255, 0, 0); + public static final Color orange = new Color(255, 165, 0); + public static final Color yellow = new Color(255, 255, 0); + public static final Color green = new Color(0, 255, 0); + public static final Color blue = new Color(0, 0, 255); + public static final Color purple = new Color(148, 0, 211); + public static final Color black = new Color(0, 0, 0); + + /* Team-specific Colors */ + public static final Color international_orange = new Color(255, 79, 0); + + public Lights() { + CANdleConfiguration candleConfiguration = new CANdleConfiguration(); + candleConfiguration.statusLedOffWhenActive = true; + candleConfiguration.disableWhenLOS = false; + candleConfiguration.stripType = LEDStripType.RGB; + candleConfiguration.vBatOutputMode = VBatOutputMode.Modulated; + candle.configAllSettings(candleConfiguration, 100); + + setDefaultCommand(defaultCommand()); + } + + public void setBrightness(double percent) { + candle.configBrightnessScalar(percent, 100); + } + + public Command defaultCommand() { + return runOnce( + () -> { + LEDSegment.MainStrip.setColor(international_orange); + }); + } + + public Command clearSegmentCommand(LEDSegment segment) { + return runOnce( + () -> { + segment.clearAnimation(); + segment.disableLEDs(); + }); + } + + public static enum LEDSegment { + MainStrip(0, 300, 2); + + public final int startIndex; + public final int segmentSize; + public final int animationSlot; + + private LEDSegment(int startIndex, int segmentSize, int animationSlot) { + this.startIndex = startIndex; + this.segmentSize = segmentSize; + this.animationSlot = animationSlot; + } + + public void setColor(Color color) { + clearAnimation(); + candle.setLEDs(color.red, color.green, color.blue, 0, startIndex, segmentSize); + } + + private void setAnimation(Animation animation) { + candle.animate(animation, animationSlot); + } + + public void fullClear() { + clearAnimation(); + disableLEDs(); + } + + public void clearAnimation() { + candle.clearAnimation(animationSlot); + } + + public void disableLEDs() { + setColor(black); + } + + public void setFlowAnimation(Color color, double speed) { + setAnimation( + new ColorFlowAnimation( + color.red, + color.green, + color.blue, + 0, + speed, + segmentSize, + Direction.Forward, + startIndex)); + } + + public void setFadeAnimation(Color color, double speed) { + setAnimation( + new SingleFadeAnimation( + color.red, color.green, color.blue, 0, speed, segmentSize, startIndex)); + } + + public void setBandAnimation(Color color, double speed) { + setAnimation( + new LarsonAnimation( + color.red, + color.green, + color.blue, + 0, + speed, + segmentSize, + BounceMode.Front, + 3, + startIndex)); + } + + public void setStrobeAnimation(Color color, double speed) { + setAnimation( + new StrobeAnimation( + color.red, color.green, color.blue, 0, speed, segmentSize, startIndex)); + } + + public void setRainbowAnimation(double speed) { + setAnimation(new RainbowAnimation(1, speed, segmentSize, false, startIndex)); + } + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java new file mode 100644 index 0000000..528ed2f --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java @@ -0,0 +1,21 @@ +/* + * LightsIO.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.Lights; + +public class LightsIO { + /* Pfft I have no clue what goes here :D */ +} diff --git a/src/main/java/org/team5924/frc2025/util/PolynomialRegression.java b/src/main/java/org/team5924/frc2025/util/PolynomialRegression.java deleted file mode 100644 index 939de6c..0000000 --- a/src/main/java/org/team5924/frc2025/util/PolynomialRegression.java +++ /dev/null @@ -1,221 +0,0 @@ -/* - * PolynomialRegression.java - */ - -/* - * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. - * - * This file, and the associated project, are offered under the GNU General - * Public License v3.0. A copy of this license can be found in LICENSE.md - * at the root of this project. - * - * If this file has been separated from the original project, you should have - * received a copy of the GNU General Public License along with it. - * If you did not, see . - */ - -package org.team5924.frc2025.util; - -import Jama.Matrix; -import Jama.QRDecomposition; - -// NOTE: This file is available at -// http://algs4.cs.princeton.edu/14analysis/PolynomialRegression.java.html - -/** - * The {@code PolynomialRegression} class performs a polynomial regression on an set of N - * data points (yi, xi). That is, it fits a polynomial - * y = β0 + β1 x + β2 - * x2 + ... + βd xd (where - * y is the response variable, x is the predictor variable, and the - * βi are the regression coefficients) that minimizes the sum of squared - * residuals of the multiple regression model. It also computes associated the coefficient of - * determination R2. - * - *

This implementation performs a QR-decomposition of the underlying Vandermonde matrix, so it is - * neither the fastest nor the most numerically stable way to perform the polynomial regression. - * - * @author Robert Sedgewick - * @author Kevin Wayne - */ -public class PolynomialRegression implements Comparable { - private final String variableName; // name of the predictor variable - private int degree; // degree of the polynomial regression - private Matrix beta; // the polynomial regression coefficients - private double sse; // sum of squares due to error - private double sst; // total sum of squares - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. Uses n as the name - * of the predictor variable. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree) { - this(x, y, degree, "n"); - } - - /** - * Performs a polynomial reggression on the data points {@code (y[i], x[i])}. - * - * @param x the values of the predictor variable - * @param y the corresponding values of the response variable - * @param degree the degree of the polynomial to fit - * @param variableName the name of the predictor variable - * @throws IllegalArgumentException if the lengths of the two arrays are not equal - */ - public PolynomialRegression(double[] x, double[] y, int degree, String variableName) { - this.degree = degree; - this.variableName = variableName; - - int n = x.length; - QRDecomposition qr = null; - Matrix matrixX = null; - - // in case Vandermonde matrix does not have full rank, reduce degree until it - // does - while (true) { - - // build Vandermonde matrix - double[][] vandermonde = new double[n][this.degree + 1]; - for (int i = 0; i < n; i++) { - for (int j = 0; j <= this.degree; j++) { - vandermonde[i][j] = Math.pow(x[i], j); - } - } - matrixX = new Matrix(vandermonde); - - // find least squares solution - qr = new QRDecomposition(matrixX); - if (qr.isFullRank()) break; - - // decrease degree and try again - this.degree--; - } - - // create matrix from vector - Matrix matrixY = new Matrix(y, n); - - // linear regression coefficients - beta = qr.solve(matrixY); - - // mean of y[] values - double sum = 0.0; - for (int i = 0; i < n; i++) sum += y[i]; - double mean = sum / n; - - // total variation to be accounted for - for (int i = 0; i < n; i++) { - double dev = y[i] - mean; - sst += dev * dev; - } - - // variation not accounted for - Matrix residuals = matrixX.times(beta).minus(matrixY); - sse = residuals.norm2() * residuals.norm2(); - } - - /** - * Returns the {@code j}th regression coefficient. - * - * @param j the index - * @return the {@code j}th regression coefficient - */ - public double beta(int j) { - // to make -0.0 print as 0.0 - if (Math.abs(beta.get(j, 0)) < 1E-4) return 0.0; - return beta.get(j, 0); - } - - /** - * Returns the degree of the polynomial to fit. - * - * @return the degree of the polynomial to fit - */ - public int degree() { - return degree; - } - - /** - * Returns the coefficient of determination R2. - * - * @return the coefficient of determination R2, which is a real number between - * 0 and 1 - */ - public double R2() { - if (sst == 0.0) return 1.0; // constant function - return 1.0 - sse / sst; - } - - /** - * Returns the expected response {@code y} given the value of the predictor variable {@code x}. - * - * @param x the value of the predictor variable - * @return the expected response {@code y} given the value of the predictor variable {@code x} - */ - public double predict(double x) { - // horner's method - double y = 0.0; - for (int j = degree; j >= 0; j--) y = beta(j) + (x * y); - return y; - } - - /** - * Returns a string representation of the polynomial regression model. - * - * @return a string representation of the polynomial regression model, including the best-fit - * polynomial and the coefficient of determination R2 - */ - public String toString() { - StringBuilder s = new StringBuilder(); - int j = degree; - - // ignoring leading zero coefficients - while (j >= 0 && Math.abs(beta(j)) < 1E-5) j--; - - // create remaining terms - while (j >= 0) { - if (j == 0) s.append(String.format("%.10f ", beta(j))); - else if (j == 1) s.append(String.format("%.10f %s + ", beta(j), variableName)); - else s.append(String.format("%.10f %s^%d + ", beta(j), variableName, j)); - j--; - } - s = s.append(" (R^2 = " + String.format("%.3f", R2()) + ")"); - - // replace "+ -2n" with "- 2n" - return s.toString().replace("+ -", "- "); - } - - /** Compare lexicographically. */ - public int compareTo(PolynomialRegression that) { - double EPSILON = 1E-5; - int maxDegree = Math.max(this.degree(), that.degree()); - for (int j = maxDegree; j >= 0; j--) { - double term1 = 0.0; - double term2 = 0.0; - if (this.degree() >= j) term1 = this.beta(j); - if (that.degree() >= j) term2 = that.beta(j); - if (Math.abs(term1) < EPSILON) term1 = 0.0; - if (Math.abs(term2) < EPSILON) term2 = 0.0; - if (term1 < term2) return -1; - else if (term1 > term2) return +1; - } - return 0; - } - - /** - * Unit tests the {@code PolynomialRegression} data type. - * - * @param args the command-line arguments - */ - public static void main(String[] args) { - double[] x = {10, 20, 40, 80, 160, 200}; - double[] y = {100, 350, 1500, 6700, 20160, 40000}; - PolynomialRegression regression = new PolynomialRegression(x, y, 3); - - System.out.println(regression); - } -} diff --git a/vendordeps/Phoenix5-5.35.1.json b/vendordeps/Phoenix5-5.35.1.json new file mode 100644 index 0000000..2190f40 --- /dev/null +++ b/vendordeps/Phoenix5-5.35.1.json @@ -0,0 +1,171 @@ +{ + "fileName": "Phoenix5-5.35.1.json", + "name": "CTRE-Phoenix (v5)", + "version": "5.35.1", + "frcYear": "2025", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "https://maven.ctr-electronics.com/release/" + ], + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix/Phoenix5-frc2025-latest.json", + "requires": [ + { + "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", + "errorMessage": "Phoenix 5 requires low-level libraries from Phoenix 6. Please add the Phoenix 6 vendordep before adding Phoenix 5.", + "offlineFileName": "Phoenix6-frc2025-latest.json", + "onlineUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2025-latest.json" + } + ], + "conflictsWith": [ + { + "uuid": "e7900d8d-826f-4dca-a1ff-182f658e98af", + "errorMessage": "Users must use the Phoenix 5 replay vendordep when using the Phoenix 6 replay vendordep.", + "offlineFileName": "Phoenix6-replay-frc2025-latest.json" + }, + { + "uuid": "fbc886a4-2cec-40c0-9835-71086a8cc3df", + "errorMessage": "Users cannot have both the replay and regular Phoenix 5 vendordeps in their robot program.", + "offlineFileName": "Phoenix5-replay-frc2025-latest.json" + } + ], + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.35.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.35.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.35.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "linuxathena" + ], + "simMode": "hwsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "wpiapi-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_Phoenix_WPISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "api-cpp-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixSim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + }, + { + "groupId": "com.ctre.phoenix.sim", + "artifactId": "cci-sim", + "version": "5.35.1", + "libName": "CTRE_PhoenixCCISim", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxx86-64", + "linuxarm64", + "osxuniversal" + ], + "simMode": "swsim" + } + ] +} From d1b742b1fbbfd926678651b7cb3af19c6458c563 Mon Sep 17 00:00:00 2001 From: marisa07 Date: Sat, 15 Mar 2025 16:36:58 -0700 Subject: [PATCH 02/14] control lights via LEDStatus command --- src/main/java/org/team5924/frc2025/Robot.java | 4 +- .../org/team5924/frc2025/RobotContainer.java | 7 +++ .../frc2025/commands/lights/LEDStatus.java | 56 +++++++++++++++++++ .../frc2025/subsystems/Lights/Lights.java | 2 +- 4 files changed, 66 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/team5924/frc2025/commands/lights/LEDStatus.java diff --git a/src/main/java/org/team5924/frc2025/Robot.java b/src/main/java/org/team5924/frc2025/Robot.java index 366d5b1..e161f78 100644 --- a/src/main/java/org/team5924/frc2025/Robot.java +++ b/src/main/java/org/team5924/frc2025/Robot.java @@ -33,8 +33,8 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team5924.frc2025.generated.TunerConstants; -import org.team5924.frc2025.subsystems.Lights.Lights; -import org.team5924.frc2025.subsystems.Lights.Lights.LEDSegment; +import org.team5924.frc2025.subsystems.lights.Lights; +import org.team5924.frc2025.subsystems.lights.Lights.LEDSegment; import org.team5924.frc2025.util.Elastic; /** diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 5a2a332..32f07c5 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -52,6 +52,7 @@ import org.team5924.frc2025.subsystems.elevator.Elevator; import org.team5924.frc2025.subsystems.elevator.ElevatorIO; import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFX; +import org.team5924.frc2025.subsystems.lights.Lights; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOKrakenFOC; @@ -73,6 +74,7 @@ public class RobotContainer { private final CoralInAndOut coralInAndOut; private final Elevator elevator; private final Vision vision; + private final Lights lights; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -98,6 +100,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIOKrakenFOC()); elevator = new Elevator(new ElevatorIOTalonFX() {}); vision = new Vision(new VisionIOLimelight()); + lights = new Lights(); break; case SIM: @@ -113,6 +116,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIOSim()); elevator = new Elevator(new ElevatorIO() {}); vision = new Vision(new VisionIO() {}); + lights = new Lights(); break; default: @@ -128,6 +132,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIO() {}); elevator = new Elevator(new ElevatorIO() {}); vision = new Vision(new VisionIO() {}); + lights = new Lights(); break; } @@ -294,6 +299,8 @@ private void configureButtonBindings() { .pov(180) .or(driveController.pov(0)) .onFalse(Commands.runOnce(() -> climber.handleNoInputState())); + + lights.defaultCommand(); } /** diff --git a/src/main/java/org/team5924/frc2025/commands/lights/LEDStatus.java b/src/main/java/org/team5924/frc2025/commands/lights/LEDStatus.java new file mode 100644 index 0000000..aac7d62 --- /dev/null +++ b/src/main/java/org/team5924/frc2025/commands/lights/LEDStatus.java @@ -0,0 +1,56 @@ +/* + * LEDStatus.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.commands.lights; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team5924.frc2025.RobotState; +import org.team5924.frc2025.subsystems.lights.Lights; + +public class LEDStatus extends Command { + /** Creates a new LEDstatus. */ + private Lights leds; + + double limeErrorTolerance = 1.5; // in degrees + + public LEDStatus(Lights leds) { + this.leds = leds; + // addRequirements(leds); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + switch (RobotState.getInstance().getElevatorState()) { + case L3 -> Lights.LEDSegment.MainStrip.setColor(Lights.blue); + default -> Lights.LEDSegment.MainStrip.disableLEDs(); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java index 882578e..165fb3a 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.Lights; +package org.team5924.frc2025.subsystems.lights; import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; From 5a37e7f870dd5b36eff213c740a4134fa8de3bc1 Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Sat, 15 Mar 2025 17:43:26 -0700 Subject: [PATCH 03/14] wip fix wilson's code --- src/main/java/org/team5924/frc2025/Robot.java | 4 +- .../frc2025/subsystems/Lights/Lights.java | 46 +++++++++---------- .../frc2025/subsystems/Lights/LightsIO.java | 7 ++- .../java/org/team5924/frc2025/util/Color.java | 43 +++++++++++++++++ 4 files changed, 73 insertions(+), 27 deletions(-) create mode 100644 src/main/java/org/team5924/frc2025/util/Color.java diff --git a/src/main/java/org/team5924/frc2025/Robot.java b/src/main/java/org/team5924/frc2025/Robot.java index 366d5b1..e161f78 100644 --- a/src/main/java/org/team5924/frc2025/Robot.java +++ b/src/main/java/org/team5924/frc2025/Robot.java @@ -33,8 +33,8 @@ import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; import org.team5924.frc2025.generated.TunerConstants; -import org.team5924.frc2025.subsystems.Lights.Lights; -import org.team5924.frc2025.subsystems.Lights.Lights.LEDSegment; +import org.team5924.frc2025.subsystems.lights.Lights; +import org.team5924.frc2025.subsystems.lights.Lights.LEDSegment; import org.team5924.frc2025.util.Elastic; /** diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java index 882578e..c977883 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.Lights; +package org.team5924.frc2025.subsystems.lights; import com.ctre.phoenix.led.Animation; import com.ctre.phoenix.led.CANdle; @@ -31,23 +31,11 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team5924.frc2025.Constants; +import org.team5924.frc2025.util.Color; public class Lights extends SubsystemBase { private static final CANdle candle = new CANdle(Constants.CANDLE_ID); - /* Creating a Color class to read in some colors */ - public static class Color { - public int red; - public int green; - public int blue; - - public Color(int red, int green, int blue) { - this.red = red; - this.green = green; - this.blue = blue; - } - } - /* Standard Colors */ public static final Color white = new Color(255, 230, 220); public static final Color red = new Color(255, 0, 0); @@ -106,7 +94,7 @@ private LEDSegment(int startIndex, int segmentSize, int animationSlot) { public void setColor(Color color) { clearAnimation(); - candle.setLEDs(color.red, color.green, color.blue, 0, startIndex, segmentSize); + candle.setLEDs(color.getRed(), color.getGreen(), color.getBlue(), 0, startIndex, segmentSize); } private void setAnimation(Animation animation) { @@ -129,9 +117,9 @@ public void disableLEDs() { public void setFlowAnimation(Color color, double speed) { setAnimation( new ColorFlowAnimation( - color.red, - color.green, - color.blue, + color.getRed(), + color.getGreen(), + color.getBlue(), 0, speed, segmentSize, @@ -142,15 +130,21 @@ public void setFlowAnimation(Color color, double speed) { public void setFadeAnimation(Color color, double speed) { setAnimation( new SingleFadeAnimation( - color.red, color.green, color.blue, 0, speed, segmentSize, startIndex)); + color.getRed(), + color.getGreen(), + color.getBlue(), + 0, + speed, + segmentSize, + startIndex)); } public void setBandAnimation(Color color, double speed) { setAnimation( new LarsonAnimation( - color.red, - color.green, - color.blue, + color.getRed(), + color.getGreen(), + color.getBlue(), 0, speed, segmentSize, @@ -162,7 +156,13 @@ public void setBandAnimation(Color color, double speed) { public void setStrobeAnimation(Color color, double speed) { setAnimation( new StrobeAnimation( - color.red, color.green, color.blue, 0, speed, segmentSize, startIndex)); + color.getRed(), + color.getGreen(), + color.getBlue(), + 0, + speed, + segmentSize, + startIndex)); } public void setRainbowAnimation(double speed) { diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java index 528ed2f..cdde6c6 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/LightsIO.java @@ -14,8 +14,11 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.Lights; +package org.team5924.frc2025.subsystems.lights; -public class LightsIO { +public interface LightsIO { /* Pfft I have no clue what goes here :D */ + public default void setBrightness() {} + + public default void setColor() {} } diff --git a/src/main/java/org/team5924/frc2025/util/Color.java b/src/main/java/org/team5924/frc2025/util/Color.java new file mode 100644 index 0000000..5b7bc49 --- /dev/null +++ b/src/main/java/org/team5924/frc2025/util/Color.java @@ -0,0 +1,43 @@ +/* + * Color.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.util; + +/** Add your docs here. */ +public class Color { + + private int red; + private int blue; + private int green; + + public Color(int red, int blue, int green) { + this.red = red; + this.green = green; + this.blue = blue; + } + + public int getRed() { + return red; + } + + public int getGreen() { + return green; + } + + public int getBlue() { + return blue; + } +} From 336843d25758f7e24c2954cd8b68efdc54cd55ef Mon Sep 17 00:00:00 2001 From: D0LPH1NB01 Date: Mon, 17 Mar 2025 12:56:09 -0700 Subject: [PATCH 04/14] Everything to do with color choice has been fixed --- src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java index 165fb3a..b621767 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/Lights.java @@ -60,6 +60,7 @@ public Color(int red, int green, int blue) { /* Team-specific Colors */ public static final Color international_orange = new Color(255, 79, 0); + public static final Color primary_red = new Color(192, 54, 44); public Lights() { CANdleConfiguration candleConfiguration = new CANdleConfiguration(); From fec6d08029a53590a6f66a70de8c163ab2fe4373 Mon Sep 17 00:00:00 2001 From: D0LPH1NB01 Date: Mon, 17 Mar 2025 13:00:00 -0700 Subject: [PATCH 05/14] Found a mistake, and fixed it --- src/main/java/org/team5924/frc2025/Robot.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2025/Robot.java b/src/main/java/org/team5924/frc2025/Robot.java index e161f78..f251abc 100644 --- a/src/main/java/org/team5924/frc2025/Robot.java +++ b/src/main/java/org/team5924/frc2025/Robot.java @@ -145,7 +145,7 @@ public void disabledInit() {} /** This function is called periodically when disabled. */ @Override public void disabledPeriodic() { - LEDSegment.MainStrip.setFadeAnimation(Lights.green, 0.5); + LEDSegment.MainStrip.setFadeAnimation(Lights.red, 0.5); } /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */ From 68bc16b5fb94ade8f5c96f95eadc2638e834b651 Mon Sep 17 00:00:00 2001 From: marisa07 Date: Wed, 2 Apr 2025 08:55:02 -0700 Subject: [PATCH 06/14] builds, needs testing --- .../org/team5924/frc2025/BuildConstants.java | 10 +- .../java/org/team5924/frc2025/Constants.java | 314 ++++++------------ 2 files changed, 101 insertions(+), 223 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 6b43bf7..908791f 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -22,11 +22,11 @@ public final class BuildConstants { public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; public static final int GIT_REVISION = 219; - public static final String GIT_SHA = "d7fc3944e01a03d126b3f619cc5087042bd37e3f"; - public static final String GIT_DATE = "2025-03-21 23:39:27 EDT"; - public static final String GIT_BRANCH = "sfr-autos"; - public static final String BUILD_DATE = "2025-03-22 13:48:55 EDT"; - public static final long BUILD_UNIX_TIME = 1742665735578L; + public static final String GIT_SHA = "e36617ed80021a9830c82f68cb60c1583cf86ca1"; + public static final String GIT_DATE = "2025-03-22 14:04:40 EDT"; + public static final String GIT_BRANCH = "main"; + public static final String BUILD_DATE = "2025-04-02 00:14:08 EDT"; + public static final long BUILD_UNIX_TIME = 1743567248838L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 96d2caf..02486f8 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -25,6 +25,7 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.units.measure.Distance; @@ -159,244 +160,121 @@ public static enum Mode { public static final Distance DELTA_X_CENTER_OF_CORAL_OUT_FROM_CENTER = Inches.of(-5.5); public static class Reef { - public static final double fieldWidth = - AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded).getFieldWidth(); + public static final double faceLength = Units.inchesToMeters(36.792600); + public static final double fieldWidth = field.getFieldWidth(); public static final Translation2d blueCenter = - new Translation2d(Units.inchesToMeters(176.75), Units.inchesToMeters(159.28)); + new Translation2d(Units.inchesToMeters(176.746), fieldWidth / 2.0); public static final Translation2d redCenter = - new Translation2d(Units.inchesToMeters(513.88), Units.inchesToMeters(159.03)); + new Translation2d(Units.inchesToMeters(513.88), fieldWidth / 2.0); + + public static final Pose2d[] centerFaces = + new Pose2d[6]; // Starting facing the driver station in clockwise order + public static final List branchPositions = + new ArrayList<>(); // Starting at the right branch facing the driver station in clockwise public static final List> branchRight2d = new ArrayList<>(); public static final List> branchLeft2d = new ArrayList<>(); - // public static final List> redBranchRight2d = new ArrayList<>(); - // public static final List> redBranchLeft2d = new ArrayList<>(); - static { - double halfIsoBaseOfBranchesAndCenter = 0.120; // Leg 1 (meters) - double distanceFromCenterToRoboCenterLineup = 2.05; // Leg 3 (meters) - double distanceFromCenterToRoboCenterShoot = 1.22; // Leg 3 but different (meters) - double offset = -.205; - double offsetCorrection = 0.09; // Correction for the offset just in case!! + // Initialize faces + centerFaces[0] = field.getTagPose(18).get().toPose2d(); + centerFaces[1] = field.getTagPose(19).get().toPose2d(); + centerFaces[2] = field.getTagPose(20).get().toPose2d(); + centerFaces[3] = field.getTagPose(21).get().toPose2d(); + centerFaces[4] = field.getTagPose(22).get().toPose2d(); + centerFaces[5] = field.getTagPose(17).get().toPose2d(); // Initialize branch positions for (int face = 0; face < 6; face++) { - Pose2d poseDirection = new Pose2d(blueCenter, Rotation2d.fromDegrees(180 - (60 * face))); - Logger.recordOutput("FacePoses/" + face, poseDirection); - - double radiusLineupCircle = - Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterLineup, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter, 2)); - - double radiusShootCircle = - Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter, 2)); - - var blueLeftBranchShoot = - new Pose2d( - blueCenter.getX() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow( - halfIsoBaseOfBranchesAndCenter + offset + offsetCorrection, 2)) - * Math.cos( - poseDirection.getRotation().getRadians() - - Math.atan( - (halfIsoBaseOfBranchesAndCenter + offset + offsetCorrection) - / distanceFromCenterToRoboCenterShoot))), - blueCenter.getY() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow( - halfIsoBaseOfBranchesAndCenter + offset + offsetCorrection, 2)) - * Math.sin( - poseDirection.getRotation().getRadians() - - Math.atan( - (halfIsoBaseOfBranchesAndCenter + offset + offsetCorrection) - / distanceFromCenterToRoboCenterShoot))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - var blueRightBranchShoot = - new Pose2d( - blueCenter.getX() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter - offset, 2)) - * Math.cos( - poseDirection.getRotation().getRadians() - + Math.atan( - (halfIsoBaseOfBranchesAndCenter - offset) - / distanceFromCenterToRoboCenterShoot))), - blueCenter.getY() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter - offset, 2)) - * Math.sin( - poseDirection.getRotation().getRadians() - + Math.atan( - (halfIsoBaseOfBranchesAndCenter - offset) - / distanceFromCenterToRoboCenterShoot))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - var blueLeftBranchLineup = - new Pose2d( - blueCenter.getX() - + (radiusLineupCircle - * Math.cos( - poseDirection.getRotation().getRadians() - - Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - blueCenter.getY() - + (radiusLineupCircle - * Math.sin( - poseDirection.getRotation().getRadians() - - Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - var blueRightBranchLineup = + Pose2d poseDirectionBlue = + new Pose2d(blueCenter, Rotation2d.fromDegrees(180 - (60 * face))); + Pose2d poseDirectionRed = new Pose2d(redCenter, Rotation2d.fromDegrees(180 - (60 * face))); + double adjustX = Units.inchesToMeters(30.738); // robot x + double adjustYLeft = Units.inchesToMeters(6.469); // robot y left + double adjustYRight = Units.inchesToMeters(6.469); // robot y right + + var rightBranchPoseShootBlue = new Pose2d( - blueCenter.getX() - + (radiusLineupCircle - * Math.cos( - poseDirection.getRotation().getRadians() - + Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - blueCenter.getY() - + (radiusLineupCircle - * Math.sin( - poseDirection.getRotation().getRadians() - + Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - ArrayList rightBranch = new ArrayList<>(); - rightBranch.add(blueRightBranchLineup); - rightBranch.add(blueRightBranchShoot); - Logger.recordOutput("ShootPosesRight/" + face, blueRightBranchShoot); - - ArrayList leftBranch = new ArrayList<>(); - leftBranch.add(blueLeftBranchLineup); - leftBranch.add(blueLeftBranchShoot); - Logger.recordOutput("ShootPosesLeft/" + face, blueLeftBranchShoot); - - branchRight2d.add(rightBranch); - branchLeft2d.add(leftBranch); - } + poseDirectionBlue + .transformBy(new Transform2d(adjustX, adjustYRight, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionBlue.getRotation().getRadians())); - for (int face = 0; face < 6; face++) { - Pose2d poseDirection = new Pose2d(redCenter, Rotation2d.fromDegrees(180 - (60 * face))); - Logger.recordOutput("FacePoses/" + face, poseDirection); + var leftBranchPoseShootBlue = + new Pose2d( + poseDirectionBlue + .transformBy(new Transform2d(adjustX, -adjustYLeft, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionBlue.getRotation().getRadians())); - double radiusLineupCircle = - Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterLineup, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter, 2)); + var rightBranchLineupPoseBlue = + new Pose2d( + poseDirectionBlue + .transformBy(new Transform2d(adjustX, adjustYRight, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionBlue.getRotation().getRadians())); - double radiusShootCircle = - Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter, 2)); + var leftBranchLineupPoseBlue = + new Pose2d( + poseDirectionBlue + .transformBy(new Transform2d(adjustX, -adjustYLeft, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionBlue.getRotation().getRadians())); - var redLeftBranchShoot = + var rightBranchPoseShootRed = new Pose2d( - redCenter.getX() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter + offset, 2)) - * Math.cos( - poseDirection.getRotation().getRadians() - - Math.atan( - (halfIsoBaseOfBranchesAndCenter + offset) - / distanceFromCenterToRoboCenterShoot))), - redCenter.getY() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter + offset, 2)) - * Math.sin( - poseDirection.getRotation().getRadians() - - Math.atan( - (halfIsoBaseOfBranchesAndCenter + offset) - / distanceFromCenterToRoboCenterShoot))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - var redRightBranchShoot = + poseDirectionRed + .transformBy(new Transform2d(adjustX, adjustYRight, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionRed.getRotation().getRadians())); + + var leftBranchPoseShootRed = new Pose2d( - redCenter.getX() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter - offset, 2)) - * Math.cos( - poseDirection.getRotation().getRadians() - + Math.atan( - (halfIsoBaseOfBranchesAndCenter - offset) - / distanceFromCenterToRoboCenterShoot))), - redCenter.getY() - + (Math.sqrt( - Math.pow(distanceFromCenterToRoboCenterShoot, 2) - + Math.pow(halfIsoBaseOfBranchesAndCenter - offset, 2)) - * Math.sin( - poseDirection.getRotation().getRadians() - + Math.atan( - (halfIsoBaseOfBranchesAndCenter - offset) - / distanceFromCenterToRoboCenterShoot))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - var redLeftBranchLineup = + poseDirectionRed + .transformBy(new Transform2d(adjustX, -adjustYLeft, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionRed.getRotation().getRadians())); + + var rightBranchLineupPoseRed = new Pose2d( - redCenter.getX() - + (radiusLineupCircle - * Math.cos( - poseDirection.getRotation().getRadians() - - Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - redCenter.getY() - + (radiusLineupCircle - * Math.sin( - poseDirection.getRotation().getRadians() - - Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - var redRightBranchLineup = + poseDirectionRed + .transformBy(new Transform2d(adjustX, adjustYRight, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionRed.getRotation().getRadians())); + + var leftBranchLineupPoseRed = new Pose2d( - redCenter.getX() - + (radiusLineupCircle - * Math.cos( - poseDirection.getRotation().getRadians() - + Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - redCenter.getY() - + (radiusLineupCircle - * Math.sin( - poseDirection.getRotation().getRadians() - + Math.atan( - halfIsoBaseOfBranchesAndCenter - / distanceFromCenterToRoboCenterLineup))), - Rotation2d.fromRadians(Math.PI / 3 * face).unaryMinus()); - - ArrayList rightBranch = new ArrayList<>(); - rightBranch.add(redRightBranchLineup); - rightBranch.add(redRightBranchShoot); - Logger.recordOutput("ShootPosesRight/" + face, redRightBranchShoot); - - ArrayList leftBranch = new ArrayList<>(); - leftBranch.add(redLeftBranchLineup); - leftBranch.add(redLeftBranchShoot); - Logger.recordOutput("ShootPosesLeft/" + face, redLeftBranchShoot); - - branchRight2d.add(rightBranch); - branchLeft2d.add(leftBranch); + poseDirectionRed + .transformBy(new Transform2d(adjustX, -adjustYLeft, Rotation2d.kZero)) + .getTranslation(), + new Rotation2d(poseDirectionRed.getRotation().getRadians())); + + ArrayList rightBranchBlue = new ArrayList<>(); + rightBranchBlue.add(rightBranchLineupPoseBlue); + rightBranchBlue.add(rightBranchPoseShootBlue); + Logger.recordOutput("ShootPosesRight/" + face, rightBranchPoseShootBlue); + + ArrayList rightBranchRed = new ArrayList<>(); + rightBranchRed.add(rightBranchLineupPoseRed); + rightBranchRed.add(rightBranchPoseShootRed); + Logger.recordOutput("ShootPosesRight/" + face, rightBranchPoseShootRed); + + ArrayList leftBranchBlue = new ArrayList<>(); + leftBranchBlue.add(leftBranchLineupPoseBlue); + leftBranchBlue.add(leftBranchPoseShootBlue); + Logger.recordOutput("ShootPosesLeft/" + face, leftBranchPoseShootBlue); + + ArrayList leftBranchRed = new ArrayList<>(); + leftBranchRed.add(leftBranchLineupPoseRed); + leftBranchRed.add(leftBranchPoseShootRed); + Logger.recordOutput("ShootPosesLeft/" + face, leftBranchPoseShootRed); + + branchRight2d.add(rightBranchBlue); + branchLeft2d.add(leftBranchBlue); + + branchRight2d.add(rightBranchRed); + branchLeft2d.add(leftBranchRed); } } } From 3e9ee0734de928af533c3b4c8773ee5ea3cbe257 Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Wed, 2 Apr 2025 20:43:26 -0700 Subject: [PATCH 07/14] fix build issues and merge conflicts --- .../pathplanner/autos/2Coral_Blue_Barge.auto | 25 ----------------- .../pathplanner/paths/2CS to Reef RB.path | 6 ++-- .../org/team5924/frc2025/BuildConstants.java | 10 +++---- .../java/org/team5924/frc2025/Constants.java | 1 - .../frc2025/subsystems/drive/Drive.java | 8 +++--- .../frc2025/subsystems/vision/Vision.java | 28 +++++++++---------- 6 files changed, 26 insertions(+), 52 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto diff --git a/src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto b/src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto deleted file mode 100644 index 861fa89..0000000 --- a/src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "2Leave BB" - } - }, - { - "type": "named", - "data": { - "name": "Run Shooter" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2CS to Reef RB.path b/src/main/deploy/pathplanner/paths/2CS to Reef RB.path index ca0db0e..d043111 100644 --- a/src/main/deploy/pathplanner/paths/2CS to Reef RB.path +++ b/src/main/deploy/pathplanner/paths/2CS to Reef RB.path @@ -17,11 +17,11 @@ { "anchor": { "x": 3.812090163934426, - "y": 2.8801741803278684 + "y": 2.904149590163935 }, "prevControl": { "x": 3.386849454589843, - "y": 2.4734973659541586 + "y": 2.497472775790225 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 56.72511201516516 + "rotation": 59.53445508054021 }, "reversed": false, "folder": null, diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 42d5999..b6743ce 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 220; - public static final String GIT_SHA = "68bc16b5fb94ade8f5c96f95eadc2638e834b651"; - public static final String GIT_DATE = "2025-04-02 11:55:02 EDT"; + public static final int GIT_REVISION = 254; + public static final String GIT_SHA = "4aafaff6810a9f059563b320056dc991cfcd3f1f"; + public static final String GIT_DATE = "2025-04-02 23:16:10 EDT"; public static final String GIT_BRANCH = "new_auto_align_poses"; - public static final String BUILD_DATE = "2025-04-02 21:52:28 EDT"; - public static final long BUILD_UNIX_TIME = 1743645148849L; + public static final String BUILD_DATE = "2025-04-02 23:21:14 EDT"; + public static final long BUILD_UNIX_TIME = 1743650474020L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index cc2f918..be494d2 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -64,7 +64,6 @@ public static enum Mode { public static final double FIELD_WIDTH = field.getFieldWidth(); public static final double FIELD_LENGTH = field.getFieldLength(); public static final double CORAL_STATION_RADIANS_NORMAL = 0.959931; - public static final double CORAL_STATION_RADIANS_NORMAL = 0.959931; /* General */ public static final double LOOP_PERIODIC_SECONDS = 0.02; diff --git a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java index 8ce0f63..d3c2344 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java +++ b/src/main/java/org/team5924/frc2025/subsystems/drive/Drive.java @@ -296,7 +296,7 @@ public void periodic() { field.setRobotPose(getPose()); - VisionFieldPoseEstimate visionPoseFront = RobotState.getInstance().getEstimatedPoseFront(); + VisionFieldPoseEstimate visionPoseFront = RobotState.getInstance().getEstimatedPoseFrontLeft(); VisionFieldPoseEstimate visionPoseBack = RobotState.getInstance().getEstimatedPoseBack(); @@ -305,7 +305,7 @@ public void periodic() { visionPoseFront.getVisionRobotPoseMeters(), visionPoseFront.getTimestampSeconds(), visionPoseFront.getVisionMeasurementStdDevs()); - RobotState.getInstance().setEstimatedPoseFront(null); + RobotState.getInstance().setEstimatedPoseFrontLeft(null); } else if (visionPoseFront == null && visionPoseBack != null) { addVisionMeasurement( visionPoseBack.getVisionRobotPoseMeters(), @@ -319,7 +319,7 @@ public void periodic() { visionPoseFront.getTimestampSeconds(), visionPoseFront.getVisionMeasurementStdDevs()); - RobotState.getInstance().setEstimatedPoseFront(null); + RobotState.getInstance().setEstimatedPoseFrontLeft(null); RobotState.getInstance().setEstimatedPoseBack(null); } else { addVisionMeasurement( @@ -327,7 +327,7 @@ public void periodic() { visionPoseBack.getTimestampSeconds(), visionPoseBack.getVisionMeasurementStdDevs()); - RobotState.getInstance().setEstimatedPoseFront(null); + RobotState.getInstance().setEstimatedPoseFrontLeft(null); RobotState.getInstance().setEstimatedPoseBack(null); } } diff --git a/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java b/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java index 2bcd1a5..54976ea 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java +++ b/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java @@ -57,9 +57,9 @@ public void periodic() { Logger.processInputs("Vision", inputs); updateVision( - inputs.frontUpLimelightSeesTarget, - inputs.frontUpFiducials, - inputs.megatag2PoseEstimateFrontUp); + inputs.frontLeftLimelightSeesTarget, + inputs.frontLeftFiducials, + inputs.megatag2PoseEstimateFrontLeft); updateVision( inputs.backLimelightSeesTarget, inputs.backFiducials, inputs.megatag2PoseEstimateBack); @@ -93,7 +93,7 @@ private void updateVision( Logger.recordOutput( "Vision/Front/" + "Megatag2Estimate", megatag2Estimate.get().getVisionRobotPoseMeters()); - RobotState.getInstance().setEstimatedPoseFront(megatag2Estimate.get()); + RobotState.getInstance().setEstimatedPoseFrontLeft(megatag2Estimate.get()); } else { Logger.recordOutput( "Vision/Back/" + "Megatag2Estimate", @@ -167,9 +167,9 @@ else if (poseEstimate.avgTagArea > 0.1 && poseDelta < 0.3) { public MegatagPoseEstimate getBotPose2dBlue() { // If all pose estimates are null, return null - if (inputs.megatag2PoseEstimateFrontUp == null + if (inputs.megatag2PoseEstimateFrontLeft == null && inputs.megatag2PoseEstimateBack == null - && inputs.megatag2PoseEstimateFrontDown == null) { + && inputs.megatag2PoseEstimateFrontRight == null) { return null; } @@ -178,10 +178,10 @@ public MegatagPoseEstimate getBotPose2dBlue() { double lowestAmbiguity = 1; // Initialize with the highest possible value // Compare front-up Limelight pose - if (inputs.megatag2PoseEstimateFrontUp != null - && inputs.lowestTagAmbiguityFrontUp < lowestAmbiguity) { - lowestAmbiguity = inputs.lowestTagAmbiguityFrontUp; - bestPose = inputs.megatag2PoseEstimateFrontUp; + if (inputs.megatag2PoseEstimateFrontLeft != null + && inputs.lowestTagAmbiguityFrontLeft < lowestAmbiguity) { + lowestAmbiguity = inputs.lowestTagAmbiguityFrontLeft; + bestPose = inputs.megatag2PoseEstimateFrontLeft; } // Compare back Limelight pose @@ -192,10 +192,10 @@ public MegatagPoseEstimate getBotPose2dBlue() { } // Compare front-down Limelight pose - if (inputs.megatag2PoseEstimateFrontDown != null - && inputs.lowestTagAmbiguityFrontDown < lowestAmbiguity) { - lowestAmbiguity = inputs.lowestTagAmbiguityFrontDown; - bestPose = inputs.megatag2PoseEstimateFrontDown; + if (inputs.megatag2PoseEstimateFrontRight != null + && inputs.lowestTagAmbiguityFrontRight < lowestAmbiguity) { + lowestAmbiguity = inputs.lowestTagAmbiguityFrontRight; + bestPose = inputs.megatag2PoseEstimateFrontRight; } return bestPose; // Returns the pose estimate with the lowest ambiguity From 6f7fad9b0f3268ca08c3a0e1bb45378ce67d22dc Mon Sep 17 00:00:00 2001 From: D0LPH1NB01 Date: Fri, 4 Apr 2025 15:33:57 -0700 Subject: [PATCH 08/14] set tuning_mode to false --- src/main/java/org/team5924/frc2025/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index 3682cfe..e1f423b 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -53,7 +53,7 @@ public static enum Mode { REPLAY } - public static final boolean TUNING_MODE = true; + public static final boolean TUNING_MODE = false; public static final boolean ALLOW_ASSERTS = false; /* Field */ From 0358bfa559f551dbc5bc1c64799be98a5fe4beaf Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Fri, 4 Apr 2025 15:41:54 -0700 Subject: [PATCH 09/14] fix autos --- .../pathplanner/autos/2Coral_Blue_Barge.auto | 25 ------ .../pathplanner/autos/2Coral_LeftBarge.auto | 2 +- .../deploy/pathplanner/autos/2Coral_Mid.auto | 3 +- .../pathplanner/paths/2CS to Reef BB.path | 81 ------------------- .../pathplanner/paths/2CS to Reef RB.path | 28 +++---- .../pathplanner/paths/2CS to ReefBB.path | 61 ++++++++++++++ .../deploy/pathplanner/paths/2Leave BB.path | 21 +++-- .../deploy/pathplanner/paths/2Leave RB.path | 27 +++---- .../pathplanner/paths/2Reef to CS BB.path | 43 +++------- .../org/team5924/frc2025/RobotContainer.java | 11 ++- .../generated/TunerConstantsGamma.java | 2 +- 11 files changed, 117 insertions(+), 187 deletions(-) delete mode 100644 src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto delete mode 100644 src/main/deploy/pathplanner/paths/2CS to Reef BB.path create mode 100644 src/main/deploy/pathplanner/paths/2CS to ReefBB.path diff --git a/src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto b/src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto deleted file mode 100644 index 861fa89..0000000 --- a/src/main/deploy/pathplanner/autos/2Coral_Blue_Barge.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": "2025.0", - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "2Leave BB" - } - }, - { - "type": "named", - "data": { - "name": "Run Shooter" - } - } - ] - } - }, - "resetOdom": true, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto b/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto index 7dbcde2..d8fd850 100644 --- a/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto +++ b/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto @@ -62,7 +62,7 @@ { "type": "path", "data": { - "pathName": "2CS to Reef BB" + "pathName": "2CS to ReefBB" } }, { diff --git a/src/main/deploy/pathplanner/autos/2Coral_Mid.auto b/src/main/deploy/pathplanner/autos/2Coral_Mid.auto index 77ea37d..52939ae 100644 --- a/src/main/deploy/pathplanner/autos/2Coral_Mid.auto +++ b/src/main/deploy/pathplanner/autos/2Coral_Mid.auto @@ -30,7 +30,6 @@ } }, { - "type": "named", "type": "wait", "data": { "waitTime": 1.0 @@ -69,7 +68,7 @@ { "type": "path", "data": { - "pathName": "2CS to Reef BB" + "pathName": "2CS to ReefBB" } }, { diff --git a/src/main/deploy/pathplanner/paths/2CS to Reef BB.path b/src/main/deploy/pathplanner/paths/2CS to Reef BB.path deleted file mode 100644 index c8eba5b..0000000 --- a/src/main/deploy/pathplanner/paths/2CS to Reef BB.path +++ /dev/null @@ -1,81 +0,0 @@ -{ - "version": "2025.0", - "waypoints": [ - { - "anchor": { - "x": 1.3785860655737705, - "y": 7.159784836065573 - "x": 1.3785860655737705, - "y": 7.159784836065573 - }, - "prevControl": null, - "nextControl": { - "x": 2.2108819922093375, - "y": 6.32277142208849 - "x": 2.2108819922093375, - "y": 6.32277142208849 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.8600409836065577, - "y": 5.1698258196721305 - }, - "prevControl": { - "x": 3.0777816362341808, - "y": 5.963512283005356 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [ - { - "name": "Constraints Zone", - "minWaypointRelativePos": 0.359449192782526, - "maxWaypointRelativePos": 1.0, - "constraints": { - "maxVelocity": 3.0, - "maxAcceleration": 1.5, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - } - } - ], - "pointTowardsZones": [], - "eventMarkers": [ - { - "name": "Elevator Height L4 Trigger", - "waypointRelativePos": 0.5, - "endWaypointRelativePos": null, - "command": null - } - ], - "globalConstraints": { - "maxVelocity": 3.0, - "maxAcceleration": 3.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 720.0, - "nominalVoltage": 12.0, - "unlimited": false - }, - "goalEndState": { - "velocity": 0, - "rotation": -58.495733280795854 - "rotation": -58.495733280795854 - }, - "reversed": false, - "folder": null, - "folder": null, - "idealStartingState": { - "velocity": 0, - "rotation": -53.67317404787975 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2CS to Reef RB.path b/src/main/deploy/pathplanner/paths/2CS to Reef RB.path index f55cf50..7749e7b 100644 --- a/src/main/deploy/pathplanner/paths/2CS to Reef RB.path +++ b/src/main/deploy/pathplanner/paths/2CS to Reef RB.path @@ -3,29 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.3665983606557375, - "y": 0.8542520491803282 - "x": 1.3665983606557375, - "y": 0.8542520491803282 + "x": 1.365, + "y": 0.85625 }, "prevControl": null, "nextControl": { - "x": 1.738734891471663, - "y": 1.2269957983383706 - "x": 1.738734891471663, - "y": 1.2269957983383706 + "x": 2.4761616572598832, + "y": 1.6692735651438038 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.800102459016393, - "y": 2.9281249999999988 + "x": 3.8220000000000005, + "y": 2.913499999999999 }, "prevControl": { - "x": 3.37486174967181, - "y": 2.521448185626289 + "x": 3.0567427992521483, + "y": 2.3673790809281288 }, "nextControl": null, "isLocked": false, @@ -38,7 +34,7 @@ "eventMarkers": [ { "name": "Elevator Height L4 Trigger", - "waypointRelativePos": 0.45, + "waypointRelativePos": 0.49460431654676074, "endWaypointRelativePos": null, "command": null } @@ -53,15 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 57.42594286542748 + "rotation": 59.381394591090626 }, "reversed": false, "folder": null, - "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 52.25319461272525 - "rotation": 52.25319461272525 + "rotation": 53.13010235415595 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path new file mode 100644 index 0000000..ae4b376 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 1.25775, + "y": 7.09625 + }, + "prevControl": null, + "nextControl": { + "x": 2.550817896251039, + "y": 6.136936260692936 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 3.85125, + "y": 5.16575 + }, + "prevControl": { + "x": 2.9356847262118766, + "y": 5.850894554364872 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator Height L4 Trigger", + "waypointRelativePos": 0.4974820143884872, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -59.23417355525227 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": -55.007979801441365 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2Leave BB.path b/src/main/deploy/pathplanner/paths/2Leave BB.path index 9b25f24..a6c5a1e 100644 --- a/src/main/deploy/pathplanner/paths/2Leave BB.path +++ b/src/main/deploy/pathplanner/paths/2Leave BB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.576229508196722, + "x": 7.564241803278687, "y": 6.1648053278688515 }, "prevControl": null, "nextControl": { - "x": 6.574537041550793, - "y": 5.867256595301896 + "x": 6.025132933524411, + "y": 5.851987013429503 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.214651639344262, - "y": 5.1098872950819665 + "x": 5.19675, + "y": 5.1267499999999995 }, "prevControl": { - "x": 6.267882900640609, - "y": 5.643685868445044 + "x": 5.778873329489986, + "y": 5.487873266046821 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.2321937321937275, + "minWaypointRelativePos": 0.3423551756885091, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, @@ -62,9 +62,8 @@ "unlimited": false }, "goalEndState": { - "velocity": 0.0, - "rotation": -122.34744349944194 - "rotation": -122.34744349944194 + "velocity": 0, + "rotation": -120.46554491945994 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2Leave RB.path b/src/main/deploy/pathplanner/paths/2Leave RB.path index 563a664..2b2d640 100644 --- a/src/main/deploy/pathplanner/paths/2Leave RB.path +++ b/src/main/deploy/pathplanner/paths/2Leave RB.path @@ -3,29 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.564241803278687, - "y": 1.9451331967213106 + "x": 7.566, + "y": 1.9092499999999994 }, "prevControl": null, "nextControl": { - "x": 6.54662969651593, - "y": 2.158300701054179 + "x": 6.28875, + "y": 2.3869999999999987 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.142725409836065, - "y": 2.868186475409835 - "x": 5.142725409836065, - "y": 2.868186475409835 + "x": 5.148, + "y": 2.8939999999999997 }, "prevControl": { - "x": 6.144354293943504, - "y": 2.4649833025324166 - "x": 6.144354293943504, - "y": 2.4649833025324166 + "x": 5.947499999999999, + "y": 2.4844999999999993 }, "nextControl": null, "isLocked": false, @@ -38,7 +34,7 @@ "eventMarkers": [ { "name": "Elevator Height L4 Trigger", - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.4974820143884962, "endWaypointRelativePos": null, "command": null } @@ -53,14 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 120.1413855520754 + "rotation": 121.35708522400999 }, "reversed": false, "folder": null, - "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/2Reef to CS BB.path b/src/main/deploy/pathplanner/paths/2Reef to CS BB.path index 53fc635..dd2406a 100644 --- a/src/main/deploy/pathplanner/paths/2Reef to CS BB.path +++ b/src/main/deploy/pathplanner/paths/2Reef to CS BB.path @@ -3,55 +3,38 @@ "waypoints": [ { "anchor": { - "x": 5.190676229508196, - "y": 5.1098872950819665 + "x": 5.1675, + "y": 5.155999999999999 }, "prevControl": null, "nextControl": { - "x": 4.507377049180327, - "y": 5.745235655737705 + "x": 3.908759898533322, + "y": 5.831647078593476 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.342622950819672, - "y": 7.111834016393442 - "x": 1.342622950819672, - "y": 7.111834016393442 + "x": 1.23825, + "y": 7.066999999999999 }, "prevControl": { - "x": 1.8380122950819653, - "y": 6.833094262295082 - "x": 1.8380122950819653, - "y": 6.833094262295082 + "x": 2.790491982756322, + "y": 6.271183145257328 }, "nextControl": null, "isLocked": false, "linkedName": null } ], - "rotationTargets": [ - { - "waypointRelativePos": 0.5, - "rotationDegrees": -71.38484035286557 - } - ], + "rotationTargets": [], "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ { "name": "Elevator Height Intake Trigger", - "waypointRelativePos": 0, - "endWaypointRelativePos": null, - "command": null - } - ], - "eventMarkers": [ - { - "name": "Elevator Height Intake Trigger", - "waypointRelativePos": 0, + "waypointRelativePos": 0.1017985611510769, "endWaypointRelativePos": null, "command": null } @@ -66,15 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": -55.49147701233164 + "rotation": -55.007979801441294 }, "reversed": false, "folder": null, - "folder": null, "idealStartingState": { "velocity": 0, - "rotation": -120.46554491945989 - "rotation": -120.46554491945989 + "rotation": -120.06858282186238 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index 1dbe2fc..ab81ed0 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -20,6 +20,8 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.events.EventTrigger; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -55,6 +57,7 @@ import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOKrakenFOC; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOSim; +import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.vision.Vision; import org.team5924.frc2025.subsystems.vision.VisionIO; import org.team5924.frc2025.subsystems.vision.VisionIOLimelight; @@ -130,8 +133,9 @@ public RobotContainer() { break; } - NamedCommands.registerCommand("Run Shooter", new RunShooter(coralInAndOut)); - NamedCommands.registerCommand("Run Intake", new RunIntake(coralInAndOut)); + NamedCommands.registerCommand("Run Shooter", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.SHOOTING_L4))); + NamedCommands.registerCommand("Run Intake", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.INTAKING))); + NamedCommands.registerCommand("Coral In Intake", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.STORED_CORAL_IN_INTAKE))); NamedCommands.registerCommand( "Elevator Height L4", Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.L4))); @@ -142,6 +146,9 @@ public RobotContainer() { "Elevator Height Intake", Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.INTAKE))); + new EventTrigger("Elevator Height L4 Trigger").onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.L4))); + new EventTrigger("Elevator Height Intake Trigger").onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.INTAKE))); + // Set up auto routines boolean isCompetition = true; diff --git a/src/main/java/org/team5924/frc2025/generated/TunerConstantsGamma.java b/src/main/java/org/team5924/frc2025/generated/TunerConstantsGamma.java index 9fc0b7c..4b76ab4 100644 --- a/src/main/java/org/team5924/frc2025/generated/TunerConstantsGamma.java +++ b/src/main/java/org/team5924/frc2025/generated/TunerConstantsGamma.java @@ -62,7 +62,7 @@ public class TunerConstantsGamma { // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput private static final Slot0Configs steerGains = new Slot0Configs() - .withKP(128) + .withKP(110) .withKI(0) .withKD(0.2) .withKS(0.1) From 4f353a0056aa1f0e853f013ab89e9fb5c3646b16 Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Fri, 4 Apr 2025 15:57:47 -0700 Subject: [PATCH 10/14] fix autos for the second time --- .../pathplanner/paths/2CS to ReefBB.path | 14 ++++++------- .../deploy/pathplanner/paths/2Leave BB.path | 14 ++++++------- .../deploy/pathplanner/paths/2Leave Mid.path | 17 +++++++-------- .../pathplanner/paths/2Reef to CS BB.path | 12 +++++------ .../org/team5924/frc2025/BuildConstants.java | 12 +++++------ .../org/team5924/frc2025/RobotContainer.java | 21 +++++++++++-------- 6 files changed, 46 insertions(+), 44 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path index ae4b376..7024e9b 100644 --- a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path +++ b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.550817896251039, - "y": 6.136936260692936 + "x": 2.9055, + "y": 6.6185 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.85125, - "y": 5.16575 + "x": 3.93, + "y": 5.15 }, "prevControl": { - "x": 2.9356847262118766, - "y": 5.850894554364872 + "x": 3.36375, + "y": 6.25775 }, "nextControl": null, "isLocked": false, @@ -49,7 +49,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -59.23417355525227 + "rotation": -59.99999999999999 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2Leave BB.path b/src/main/deploy/pathplanner/paths/2Leave BB.path index a6c5a1e..5ee7ca3 100644 --- a/src/main/deploy/pathplanner/paths/2Leave BB.path +++ b/src/main/deploy/pathplanner/paths/2Leave BB.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 6.025132933524411, - "y": 5.851987013429503 + "x": 5.565088238781995, + "y": 6.1648053278688515 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.19675, - "y": 5.1267499999999995 + "x": 5.17, + "y": 5.07 }, "prevControl": { - "x": 5.778873329489986, - "y": 5.487873266046821 + "x": 5.768198924376331, + "y": 6.106110930052859 }, "nextControl": null, "isLocked": false, @@ -63,7 +63,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": -120.46554491945994 + "rotation": -119.99999999999999 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2Leave Mid.path b/src/main/deploy/pathplanner/paths/2Leave Mid.path index 434edb1..9127098 100644 --- a/src/main/deploy/pathplanner/paths/2Leave Mid.path +++ b/src/main/deploy/pathplanner/paths/2Leave Mid.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 7.588217213114754, + "x": 7.588, "y": 4.04298155737705 }, "prevControl": null, "nextControl": { - "x": 6.287462510399758, - "y": 4.791696373149065 + "x": 6.747000000000001, + "y": 5.6922500000000005 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.226639344262295, - "y": 5.121875 + "x": 5.17, + "y": 5.07 }, "prevControl": { - "x": 5.435777039551542, - "y": 4.984902650932381 + "x": 5.5950000000000015, + "y": 5.806121593216773 }, "nextControl": null, "isLocked": false, @@ -63,11 +63,10 @@ }, "goalEndState": { "velocity": 0, - "rotation": -120.46554491945994 + "rotation": -119.99999999999999 }, "reversed": false, "folder": null, - "folder": null, "idealStartingState": { "velocity": 0, "rotation": 180.0 diff --git a/src/main/deploy/pathplanner/paths/2Reef to CS BB.path b/src/main/deploy/pathplanner/paths/2Reef to CS BB.path index dd2406a..0c347cd 100644 --- a/src/main/deploy/pathplanner/paths/2Reef to CS BB.path +++ b/src/main/deploy/pathplanner/paths/2Reef to CS BB.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.1675, - "y": 5.155999999999999 + "x": 5.17, + "y": 5.07 }, "prevControl": null, "nextControl": { - "x": 3.908759898533322, - "y": 5.831647078593476 + "x": 5.187, + "y": 5.448499999999999 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 7.066999999999999 }, "prevControl": { - "x": 2.790491982756322, - "y": 6.271183145257328 + "x": 2.964, + "y": 6.43325 }, "nextControl": null, "isLocked": false, diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 79fffec..6f41ad0 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 261; - public static final String GIT_SHA = "ee01ef8e151ba79487087c195fac7c241fdb8556"; - public static final String GIT_DATE = "2025-04-04 12:08:29 EDT"; - public static final String GIT_BRANCH = "gamma_merge_test_branch"; - public static final String BUILD_DATE = "2025-04-04 15:16:16 EDT"; - public static final long BUILD_UNIX_TIME = 1743794176123L; + public static final int GIT_REVISION = 272; + public static final String GIT_SHA = "15feb89e99564b6b35a07a46c11bda0eed6aa68e"; + public static final String GIT_DATE = "2025-04-04 18:42:42 EDT"; + public static final String GIT_BRANCH = "ebr-hotfix"; + public static final String BUILD_DATE = "2025-04-04 18:55:52 EDT"; + public static final long BUILD_UNIX_TIME = 1743807352620L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index ab81ed0..d762bb5 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -21,7 +21,6 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.events.EventTrigger; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj.GenericHID; @@ -34,8 +33,6 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import java.util.Set; -import org.team5924.frc2025.commands.coralInAndOut.RunIntake; -import org.team5924.frc2025.commands.coralInAndOut.RunShooter; import org.team5924.frc2025.commands.coralInAndOut.TeleopShoot; import org.team5924.frc2025.commands.drive.DriveCommands; import org.team5924.frc2025.commands.elevator.RunElevator; @@ -54,10 +51,10 @@ import org.team5924.frc2025.subsystems.elevator.ElevatorIO; import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFXGamma; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut; +import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOKrakenFOC; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIOSim; -import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.vision.Vision; import org.team5924.frc2025.subsystems.vision.VisionIO; import org.team5924.frc2025.subsystems.vision.VisionIOLimelight; @@ -133,9 +130,13 @@ public RobotContainer() { break; } - NamedCommands.registerCommand("Run Shooter", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.SHOOTING_L4))); - NamedCommands.registerCommand("Run Intake", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.INTAKING))); - NamedCommands.registerCommand("Coral In Intake", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.STORED_CORAL_IN_INTAKE))); + NamedCommands.registerCommand( + "Run Shooter", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.SHOOTING_L4))); + NamedCommands.registerCommand( + "Run Intake", Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.INTAKING))); + NamedCommands.registerCommand( + "Coral In Intake", + Commands.runOnce(() -> coralInAndOut.setGoalState(CoralState.STORED_CORAL_IN_INTAKE))); NamedCommands.registerCommand( "Elevator Height L4", Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.L4))); @@ -146,8 +147,10 @@ public RobotContainer() { "Elevator Height Intake", Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.INTAKE))); - new EventTrigger("Elevator Height L4 Trigger").onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.L4))); - new EventTrigger("Elevator Height Intake Trigger").onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.INTAKE))); + new EventTrigger("Elevator Height L4 Trigger") + .onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.L4))); + new EventTrigger("Elevator Height Intake Trigger") + .onTrue(Commands.runOnce(() -> elevator.setGoalState(Elevator.ElevatorState.INTAKE))); // Set up auto routines boolean isCompetition = true; From eaa28ff082018cd1cb478d8e4d1d25d86cf02d9a Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Sat, 5 Apr 2025 08:31:55 -0700 Subject: [PATCH 11/14] push autos for match --- .../pathplanner/autos/2Coral_LeftBarge.auto | 30 +++++++++++++++++++ .../pathplanner/paths/2CS to ReefBB.path | 18 +++++++++-- .../deploy/pathplanner/paths/2Leave BB.path | 4 +-- 3 files changed, 48 insertions(+), 4 deletions(-) diff --git a/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto b/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto index d8fd850..66b072b 100644 --- a/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto +++ b/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto @@ -17,6 +17,12 @@ ] } }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, { "type": "named", "data": { @@ -65,11 +71,35 @@ "pathName": "2CS to ReefBB" } }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, { "type": "named", "data": { "name": "Run Shooter" } + }, + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Elevator Height Intake" + } + }, + { + "type": "named", + "data": { + "name": "Stop CoralInAndOut" + } } ] } diff --git a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path index 7024e9b..4fa0eaa 100644 --- a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path +++ b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path @@ -29,12 +29,26 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { "name": "Elevator Height L4 Trigger", - "waypointRelativePos": 0.4974820143884872, + "waypointRelativePos": 0.8, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/deploy/pathplanner/paths/2Leave BB.path b/src/main/deploy/pathplanner/paths/2Leave BB.path index 5ee7ca3..12b2186 100644 --- a/src/main/deploy/pathplanner/paths/2Leave BB.path +++ b/src/main/deploy/pathplanner/paths/2Leave BB.path @@ -32,7 +32,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.3423551756885091, + "minWaypointRelativePos": 0.7, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, @@ -48,7 +48,7 @@ "eventMarkers": [ { "name": "Elevator Height L4 Trigger", - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.8, "endWaypointRelativePos": null, "command": null } From 0711cd7c38749360cceb622efd05200c7ac79562 Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Sat, 5 Apr 2025 16:55:15 -0700 Subject: [PATCH 12/14] LED untested --- .../pathplanner/autos/2Coral_LeftBarge.auto | 10 +-- .../pathplanner/autos/2Coral_RightBarge.auto | 2 +- .../deploy/pathplanner/paths/1Leave BB.path | 75 +++++++++++++++++++ .../deploy/pathplanner/paths/1Leave RB.path | 61 +++++++++++++++ .../pathplanner/paths/2CS to Reef RB.path | 24 +++--- .../pathplanner/paths/2CS to ReefBB.path | 24 +++--- .../deploy/pathplanner/paths/2Leave Mid.path | 12 +-- .../deploy/pathplanner/paths/2Leave RB.path | 12 +-- .../pathplanner/paths/2Reef to CS BB.path | 40 +++++++--- .../pathplanner/paths/2Reef to CS RB.path | 36 ++++++--- .../org/team5924/frc2025/BuildConstants.java | 10 +-- .../java/org/team5924/frc2025/Constants.java | 13 ++-- .../org/team5924/frc2025/RobotContainer.java | 18 +++-- .../commands/lights/SetLEDColorCommand.java | 43 +++++++++++ .../subsystems/Lights/LEDSubsystem.java | 32 ++++++++ .../frc2025/subsystems/elevator/Elevator.java | 2 +- .../rollers/CoralInAndOut/CoralInAndOut.java | 2 +- .../frc2025/subsystems/vision/Vision.java | 2 +- 18 files changed, 340 insertions(+), 78 deletions(-) create mode 100644 src/main/deploy/pathplanner/paths/1Leave BB.path create mode 100644 src/main/deploy/pathplanner/paths/1Leave RB.path create mode 100644 src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java create mode 100644 src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java diff --git a/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto b/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto index 66b072b..3244cab 100644 --- a/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto +++ b/src/main/deploy/pathplanner/autos/2Coral_LeftBarge.auto @@ -11,7 +11,7 @@ { "type": "path", "data": { - "pathName": "2Leave BB" + "pathName": "1Leave BB" } } ] @@ -32,7 +32,7 @@ { "type": "wait", "data": { - "waitTime": 1.0 + "waitTime": 0.5 } }, { @@ -56,7 +56,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.5 } }, { @@ -92,13 +92,13 @@ { "type": "named", "data": { - "name": "Elevator Height Intake" + "name": "Stop CoralInAndOut" } }, { "type": "named", "data": { - "name": "Stop CoralInAndOut" + "name": "Elevator Height Intake" } } ] diff --git a/src/main/deploy/pathplanner/autos/2Coral_RightBarge.auto b/src/main/deploy/pathplanner/autos/2Coral_RightBarge.auto index 7cb3f2a..87d5b49 100644 --- a/src/main/deploy/pathplanner/autos/2Coral_RightBarge.auto +++ b/src/main/deploy/pathplanner/autos/2Coral_RightBarge.auto @@ -50,7 +50,7 @@ { "type": "wait", "data": { - "waitTime": 2.0 + "waitTime": 1.5 } }, { diff --git a/src/main/deploy/pathplanner/paths/1Leave BB.path b/src/main/deploy/pathplanner/paths/1Leave BB.path new file mode 100644 index 0000000..32ca442 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1Leave BB.path @@ -0,0 +1,75 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.564, + "y": 6.1648053278688515 + }, + "prevControl": null, + "nextControl": { + "x": 5.564846435503308, + "y": 6.1648053278688515 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.18, + "y": 5.04 + }, + "prevControl": { + "x": 5.778198924376331, + "y": 6.076110930052859 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 3.0, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator Height L4 Trigger", + "waypointRelativePos": 0.8, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": -119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/1Leave RB.path b/src/main/deploy/pathplanner/paths/1Leave RB.path new file mode 100644 index 0000000..cc70462 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/1Leave RB.path @@ -0,0 +1,61 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 7.566, + "y": 1.9092499999999994 + }, + "prevControl": null, + "nextControl": { + "x": 6.28875, + "y": 2.3869999999999987 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.02, + "y": 2.92 + }, + "prevControl": { + "x": 5.502356557377048, + "y": 2.041034836065575 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [ + { + "name": "Elevator Height L4 Trigger", + "waypointRelativePos": 0.4974820143884962, + "endWaypointRelativePos": null, + "command": null + } + ], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 119.99999999999999 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 180.0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2CS to Reef RB.path b/src/main/deploy/pathplanner/paths/2CS to Reef RB.path index 7749e7b..37fae7a 100644 --- a/src/main/deploy/pathplanner/paths/2CS to Reef RB.path +++ b/src/main/deploy/pathplanner/paths/2CS to Reef RB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.365, - "y": 0.85625 + "x": 1.174795081967213, + "y": 0.7823258196721316 }, "prevControl": null, "nextControl": { - "x": 2.4761616572598832, - "y": 1.6692735651438038 + "x": 2.285956739227096, + "y": 1.5953493848159352 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.8220000000000005, - "y": 2.913499999999999 + "x": 3.8, + "y": 3.01 }, "prevControl": { - "x": 3.0567427992521483, - "y": 2.3673790809281288 + "x": 3.0347427992521476, + "y": 2.4638790809281295 }, "nextControl": null, "isLocked": false, @@ -32,6 +32,12 @@ "constraintZones": [], "pointTowardsZones": [], "eventMarkers": [ + { + "name": "Elevator Height Zero Trigger", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": null + }, { "name": "Elevator Height L4 Trigger", "waypointRelativePos": 0.49460431654676074, @@ -49,7 +55,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 59.381394591090626 + "rotation": 59.99999999999999 }, "reversed": false, "folder": null, diff --git a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path index 4fa0eaa..c221ed2 100644 --- a/src/main/deploy/pathplanner/paths/2CS to ReefBB.path +++ b/src/main/deploy/pathplanner/paths/2CS to ReefBB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 1.25775, - "y": 7.09625 + "x": 1.1867827868852459, + "y": 7.111834016393442 }, "prevControl": null, "nextControl": { - "x": 2.9055, - "y": 6.6185 + "x": 2.834532786885246, + "y": 6.634084016393442 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.93, - "y": 5.15 + "x": 3.95, + "y": 5.13 }, "prevControl": { - "x": 3.36375, - "y": 6.25775 + "x": 3.38375, + "y": 6.237749999999999 }, "nextControl": null, "isLocked": false, @@ -33,7 +33,7 @@ { "name": "Constraints Zone", "minWaypointRelativePos": 0.7, - "maxWaypointRelativePos": 1.0, + "maxWaypointRelativePos": 1, "constraints": { "maxVelocity": 3.0, "maxAcceleration": 1.5, @@ -46,6 +46,12 @@ ], "pointTowardsZones": [], "eventMarkers": [ + { + "name": "Elevator Height Zero Trigger", + "waypointRelativePos": 0.1, + "endWaypointRelativePos": null, + "command": null + }, { "name": "Elevator Height L4 Trigger", "waypointRelativePos": 0.8, diff --git a/src/main/deploy/pathplanner/paths/2Leave Mid.path b/src/main/deploy/pathplanner/paths/2Leave Mid.path index 9127098..fd120bb 100644 --- a/src/main/deploy/pathplanner/paths/2Leave Mid.path +++ b/src/main/deploy/pathplanner/paths/2Leave Mid.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 6.747000000000001, - "y": 5.6922500000000005 + "x": 6.557274590163935, + "y": 5.097899590163934 }, "isLocked": false, "linkedName": null @@ -20,8 +20,8 @@ "y": 5.07 }, "prevControl": { - "x": 5.5950000000000015, - "y": 5.806121593216773 + "x": 5.4304303278688515, + "y": 5.7212602459016395 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [ { "name": "Constraints Zone", - "minWaypointRelativePos": 0.3423551756885091, + "minWaypointRelativePos": 0.7, "maxWaypointRelativePos": 1.0, "constraints": { "maxVelocity": 3.0, @@ -48,7 +48,7 @@ "eventMarkers": [ { "name": "Elevator Height L4 Trigger", - "waypointRelativePos": 0.5, + "waypointRelativePos": 0.8, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/deploy/pathplanner/paths/2Leave RB.path b/src/main/deploy/pathplanner/paths/2Leave RB.path index 2b2d640..ed7c653 100644 --- a/src/main/deploy/pathplanner/paths/2Leave RB.path +++ b/src/main/deploy/pathplanner/paths/2Leave RB.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 5.148, - "y": 2.8939999999999997 + "x": 5.02, + "y": 2.92 }, "prevControl": { - "x": 5.947499999999999, - "y": 2.4844999999999993 + "x": 5.595, + "y": 1.9240707856478956 }, "nextControl": null, "isLocked": false, @@ -49,13 +49,13 @@ }, "goalEndState": { "velocity": 0, - "rotation": 121.35708522400999 + "rotation": 119.99999999999999 }, "reversed": false, "folder": null, "idealStartingState": { "velocity": 0, - "rotation": 0.0 + "rotation": 180.0 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/2Reef to CS BB.path b/src/main/deploy/pathplanner/paths/2Reef to CS BB.path index 0c347cd..009dc46 100644 --- a/src/main/deploy/pathplanner/paths/2Reef to CS BB.path +++ b/src/main/deploy/pathplanner/paths/2Reef to CS BB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.17, - "y": 5.07 + "x": 5.18, + "y": 5.04 }, "prevControl": null, "nextControl": { - "x": 5.187, - "y": 5.448499999999999 + "x": 5.197, + "y": 5.418499999999999 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.23825, - "y": 7.066999999999999 + "x": 1.1867827868852459, + "y": 7.123821721311475 }, "prevControl": { - "x": 2.964, - "y": 6.43325 + "x": 2.9125327868852455, + "y": 6.490071721311476 }, "nextControl": null, "isLocked": false, @@ -29,12 +29,32 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.7317188983855645, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.75, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ + { + "name": "Elevator Height Zero Trigger", + "waypointRelativePos": 0, + "endWaypointRelativePos": null, + "command": null + }, { "name": "Elevator Height Intake Trigger", - "waypointRelativePos": 0.1017985611510769, + "waypointRelativePos": 0.9003597122302117, "endWaypointRelativePos": null, "command": null } diff --git a/src/main/deploy/pathplanner/paths/2Reef to CS RB.path b/src/main/deploy/pathplanner/paths/2Reef to CS RB.path index d84fa50..c4fe179 100644 --- a/src/main/deploy/pathplanner/paths/2Reef to CS RB.path +++ b/src/main/deploy/pathplanner/paths/2Reef to CS RB.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 5.154713114754099, - "y": 2.868186475409835 + "x": 5.02, + "y": 2.92 }, "prevControl": null, "nextControl": { - "x": 3.932676308904921, - "y": 2.1860196901107094 + "x": 5.38247950819672, + "y": 2.436629098360655 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 1.3306352459016395, - "y": 0.8662397540983602 + "x": 1.1388319672131146, + "y": 0.7943135245901636 }, "prevControl": { - "x": 2.9125237958491885, - "y": 1.6570252923589135 + "x": 2.7207205171606637, + "y": 1.5850990628507169 }, "nextControl": null, "isLocked": false, @@ -29,7 +29,21 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Constraints Zone", + "minWaypointRelativePos": 0.6937321937321907, + "maxWaypointRelativePos": 1.0, + "constraints": { + "maxVelocity": 1.5, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + } + } + ], "pointTowardsZones": [], "eventMarkers": [ { @@ -54,8 +68,8 @@ "reversed": false, "folder": null, "idealStartingState": { - "velocity": 0, - "rotation": 120.61860540890937 + "velocity": 0.0, + "rotation": 119.99999999999999 }, "useDefaultConstraints": true } \ No newline at end of file diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index ce51bb3..50cecc8 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 223; - public static final String GIT_SHA = "e51c7eda7b0bb86b433b32483a325aef94f197c5"; - public static final String GIT_DATE = "2025-04-05 13:05:33 EDT"; + public static final int GIT_REVISION = 287; + public static final String GIT_SHA = "24a99eaed60bbad2ae1463174fcd670046c7d588"; + public static final String GIT_DATE = "2025-04-05 14:18:45 EDT"; public static final String GIT_BRANCH = "LED-code"; - public static final String BUILD_DATE = "2025-04-05 13:11:24 EDT"; - public static final long BUILD_UNIX_TIME = 1743873084978L; + public static final String BUILD_DATE = "2025-04-05 19:43:12 EDT"; + public static final long BUILD_UNIX_TIME = 1743896592782L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/Constants.java b/src/main/java/org/team5924/frc2025/Constants.java index ec2200d..4cc8547 100644 --- a/src/main/java/org/team5924/frc2025/Constants.java +++ b/src/main/java/org/team5924/frc2025/Constants.java @@ -127,7 +127,7 @@ public static enum Mode { public static final double ALGAE_REDUCTION = 1.0; // Adjust value as needed /*Lights */ - public static final int CANDLE_ID = 49; + public static final int CANDLE_ID = 41; /* # Vision # */ public static String APRIL_TAG_LIMELIGHT_NAME_FRONTL = "limelight-frontl"; @@ -169,11 +169,10 @@ public static class Reef { public static final double faceLength = Units.inchesToMeters(36.792600); public static final double fieldWidth = field.getFieldWidth(); public static final Translation2d blueCenter = - new Translation2d(Units.inchesToMeters(176.746), fieldWidth / 2.0); + new Translation2d(Units.inchesToMeters(176.745), Units.inchesToMeters(158.5)); public static final Translation2d redCenter = - new Translation2d(Units.inchesToMeters(513.88), fieldWidth / 2.0); - + new Translation2d(Units.inchesToMeters(514.13), Units.inchesToMeters(158.5)); public static final Pose2d[] centerFaces = new Pose2d[6]; // Starting facing the driver station in clockwise order public static final List branchPositions = @@ -196,9 +195,9 @@ public static class Reef { Pose2d poseDirectionBlue = new Pose2d(blueCenter, Rotation2d.fromDegrees(180 - (60 * face))); Pose2d poseDirectionRed = new Pose2d(redCenter, Rotation2d.fromDegrees(180 - (60 * face))); - double adjustX = Units.inchesToMeters(49.75 - 2); // robot x - double adjustYLeft = Units.inchesToMeters(6.469 - 6); // robot y left - double adjustYRight = Units.inchesToMeters(6.469 + 6); // robot y right + double adjustX = Units.inchesToMeters(48.15); // robot x + double adjustYLeft = Units.inchesToMeters(6.469 - 3); // robot y left + double adjustYRight = Units.inchesToMeters(6.469 + 3.25); // robot y right var rightBranchPoseShootBlue = new Pose2d( diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index ca43002..3fa2b41 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -17,6 +17,7 @@ package org.team5924.frc2025; import static edu.wpi.first.units.Units.Seconds; +import static org.team5924.frc2025.Constants.CANDLE_ID; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -36,7 +37,9 @@ import org.team5924.frc2025.commands.coralInAndOut.TeleopShoot; import org.team5924.frc2025.commands.drive.DriveCommands; import org.team5924.frc2025.commands.elevator.RunElevator; +import org.team5924.frc2025.commands.lights.SetLEDColorCommand; import org.team5924.frc2025.generated.TunerConstantsGamma; +import org.team5924.frc2025.subsystems.Lights.LEDSubsystem; import org.team5924.frc2025.subsystems.climber.Climber; import org.team5924.frc2025.subsystems.climber.ClimberIO; import org.team5924.frc2025.subsystems.climber.ClimberIOSim; @@ -50,7 +53,6 @@ import org.team5924.frc2025.subsystems.elevator.Elevator; import org.team5924.frc2025.subsystems.elevator.ElevatorIO; import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFXGamma; -import org.team5924.frc2025.subsystems.lights.Lights; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO; @@ -73,7 +75,8 @@ public class RobotContainer { private final CoralInAndOut coralInAndOut; private final Elevator elevator; private final Vision vision; - private final Lights lights; + // private final Lights lights; + private final LEDSubsystem ledSubsystem = new LEDSubsystem(CANDLE_ID); // CAN ID = 0 // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -99,7 +102,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIOKrakenFOC()); elevator = new Elevator(new ElevatorIOTalonFXGamma() {}); vision = new Vision(new VisionIOLimelight()); - lights = new Lights(); + // lights = new Lights(); break; case SIM: @@ -115,7 +118,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIOSim()); elevator = new Elevator(new ElevatorIO() {}); vision = new Vision(new VisionIO() {}); - lights = new Lights(); + // lights = new Lights(); break; default: @@ -131,7 +134,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIO() {}); elevator = new Elevator(new ElevatorIO() {}); vision = new Vision(new VisionIO() {}); - lights = new Lights(); + // lights = new Lights(); break; } @@ -322,7 +325,10 @@ private void configureButtonBindings() { .or(driveController.pov(0)) .onFalse(Commands.runOnce(() -> climber.handleNoInputState())); - lights.defaultCommand(); + // lights.defaultCommand(); + ledSubsystem.setDefaultCommand( + new SetLEDColorCommand(ledSubsystem, 0, 255, 0) // default: green + ); } /** diff --git a/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java b/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java new file mode 100644 index 0000000..8ed169f --- /dev/null +++ b/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java @@ -0,0 +1,43 @@ +/* + * SetLEDColorCommand.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.commands.lights; + +import edu.wpi.first.wpilibj2.command.Command; +import org.team5924.frc2025.subsystems.Lights.LEDSubsystem; + +public class SetLEDColorCommand extends Command { + private final LEDSubsystem led; + private final int r, g, b; + + public SetLEDColorCommand(LEDSubsystem led, int r, int g, int b) { + this.led = led; + this.r = r; + this.g = g; + this.b = b; + addRequirements(led); + } + + @Override + public void initialize() { + led.setColor(r, g, b); + } + + @Override + public boolean isFinished() { + return true; // run once and done + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java new file mode 100644 index 0000000..0993c27 --- /dev/null +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java @@ -0,0 +1,32 @@ +/* + * LEDSubsystem.java + */ + +/* + * Copyright (C) 2024-2025 Team 5924 - Golden Gate Robotics and/or its affiliates. + * + * This file, and the associated project, are offered under the GNU General + * Public License v3.0. A copy of this license can be found in LICENSE.md + * at the root of this project. + * + * If this file has been separated from the original project, you should have + * received a copy of the GNU General Public License along with it. + * If you did not, see . + */ + +package org.team5924.frc2025.subsystems.Lights; + +import com.ctre.phoenix.led.CANdle; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class LEDSubsystem extends SubsystemBase { + private final CANdle candle; + + public LEDSubsystem(int canId) { + candle = new CANdle(canId); + } + + public void setColor(int r, int g, int b) { + candle.setLEDs(r, g, b); + } +} diff --git a/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java b/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java index bdb6dd0..84adf13 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java +++ b/src/main/java/org/team5924/frc2025/subsystems/elevator/Elevator.java @@ -52,7 +52,7 @@ public enum ElevatorState { L1(new LoggedTunableNumber("Elevator/L1Height", 0.15)), L2(new LoggedTunableNumber("Elevator/L2Height", 0.245)), L3(new LoggedTunableNumber("Elevator/L3Height", .44)), - L4(new LoggedTunableNumber("Elevator/L4Height", .755)), + L4(new LoggedTunableNumber("Elevator/L4Height", .762)), MOVING(new LoggedTunableNumber("Elevator/MovingHeight", 0)), MANUAL(new LoggedTunableNumber("Elevator/ManualHeight", 0)), STOW(new LoggedTunableNumber("Elevator/StowHeight", -0.01)); diff --git a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java index 7d277ee..8671a91 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java +++ b/src/main/java/org/team5924/frc2025/subsystems/rollers/CoralInAndOut/CoralInAndOut.java @@ -53,7 +53,7 @@ public enum CoralState implements VoltageState { new LoggedTunableNumber("CoralInAndOut/LoadShootMotor/ShootingVoltage", 2.5), new LoggedTunableNumber("CoralInAndOut/HandoffMotor/ShootingVoltage", -2.5)), SHOOTING_L4( - new LoggedTunableNumber("CoralInAndOut/LoadShootMotor/ShootingVoltage", 3.5), + new LoggedTunableNumber("CoralInAndOut/LoadShootMotor/ShootingVoltage", 2.5), new LoggedTunableNumber("CoralInAndOut/HandoffMotor/ShootingVoltage", -2.5)), SHOOTING_L1( new LoggedTunableNumber("CoralInAndOut/LoadShootMotor/ShootingVoltage", 2.0), diff --git a/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java b/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java index fe81b4e..19b232c 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java +++ b/src/main/java/org/team5924/frc2025/subsystems/vision/Vision.java @@ -207,7 +207,7 @@ public double getLatencySecondsBack() { return inputs.backAprilTagCaptureLatencySeconds + inputs.backAprilTagPipelineLatencySeconds; } - public double getLatencySecondsFronRtRight() { + public double getLatencySecondsFrontRight() { return inputs.frontRightAprilTagCaptureLatencySeconds + inputs.frontRightAprilTagPipelineLatencySeconds; } From cf6643bd2f22632c4ca95dbeed64be17df411994 Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Sat, 5 Apr 2025 17:39:13 -0700 Subject: [PATCH 13/14] bellarmine kid assist --- src/main/java/org/team5924/frc2025/BuildConstants.java | 10 +++++----- src/main/java/org/team5924/frc2025/RobotContainer.java | 7 +++++-- .../frc2025/commands/lights/SetLEDColorCommand.java | 2 +- .../frc2025/subsystems/Lights/LEDSubsystem.java | 2 +- 4 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index 8d71b8c..b22f54e 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,12 +21,12 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 288; - public static final String GIT_SHA = "0711cd7c38749360cceb622efd05200c7ac79562"; - public static final String GIT_DATE = "2025-04-05 19:55:15 EDT"; + public static final int GIT_REVISION = 290; + public static final String GIT_SHA = "eaa0e058345c51549742436d7a2f0366d7977cb8"; + public static final String GIT_DATE = "2025-04-05 20:09:14 EDT"; public static final String GIT_BRANCH = "LED-code"; - public static final String BUILD_DATE = "2025-04-05 20:06:48 EDT"; - public static final long BUILD_UNIX_TIME = 1743898008658L; + public static final String BUILD_DATE = "2025-04-05 20:38:08 EDT"; + public static final long BUILD_UNIX_TIME = 1743899888490L; public static final int DIRTY = 1; private BuildConstants() {} diff --git a/src/main/java/org/team5924/frc2025/RobotContainer.java b/src/main/java/org/team5924/frc2025/RobotContainer.java index eae58f2..b41a64e 100644 --- a/src/main/java/org/team5924/frc2025/RobotContainer.java +++ b/src/main/java/org/team5924/frc2025/RobotContainer.java @@ -39,7 +39,6 @@ import org.team5924.frc2025.commands.elevator.RunElevator; import org.team5924.frc2025.commands.lights.SetLEDColorCommand; import org.team5924.frc2025.generated.TunerConstantsGamma; -import org.team5924.frc2025.subsystems.Lights.LEDSubsystem; import org.team5924.frc2025.subsystems.climber.Climber; import org.team5924.frc2025.subsystems.climber.ClimberIO; import org.team5924.frc2025.subsystems.climber.ClimberIOSim; @@ -53,6 +52,7 @@ import org.team5924.frc2025.subsystems.elevator.Elevator; import org.team5924.frc2025.subsystems.elevator.ElevatorIO; import org.team5924.frc2025.subsystems.elevator.ElevatorIOTalonFXGamma; +import org.team5924.frc2025.subsystems.lights.LEDSubsystem; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOut.CoralState; import org.team5924.frc2025.subsystems.rollers.CoralInAndOut.CoralInAndOutIO; @@ -76,7 +76,7 @@ public class RobotContainer { private final Elevator elevator; private final Vision vision; // private final Lights lights; - private final LEDSubsystem ledSubsystem = new LEDSubsystem(CANDLE_ID); // CAN ID = 0 + private final LEDSubsystem ledSubsystem; // Controller private final CommandXboxController driveController = new CommandXboxController(0); @@ -102,6 +102,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIOKrakenFOC()); elevator = new Elevator(new ElevatorIOTalonFXGamma() {}); vision = new Vision(new VisionIOLimelight()); + ledSubsystem = new LEDSubsystem(CANDLE_ID); // lights = new Lights(); break; @@ -118,6 +119,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIOSim()); elevator = new Elevator(new ElevatorIO() {}); vision = new Vision(new VisionIO() {}); + ledSubsystem = new LEDSubsystem(CANDLE_ID); // lights = new Lights(); break; @@ -134,6 +136,7 @@ public RobotContainer() { coralInAndOut = new CoralInAndOut(new CoralInAndOutIO() {}); elevator = new Elevator(new ElevatorIO() {}); vision = new Vision(new VisionIO() {}); + ledSubsystem = new LEDSubsystem(CANDLE_ID); // lights = new Lights(); break; } diff --git a/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java b/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java index 8ed169f..77bc893 100644 --- a/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java +++ b/src/main/java/org/team5924/frc2025/commands/lights/SetLEDColorCommand.java @@ -17,7 +17,7 @@ package org.team5924.frc2025.commands.lights; import edu.wpi.first.wpilibj2.command.Command; -import org.team5924.frc2025.subsystems.Lights.LEDSubsystem; +import org.team5924.frc2025.subsystems.lights.LEDSubsystem; public class SetLEDColorCommand extends Command { private final LEDSubsystem led; diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java index 0993c27..3151e19 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java @@ -14,7 +14,7 @@ * If you did not, see . */ -package org.team5924.frc2025.subsystems.Lights; +package org.team5924.frc2025.subsystems.lights; import com.ctre.phoenix.led.CANdle; import edu.wpi.first.wpilibj2.command.SubsystemBase; From 582d5c9184869895f113b738d265da1fbd5c7143 Mon Sep 17 00:00:00 2001 From: Newton Wong Date: Sun, 6 Apr 2025 06:49:22 -0700 Subject: [PATCH 14/14] nothing works --- .../java/org/team5924/frc2025/BuildConstants.java | 12 ++++++------ .../frc2025/subsystems/Lights/LEDSubsystem.java | 7 ++++++- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/team5924/frc2025/BuildConstants.java b/src/main/java/org/team5924/frc2025/BuildConstants.java index b22f54e..22bf7d0 100644 --- a/src/main/java/org/team5924/frc2025/BuildConstants.java +++ b/src/main/java/org/team5924/frc2025/BuildConstants.java @@ -21,13 +21,13 @@ public final class BuildConstants { public static final String MAVEN_GROUP = ""; public static final String MAVEN_NAME = "GoldenGateRobotics2025"; public static final String VERSION = "unspecified"; - public static final int GIT_REVISION = 290; - public static final String GIT_SHA = "eaa0e058345c51549742436d7a2f0366d7977cb8"; - public static final String GIT_DATE = "2025-04-05 20:09:14 EDT"; + public static final int GIT_REVISION = 291; + public static final String GIT_SHA = "cf6643bd2f22632c4ca95dbeed64be17df411994"; + public static final String GIT_DATE = "2025-04-05 20:39:13 EDT"; public static final String GIT_BRANCH = "LED-code"; - public static final String BUILD_DATE = "2025-04-05 20:38:08 EDT"; - public static final long BUILD_UNIX_TIME = 1743899888490L; - public static final int DIRTY = 1; + public static final String BUILD_DATE = "2025-04-05 21:13:08 EDT"; + public static final long BUILD_UNIX_TIME = 1743901988846L; + public static final int DIRTY = 0; private BuildConstants() {} } diff --git a/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java b/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java index 3151e19..e23f6e5 100644 --- a/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java +++ b/src/main/java/org/team5924/frc2025/subsystems/Lights/LEDSubsystem.java @@ -17,13 +17,18 @@ package org.team5924.frc2025.subsystems.lights; import com.ctre.phoenix.led.CANdle; +import com.ctre.phoenix.led.CANdleConfiguration; + import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class LEDSubsystem extends SubsystemBase { +public class LEDSubsystem extends SubsystemBase { private final CANdle candle; public LEDSubsystem(int canId) { candle = new CANdle(canId); + + CANdleConfiguration config = new CANdleConfiguration(); + } public void setColor(int r, int g, int b) {