From 05d7bd5120e037933d30067c4a62e6c1db421440 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Tue, 14 Jan 2025 20:00:11 -0800 Subject: [PATCH 01/39] Hghhgn --- .wpilib/wpilib_preferences.json | 6 + .../java/org/carlmontrobotics/Config.java | 119 ++ .../java/org/carlmontrobotics/Constants 2.j | 467 +++++++ .../java/org/carlmontrobotics/Constants.java | 190 ++- .../org/carlmontrobotics/RobotContainer 2.j | 488 ++++++++ .../org/carlmontrobotics/RobotContainer.java | 239 +++- .../commands/TeleopDrive.java | 152 +++ .../subsystems/Drivetrain.java | 1090 +++++++++++++++++ 8 files changed, 2705 insertions(+), 46 deletions(-) create mode 100644 .wpilib/wpilib_preferences.json create mode 100644 src/main/java/org/carlmontrobotics/Config.java create mode 100644 src/main/java/org/carlmontrobotics/Constants 2.j create mode 100644 src/main/java/org/carlmontrobotics/RobotContainer 2.j create mode 100644 src/main/java/org/carlmontrobotics/commands/TeleopDrive.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..8d161f6 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "enableCppIntellisense": false, + "currentLanguage": "java", + "projectYear": "2024", + "teamNumber": 199 +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java new file mode 100644 index 0000000..f8a0362 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -0,0 +1,119 @@ +package org.carlmontrobotics; + +import java.lang.reflect.Method; +import java.lang.reflect.Modifier; +import java.util.ArrayList; +import java.util.List; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; + +public abstract class Config implements Sendable { + public static final Config CONFIG = new Config() { + { + // Override config settings here, like this: + // this.exampleFlagEnabled = true; + + // NOTE: PRs with overrides will NOT be merged because we don't want them + // polluting the master branch. + // Feel free to add them when testing, but remove them before pushing. + } + }; + + // Add additional config settings by declaring a protected field, and... + protected boolean exampleFlagEnabled = false; + protected boolean swimShady = false; + protected boolean setupSysId = false; + protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of + // outtake through SmartDashboard + // Note: disables joystick control of arm and + // outtake command if + // using SmartDashboard + + // ...a public getter starting with "is" for booleans or "get" for other types. + // Do NOT remove this example. It is used by unit tests. + + public boolean isExampleFlagEnabled() { + return exampleFlagEnabled; + } + + public boolean isSwimShady() { + return swimShady; + } + + public boolean isSysIdTesting() { + return setupSysId; + } + + public boolean useSmartDashboardControl() { + return useSmartDashboardControl; + } + + // --- For clarity, place additional config settings ^above^ this line --- + + private static class MethodResult { + String methodName = null; + Object retVal = null; + Object defaultRetVal = null; + + MethodResult(String name, Object retVal, Object defaultRetval) { + this.methodName = name; + this.retVal = retVal; + this.defaultRetVal = defaultRetval; + } + } + + private List getMethodResults() { + var methodResults = new ArrayList(); + var defaultConfig = new Config() { + }; + for (Method m : Config.class.getDeclaredMethods()) { + var name = m.getName(); + if (!Modifier.isPublic(m.getModifiers()) || m.isSynthetic() || m.getParameterCount() != 0 + || !name.matches("^(get|is)[A-Z].*")) { + continue; + } + Object retVal = null; + try { + retVal = m.invoke(this); + } catch (Exception ex) { + retVal = ex; + } + Object defaultRetVal = null; + try { + defaultRetVal = m.invoke(defaultConfig); + } catch (Exception ex) { + defaultRetVal = ex; + } + methodResults.add(new MethodResult(name, retVal, defaultRetVal)); + } + return methodResults; + } + + @Override + public void initSendable(SendableBuilder builder) { + getMethodResults().forEach(mr -> { + if (!mr.retVal.equals(mr.defaultRetVal)) { + builder.publishConstString("%s()".formatted(mr.methodName), + String.format("%s (default is %s)", mr.retVal, mr.defaultRetVal)); + } + }); + } + + @Override + public String toString() { + StringBuilder stringBuilder = new StringBuilder(); + getMethodResults().forEach(mr -> { + if (!mr.retVal.equals(mr.defaultRetVal)) { + stringBuilder.append( + String.format("%s() returns %s (default is %s)", mr.methodName, mr.retVal, mr.defaultRetVal)); + } + }); + if (stringBuilder.isEmpty()) { + stringBuilder.append("Using default config values"); + } else { + stringBuilder.insert(0, "WARNING: USING OVERRIDDEN CONFIG VALUES\n"); + } + return stringBuilder.toString(); + } +} diff --git a/src/main/java/org/carlmontrobotics/Constants 2.j b/src/main/java/org/carlmontrobotics/Constants 2.j new file mode 100644 index 0000000..2b72303 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/Constants 2.j @@ -0,0 +1,467 @@ + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics; + +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.button.POVButton; +import edu.wpi.first.wpilibj.util.Color; + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the + * constants are needed, to reduce verbosity. + */ +public final class Constants { + public static final double g = 9.81; // meters per second squared + public static final class Led { + + public static final Color8Bit DEFAULT_COLOR_BLUE = new Color8Bit(0, 0, 200); + public static final Color8Bit DETECT_NOTE_YELLOW = new Color8Bit(255, 255, 0); + public static final Color8Bit HOLDING_GREEN = new Color8Bit(0, 250, 0); + public static final Color8Bit RED_NEO_550_MOTOR = new Color8Bit(255,0,0); + public static final int ledPort = 0; + // TODO: figure out how to get port of LED, it could be 0 or + } + + public static final class Effectorc { + // PID values + + public static final int INTAKE = 0; + public static final int OUTTAKE = 1; + // 0.0001184 + + /* + * public static final double kP = 0.0001067; public static final double kI = 0; public + * static final double kD = 0; public static final double kS = 0; public static final double + * kV = 0.11124; public static final double kA = 0.039757; + */ + + public static final double[] kP = {0, 0.0001067}; + public static final double[] kI = { /* /Intake/ */0, /* /Outake/ */0 }; + public static final double[] kD = { /* /Intake/ */0, /* /Outake/ */0 }; + public static final double[] kS = { /* /Intake/ */0.22, /* /Outake/ */0.29753 * 2 }; + public static final double[] kV = {0.122, 0.11124}; + public static final double[] kA = {0, 0.039757}; + public static final int INTAKE_PORT = 9; // port + public static final int OUTTAKE_PORT = 10; // port + public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port + public static final int OUTAKE_DISTANCE_SENSOR_PORT = 10; // port + public static final double DISTANCE_BETWEEN_SENSORS_INCHES = 8.189; // inches + public static final double OFFSET_FROM_GROUND_INCHES = 21; // in + public static final double DS_DEPTH_INCHES = 9.97; // Distance sensor Depth + public static final double DETECT_DISTANCE_INCHES = 13; + + public static final double INTAKE_RPM = 6300; + public static final double INTAKE_SLOWDOWN_RPM = 4500; + public static final double MAX_SECONDS_OVERLOAD = 2.0; + public static final double PASS_RPM = 2000; + public static final double TEST_RPM = 3000; + public static final double AMP_RPM = 1000; + public static final double SUBWOOFER_RPM = 2100; + public static final double PODIUM_RPM = 4000; + public static final double SPEAKER_RPM = 2100; + // WTF FAB ISSUE + //public static final double SAFE_RPM = 6000;// WTF FAB ISSUE + + public static final double EJECT_RPM_INTAKE = -2550; + public static final double EJECT_RPM_OUTAKE = -2550; + + public static final double MANUAL_RPM_MAX = 9500; + + public static final double RPM_TOLERANCE = 200; + public static final double SPEAKER_HEIGHT_INCHES = 83; // inches + + public static final boolean INTAKE_MOTOR_INVERSION = true; + public static final boolean OUTAKE_MOTOR_INVERSION = false; + + public static final double EJECT_TIME_SECS = 5.; + public static final double EJECT_MIN_SECS = 1.25; + public static final double INTAKE_TIME_SECS = 4.; + public static final double ledDefaultColorRestoreTime = 3; + public static final Color defaultColor = new Color(0, 0, 200); + public static final Color pickupSuccessColor = new Color(0, 200, 0); + + } + + public static final class Armc { + + // Motor port + public static final int ARM_MOTOR_PORT_MASTER = + CONFIG.isSwimShady() ? 20 : 13; + public final static int ARM_MOTOR_PORT_FOLLOWER = 18; + // Config for motors + public static final boolean MOTOR_INVERTED_MASTER = false; + public static final boolean MOTOR_INVERTED_FOLLOWER = true; // verifyed by design AND physical testing + + public static final double ROTATION_TO_RAD = 2 * Math.PI; + public static final boolean ENCODER_INVERTED = true; + + // difference between CoG and arm is .328 rad + public static final double ENCODER_OFFSET_RAD = 6.132; + // 0.08 + .328 - 0.404; // - 0.6095; + + // TODO: finish understand why this is broken public static final Measure + // INTAKE_ANGLE = Degrees.to(-1); + + // USE RADIANS FOR THE ARM + public static final double INTAKE_ANGLE_RAD = Units.degreesToRadians(0); + public static final double HANG_ANGLE_RAD = Units.degreesToRadians(90); + public static final double AMP_ANGLE_RAD = 1.28; + public static final double AMP_ANGLE_RAD_NEW_MOTOR = 1.456; + public static final double SPEAKER_ANGLE_RAD = -0.2; + public static final double PODIUM_ANGLE_RAD = -0.2 + Units.degreesToRadians(7.5); + // -0.427725 + public static final double GROUND_INTAKE_POS = -0.34537; + + public static final double HANG_ANGL_RAD = GROUND_INTAKE_POS + Units.degreesToRadians(30); + + public static final double SUBWOOFER_ANGLE_RAD = -0.22;// touching the base of the speaker + public static final double SAFE_ZONE_ANGLE_RAD = Units.degreesToRadians(36);// touching the white line + //public static final double PODIUM_ANGLE_RAD = Units.degreesToRadians(40);// touching the safe pad on the stage + + // 0.4 rad for shooting at podium + + // PID, Feedforward, Trapezoid + public static final double kP = 0.45;// 5.7938; // (2 * Math.PI); + public static final double kI = 0; + public static final double kD = 0 * 1000; + public static final double kS = 1.6 / 2; // 0.1498; + public static final double kG = 0.8067; // 0.3489; + public static final double kV = 5.1201 / (2 * Math.PI); + public static final double kA = 0.43308 / (2 * Math.PI); + public static final double IZONE_RAD = 0; + // fine for now, change it later before use - ("Incorect use of setIZone()" + // Issue #22) + // public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s + public static final double MAX_FF_VEL_RAD_P_S = Math.PI * .5; + public static final double MAX_FF_ACCEL_RAD_P_S = 53.728 / 4; // rad / s^2 + // ((.89*2)/(1.477/(61.875^2))/61.875)-20.84 + + public static final double MAX_FF_VEL_RAD_P_S_BABY = 0; + public static final double MAX_FF_ACCEL_RAD_P_S_BABY = 0; + //TODO: determine these values^ + + + public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0; + public static final double CLIMB_POS = 1.701; //RADIANS + public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12); + public static final double MAX_VOLTAGE = 0.5; // (kS + kG + 1)/12; + public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE/12 *0.7; + public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE/12*0.7; + public static final double CLIMB_FINISH_POS = -0.38; + // if needed + public static final double COM_ARM_LENGTH_METERS = 0.381; + public static final double ARM_MASS_KG = 9.59302503; + + public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS;// initalized by arm constructor + // other0; + + // public static final double MARGIN_OF_ERROR = Math.PI / 18; v^2 /a + + // Boundaries + public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder + public static final double POS_TOLERANCE_RAD = + Units.degreesToRadians(5); // placeholder //Whether or not this is the actual + // account + // idk TODO: test on actual encoder without a conversion + // factor + public static final double VEL_TOLERANCE_RAD_P_SEC = (POS_TOLERANCE_RAD / 0.02); // 20ms per robot loop + public static final double UPPER_ANGLE_LIMIT_RAD = 1.63; + public static final double LOWER_ANGLE_LIMIT_RAD = -0.5; + public static final double CLIMBER_UP_ANGLE_RAD = UPPER_ANGLE_LIMIT_RAD; + public static final double CLIMBER_DOWN_ANGLE_RAD = LOWER_ANGLE_LIMIT_RAD; + + public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; + + public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = 0.5; + public static final double SMART_CURRENT_LIMIT_TIMEOUT = 0.8; + + + public static final int NEO_BUILTIN_ENCODER_CPR = 42; + } + + public static final class Drivetrainc { + + // #region Subsystem Constants + + public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) + : Units.inchesToMeters(16.75); + public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) + : Units.inchesToMeters(23.75); + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); + // The gearing reduction from the drive motor controller to the wheels + // Gearing for the Swerve Modules is 6.75 : 1 + public static final double driveGearing = 6.75; + // Turn motor shaft to "module shaft" + public static final double turnGearing = 150 / 7; + + public static final double driveModifier = 1; + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical + * correction + */; + public static final double mu = 1; /* 70/83.2; */ + + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + // Angular speed to translational speed --> v = omega * r / gearing + public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; + public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + // seconds it takes to go from 0 to 12 volts(aka MAX) + public static final double secsPer12Volts = 0.1; + + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; + + public static final boolean[] reversed = { false, false, false, false }; + // public static final boolean[] reversed = {true, true, true, true}; + // Determine correct turnZero constants (FL, FR, BL, BR) + public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } + : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } + : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ + + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. + // Determine correct turn PID constants + public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, + // 0.00374}; + public static final double[] turnkI = { 0, 0, 0, 0 }; + public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d + // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; + + // V = kS + kV * v + kA * a + // 12 = 0.2 + 0.00463 * v + // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s + public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 }; + public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 }; + + // kP is an average of the forward and backward kP values + // Forward: 1.72, 1.71, 1.92, 1.94 + // Backward: 1.92, 1.92, 2.11, 1.89 + // Order of modules: (FL, FR, BL, BR) + public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, + // 1.915/100}; + public static final double[] drivekI = { 0, 0, 0, 0 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isSwimShady() + ? new boolean[] { false, false, false, false } + : new boolean[] { true, false, true, false }); + public static final boolean[] turnInversion = { true, true, true, true }; + // kS + public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kBackwardVolts = kForwardVolts; + + public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kBackwardVels = kForwardVels; + public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kBackwardAccels = kForwardAccels; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * g * 2; + + public static final boolean isGyroReversed = true; + + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } + : new double[] { 2, 0.0, 0.0 }; + public static final double[] yPIDController = xPIDController; + public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } + : new double[] {0.05, 0.0, 0.00}; + + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, + driveInversion, reversed, driveModifier, turnInversion); + + // public static final Limelight.Transform limelightTransformForPoseEstimation = + // Transform.BOTPOSE_WPIBLUE; + + // #endregion + + // #region Ports + + public static final int driveFrontLeftPort = CONFIG.isSwimShady() ? 8 : 11; // + public static final int driveFrontRightPort = CONFIG.isSwimShady() ? 13 : 19; // + public static final int driveBackLeftPort = CONFIG.isSwimShady() ? 5 : 14; // + public static final int driveBackRightPort = CONFIG.isSwimShady() ? 11 : 17; // correct + + public static final int turnFrontLeftPort = CONFIG.isSwimShady() ? 7 : 12; // + public static final int turnFrontRightPort = CONFIG.isSwimShady() ? 14 : 20; // 20 + public static final int turnBackLeftPort = CONFIG.isSwimShady() ? 6 : 15; // + public static final int turnBackRightPort = CONFIG.isSwimShady() ? 12 : 16; // correct + + public static final int canCoderPortFL = CONFIG.isSwimShady() ? 4 : 0; + public static final int canCoderPortFR = CONFIG.isSwimShady() ? 2 : 3; + public static final int canCoderPortBL = CONFIG.isSwimShady() ? 3 : 2; + public static final int canCoderPortBR = CONFIG.isSwimShady() ? 1 : 1; + + // #endregion + + // #region Command Constants + + public static double kNormalDriveSpeed = 1; // Percent Multiplier + public static double kNormalDriveRotation = 0.5; // Percent Multiplier + public static double kSlowDriveSpeed = 0.4; // Percent Multiplier + public static double kSlowDriveRotation = 0.250; // Percent Multiplier + + //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use + public static double kBabyDriveSpeed = 0.3; + public static double kBabyDriveRotation = 0.2; + public static double kAlignMultiplier = 1D / 3D; + public static final double kAlignForward = 0.6; + + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to + // orient the wheels to the correct angle. This + // should be very small to avoid actually moving the + // robot. + + public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, + // Meters, + // Degrees + public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, + // Meters, + // Degrees/Second + + // #endregion + // #region Subsystem Constants + + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; + public static final double turnIzone = .1; + + public static final double driveIzone = .1; + + public static final class Autoc { + public static final ReplanningConfig replanningConfig = new ReplanningConfig( /* + * put in + * Constants.Drivetrain.Auto + */ + false, // replan at start of path if robot not at start of path? + false, // replan if total error surpasses total error/spike threshold? + 1.5, // total error threshold in meters that will cause the path to be replanned + .8 // error spike threshold, in meters, that will cause the path to be replanned + ); + public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, + 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the + // angular constraints have no effect. + } + } + + public static final class Limelightc { + public static final String INTAKE_LL_NAME = "limelight-intake"; + public static final String SHOOTER_LL_NAME = "limelight-shooter"; + + public static final int[] VALID_IDS = { 4, 7 }; + + public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high + // to represent unreliable heading + public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity + + public static final double ERROR_TOLERANCE_RAD = 0.1; + public static final double HORIZONTAL_FOV_DEG = 0; // unused + public static final double RESOLUTION_WIDTH_PIX = 640; // unused + public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; + public static final double MOUNT_ANGLE_DEG_INTAKE = -29; + public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); + public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = + Units.inchesToMeters(13); + public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused + public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused + public static final double MIN_MOVEMENT_METERSPSEC = 1.5; + public static final double MIN_MOVEMENT_RADSPSEC = 0.5; + public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); + public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); + public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); + public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused + public static final class Apriltag { + public static final int RED_SPEAKER_CENTER_TAG_ID = 4; + public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; + public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); + public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); + public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); + } + } + + public static final class OI { + public static final class Driver { + public static final int port = 0; + + public static final int slowDriveButton = Button.kLeftBumper.value; + public static final int resetFieldOrientationButton = Button.kRightBumper.value; + public static final int toggleFieldOrientedButton = Button.kStart.value; + + public static final int rotateFieldRelative0Deg = Button.kY.value; + public static final int rotateFieldRelative90Deg = Button.kB.value; + public static final int rotateFieldRelative180Deg = Button.kA.value; + public static final int rotateFieldRelative270Deg = Button.kX.value; + } + + public static final class Manipulator { + public static final int port = 1; + // NEW BINDINGS(easier for manipulator) + // Xbox left joy Y axis -> raw Intake control + // Xbox right joy Y axis -> raw Outtake control + // Xbox right trigger axis -> Intake pos + intake + // Xbox left trigger axis -> amp pos , eject into amp + // Xbox left bumper button -> CLOSE Speaker pos , Fire + // Xbox right bumper button -> SAFE Speaker pos , Fire + // Xbox X button -> goto Intake pos + // Xbox Y button -> Eject rpm + public static final Axis INTAKE_BUTTON = Axis.kLeftTrigger; + public static final Axis SHOOTER_BUTTON = Axis.kRightTrigger; + public static final int EJECT_BUTTON = Button.kLeftBumper.value; + public static final int OPPOSITE_EJECT = Button.kRightBumper.value; + public static final Axis INTAKE_AX = Axis.kRightTrigger; + public static final Axis AMP_AX = Axis.kLeftTrigger; + public static final int SPEAKER_POS = Button.kX.value; + public static final int ARM_TO_AMP_BUTTON = Button.kY.value; + public static final int PASS_TO_OUTTAKE_STICK = + Button.kLeftStick.value; + public static final int PASS_TO_INTAKE_STICK = + Button.kRightStick.value; + public static final int UP_D_PAD = 0; + public static final int DOWN_D_PAD = 180; + public static final int LEFT_D_PAD = 270; + + public static final int A_BUTTON = Button.kA.value; + public static final int RAMP_OUTTAKE = Button.kB.value; + } + + + public static final double JOY_THRESH = 0.01; + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. + + } +} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..c3776c3 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,15 +1,185 @@ + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + package org.carlmontrobotics; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; +import edu.wpi.first.wpilibj.util.Color; +import edu.wpi.first.wpilibj.util.Color8Bit; +import edu.wpi.first.wpilibj2.command.button.POVButton; +import edu.wpi.first.wpilibj.util.Color; + + +/** + * The Constants class provides a convenient place for teams to hold robot-wide + * numerical or boolean + * constants. This class should not be used for any other purpose. All constants + * should be declared + * globally (i.e. public static). Do not put anything functional in this class. + * + *

+ * It is advised to statically import this class (or one of its inner classes) + * wherever the + * constants are needed, to reduce verbosity. + */ public final class Constants { - // public static final class Drivetrain { - // public static final double MAX_SPEED_MPS = 2; - // } - public static final class OI { - public static final class Driver { - public static final int port = 0; - } - public static final class Manipulator { - public static final int port = 1; + public static final double g = 9.81; // meters per second squared + public static final class Led { + + } + + public static final class Effectorc { + + } + + public static final class Armc { + + + } + + public static final class Drivetrainc { + + // #region Subsystem Constants + + public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) + : Units.inchesToMeters(16.75); + public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) + : Units.inchesToMeters(23.75); + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); + // The gearing reduction from the drive motor controller to the wheels + // Gearing for the Swerve Modules is 6.75 : 1 + public static final double driveGearing = 6.75; + // Turn motor shaft to "module shaft" + public static final double turnGearing = 150 / 7; + + public static final double driveModifier = 1; + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical + * correction + */; + public static final double mu = 1; /* 70/83.2; */ + + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + // Angular speed to translational speed --> v = omega * r / gearing + public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; + public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + // seconds it takes to go from 0 to 12 volts(aka MAX) + public static final double secsPer12Volts = 0.1; + + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; + + public static final boolean[] reversed = { false, false, false, false }; + // public static final boolean[] reversed = {true, true, true, true}; + // Determine correct turnZero constants (FL, FR, BL, BR) + public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } + : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } + : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ + + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. + // Determine correct turn PID constants + public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, + // 0.00374}; + public static final double[] turnkI = { 0, 0, 0, 0 }; + public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d + // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; + + // V = kS + kV * v + kA * a + // 12 = 0.2 + 0.00463 * v + // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s + public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 }; + public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 }; + + // kP is an average of the forward and backward kP values + // Forward: 1.72, 1.71, 1.92, 1.94 + // Backward: 1.92, 1.92, 2.11, 1.89 + // Order of modules: (FL, FR, BL, BR) + public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, + // 1.915/100}; + public static final double[] drivekI = { 0, 0, 0, 0 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isSwimShady() + ? new boolean[] { false, false, false, false } + : new boolean[] { true, false, true, false }); + public static final boolean[] turnInversion = { true, true, true, true }; + // kS + public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kBackwardVolts = kForwardVolts; + + public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kBackwardVels = kForwardVels; + public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kBackwardAccels = kForwardAccels; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * g * 2; + + public static final boolean isGyroReversed = true; + + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } + : new double[] { 2, 0.0, 0.0 }; + public static final double[] yPIDController = xPIDController; + public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } + : new double[] {0.05, 0.0, 0.00}; + + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, + driveInversion, reversed, driveModifier, turnInversion); + + public static final class Autoc { + } + } + + public static final class Limelightc { + + public static final class Apriltag { + + } + } + + public static final class OI { + public static final class Driver { + public static final int port = 0; + + public static final int slowDriveButton = Button.kLeftBumper.value; + public static final int resetFieldOrientationButton = Button.kRightBumper.value; + public static final int toggleFieldOrientedButton = Button.kStart.value; + + public static final int rotateFieldRelative0Deg = Button.kY.value; + public static final int rotateFieldRelative90Deg = Button.kB.value; + public static final int rotateFieldRelative180Deg = Button.kA.value; + public static final int rotateFieldRelative270Deg = Button.kX.value; + } + + public static final class Manipulator { + public static final int port = 1; } - } + + public static final double JOY_THRESH = 0.01; + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. + + } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer 2.j b/src/main/java/org/carlmontrobotics/RobotContainer 2.j new file mode 100644 index 0000000..ebddde9 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/RobotContainer 2.j @@ -0,0 +1,488 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics; + +// java +import java.util.ArrayList; +import java.util.List; + +// constants +import static org.carlmontrobotics.Constants.Armc.*; +import static org.carlmontrobotics.Constants.OI.Manipulator.*; +import static org.carlmontrobotics.Constants.Effectorc.*; +import static org.carlmontrobotics.Constants.Limelightc.*; + +// non static constants +import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Driver; +import org.carlmontrobotics.Constants.OI.Manipulator; +import org.carlmontrobotics.Constants.Armc; +import org.carlmontrobotics.Constants.Drivetrainc.Autoc; +import org.carlmontrobotics.Constants.Effectorc; +// robotcode2024 imports +import org.carlmontrobotics.commands.*; +import org.carlmontrobotics.subsystems.*; + +import com.pathplanner.lib.auto.AutoBuilder; +// pathplanner +import com.pathplanner.lib.auto.NamedCommands; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; + +// wpilib geometry classes +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; +// controllers +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; + +// dashboards +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +// commands +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +// control bindings +import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; + +public class RobotContainer { + private static boolean babyMode = false; + + // 1. using GenericHID allows us to use different kinds of controllers + // 2. Use absolute paths from constants to reduce confusion + public final GenericHID driverController = new GenericHID(Driver.port); + public final GenericHID manipulatorController = new GenericHID(Manipulator.port); + private final IntakeShooter intakeShooter = new IntakeShooter(); + + // ignore warning, LED must be initialized + private final Led led = new Led(intakeShooter); + private final Arm arm = new Arm(); + private final Drivetrain drivetrain = new Drivetrain(); + private final Limelight limelight = new Limelight(drivetrain); + + /* These are assumed to be equal to the AUTO ames in pathplanner */ + /* These must be equal to the pathPlanner path names from the GUI! */ + // Order matters - but the first one is index 1 on the physical selector - index + // 0 is reserved for + // null command. + // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 + // is simple + // straight + private List autoCommands; + private SendableChooser autoSelector = new SendableChooser(); + + private boolean hasSetupAutos = false; + private final String[] autoNames = new String[] { + /* These are assumed to be equal to the AUTO ames in pathplanner */ + "Left-Auto Ruiner", "Center-Auto Ruiner", "Right-Auto Ruiner", + "Right Limelight 4 Piece", "Left Limelight 4 Piece", + "Center Limelight 4 Piece", + + "Left-Amp", + + "Center Limelight 1 Piece", "Left Limelight 1 Piece", + "Right Limelight 1 Piece", "Center Field Limelight", + "Center Field Limelight(No Preload)", "Center Forward", + "Right Forward", "Left Forward", + "Backup-Center", "Backup-Right", "Backup-Left", + "Preload1Center", "Preload1Right", "Preload1Left", + + }; + DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; + + public RobotContainer() { + { + // Put any configuration overrides to the dashboard and the terminal + SmartDashboard.putData("CONFIG overrides", Config.CONFIG); + System.out.println(Config.CONFIG); + + SmartDashboard.putData("BuildConstants", BuildInfo.getInstance()); + + SmartDashboard.setDefaultBoolean("babymode", babyMode); + SmartDashboard.setPersistent("babymode"); + // safe auto setup... stuff in setupAutos() is not safe to run here - will break + // robot + registerAutoCommands(); + SmartDashboard.putData(autoSelector); + SmartDashboard.setPersistent("SendableChooser[0]"); + + autoSelector.addOption("Nothing", 0); + autoSelector.addOption("Raw Forward", 1); + autoSelector.addOption("PP Simple Forward", 2);// index corresponds to index in autoCommands[] + + int i = 3; + for (String n : autoNames) { + autoSelector.addOption(n, i); + i++; + } + + ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); + autoSelectorTab.add(autoSelector).withSize(2, 1); + } + + setDefaultCommands(); + setBindingsDriver(); + // setBindingsManipulatorENDEFF(); + setBindingsManipulator(); + } + + private void setDefaultCommands() { + drivetrain.setDefaultCommand(new TeleopDrive(drivetrain, + () -> ProcessedAxisValue(driverController, Axis.kLeftY), + () -> ProcessedAxisValue(driverController, Axis.kLeftX), + () -> ProcessedAxisValue(driverController, Axis.kRightX), + () -> driverController.getRawButton(Driver.slowDriveButton))); + // TODO: Are we going to use default command for intakeshooter? + intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter, + () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), + manipulatorController, driverController)); + // TODO + // intakeShooter.setDefaultCommand(new RampMaxRPMDriving(intakeShooter)); + + arm.setDefaultCommand( + Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm) + : new TeleopArm(arm, + () -> ProcessedAxisValue(manipulatorController, + Axis.kLeftY))); + + } + + private void setBindingsDriver() { + new JoystickButton(driverController, Driver.resetFieldOrientationButton) + .onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); + // axisTrigger(driverController, Axis.kRightTrigger) + // .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), + // new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight))); + + new POVButton(driverController, 0) + .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter), + new AutoMATICALLYGetNote(drivetrain, limelight, + intakeShooter, 1))); + + axisTrigger(driverController, Axis.kLeftTrigger) + // .onTrue(new AlignToApriltag(drivetrain, limelight)); + .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false))) + .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true))); + + axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) + .whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0)); + new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue( + new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); + new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(270), + drivetrain)); + new JoystickButton(driverController, Driver.rotateFieldRelative180Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(180), + drivetrain)); + new JoystickButton(driverController, Driver.rotateFieldRelative270Deg) + .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), + drivetrain)); + } + + private void setBindingsManipulator() { + new JoystickButton(manipulatorController, EJECT_BUTTON) + .onTrue(new Eject(intakeShooter)); + + // new JoystickButton(manipulatorController, A_BUTTON) + // .onTrue(new RampMaxRPMDriving(intakeShooter)); + // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( + // new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), + // new PassToOuttake(intakeShooter))); + + axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( + new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), + new PassToOuttake(intakeShooter)), new InstantCommand(() -> { + }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME))); + + // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) + // .whileTrue(new PassToOuttake(intakeShooter)); + + // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) + // .whileTrue(new AimArmSpeaker(arm, limelight)); + + new JoystickButton(manipulatorController, RAMP_OUTTAKE) + .whileTrue(new RampMaxRPM(intakeShooter)); + new JoystickButton(manipulatorController, OPPOSITE_EJECT) + .whileTrue(new EjectOuttakeSide(intakeShooter)); + + /* + * axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) + * .onTrue(Config.CONFIG.useSmartDashboardControl + * ? new TestRPM(intakeShooter) + * : new Outtake(intakeShooter, arm) + */ + + // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) + // .onTrue(new PassToOuttake(intakeShooter)); + + axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) + .whileTrue(new Intake(intakeShooter)); + new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON) + .onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR)); + new JoystickButton(manipulatorController, A_BUTTON) + .onTrue(new ArmToPos(arm, GROUND_INTAKE_POS)); + new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK) + .onTrue(new PassToOuttake(intakeShooter)); + new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK) + .onTrue(new PassToIntake(intakeShooter)); + new JoystickButton(manipulatorController, SPEAKER_POS) + .onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD)); + new POVButton(manipulatorController, UP_D_PAD) + .onTrue(new ArmToPos(arm, CLIMB_POS)); + new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm)); + new POVButton(manipulatorController, LEFT_D_PAD) + .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD)); + + } + + /** + * Flips an axis' Y coordinates upside down, but only if the select axis is a + * joystick axis + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double getStickValue(GenericHID hid, Axis axis) { + return hid.getRawAxis(axis.value) + * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); + } + + /** + * Processes an input from the joystick into a value between -1 and 1, + * sinusoidally instead of + * linearly + * + * @param value The value to be processed. + * @return The processed value. + */ + private double inputProcessing(double value) { + double processedInput; + // processedInput = + // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); + processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) + * ((1 - Math.cos(value * Math.PI)) / 2), value); + return processedInput; + } + + /** + * Combines both getStickValue and inputProcessing into a single function for + * processing joystick + * outputs + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double ProcessedAxisValue(GenericHID hid, Axis axis) { + return DeadzonedAxis(inputProcessing(getStickValue(hid, axis))); + } + + /** + * Returns zero if a axis input is inside the deadzone + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double DeadzonedAxis(double axOut) { + return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut; + } + + /** + * Returns a new instance of Trigger based on the given Joystick and Axis + * objects. The Trigger is + * triggered when the absolute value of the stick value on the specified axis + * exceeds a minimum + * threshold value. + * + * @param stick The Joystick object to retrieve stick value from. + * @param axis The Axis object to retrieve value from the Joystick. + * @return A new instance of Trigger based on the given Joystick and Axis + * objects. * @throws + * NullPointerException if either stick or axis is null. + */ + private Trigger axisTrigger(GenericHID controller, Axis axis) { + return new Trigger(() -> Math + .abs(getStickValue(controller, axis)) > OI.MIN_AXIS_TRIGGER_VALUE); + } + + private void registerAutoCommands() { + //// AUTO-USABLE COMMANDS + NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); + NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); + + // NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm, + // Armc.SPEAKER_ANGLE_RAD, 0)); + NamedCommands.registerCommand("ArmToAmp", + new ArmToPos(arm, Armc.AMP_ANGLE_RAD)); + NamedCommands.registerCommand("ArmToSubwoofer", + new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD)); + NamedCommands.registerCommand("ArmToPodium", + new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD)); + NamedCommands.registerCommand("ArmToGround", + new ArmToPos(arm, GROUND_INTAKE_POS)); + + NamedCommands.registerCommand("RampRPMAuton", + new RampRPMAuton(intakeShooter)); + + NamedCommands.registerCommand("SwitchRPMShoot", + new Outtake(intakeShooter, arm)); + + // NamedCommands.registerCommand("AutonRuinerShoot", new + // AutonRuinerShoot(intakeShooter)); + // NamedCommands.registerCommand("IntakeAutonRuiner", new + // IntakeAutonRuiner(intakeShooter)); + + NamedCommands.registerCommand("AutonRuinerShootAndIntake", + new AutonRuinerShootAndIntake(intakeShooter, arm)); + + NamedCommands.registerCommand("PassToOuttake", + new PassToOuttake(intakeShooter)); + NamedCommands.registerCommand("AimArmSpeakerMT2", + new AimArmSpeaker(arm, limelight)); + NamedCommands.registerCommand("AlignToAprilTagMegaTag2", + new AlignToApriltag(drivetrain, limelight, 0.0)); + NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( + new ParallelDeadlineGroup( + new WaitCommand(3.0), + new SequentialCommandGroup( + // TODO: Use Align To Drivetrain + // new AlignDrivetrain(drivetrain), + new ParallelCommandGroup( + new AlignToApriltag(drivetrain, limelight, 0.0), + new AimArmSpeaker(arm, limelight), + new RampRPMAuton(intakeShooter)), + new PassToOuttake(intakeShooter), + new ArmToPos(arm, GROUND_INTAKE_POS))))); + NamedCommands.registerCommand("ShootSubwoofer", + new SequentialCommandGroup(new ParallelCommandGroup( + new ArmToPos(arm, + Armc.SUBWOOFER_ANGLE_RAD), + new RampRPMAuton(intakeShooter)), + new PassToOuttake(intakeShooter), + new ArmToPos(arm, GROUND_INTAKE_POS))); + NamedCommands.registerCommand("Limelight Intake CCW", + new ParallelCommandGroup(new Intake(intakeShooter), + new AutoMATICALLYGetNote(drivetrain, limelight, + intakeShooter, 1))); + NamedCommands.registerCommand("Limelight Intake CW", + new ParallelCommandGroup(new Intake(intakeShooter), + new AutoMATICALLYGetNote(drivetrain, limelight, + intakeShooter, -1))); + + NamedCommands.registerCommand("Limelight Intake Straight", + new ParallelCommandGroup(new Intake(intakeShooter), + new AutoMATICALLYGetNote(drivetrain, limelight, + intakeShooter, 0))); + + NamedCommands.registerCommand("StopIntake", + new InstantCommand(intakeShooter::stopIntake)); + NamedCommands.registerCommand("StopOutake", + new InstantCommand(intakeShooter::stopOuttake)); + NamedCommands.registerCommand("StopBoth", + new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake), + new InstantCommand(intakeShooter::stopOuttake))); + } + + private void setupAutos() { + //// CREATING PATHS from files + if (!hasSetupAutos) { + autoCommands=new ArrayList();//clear old/nonexistent autos + + for (int i = 0; i < autoNames.length; i++) { + String name = autoNames[i]; + + autoCommands.add(new PathPlannerAuto(name)); + + /* + * // Charles' opinion: we shouldn't have it path find to the starting pose at the start of match + * new SequentialCommandGroup( + * AutoBuilder.pathfindToPose( + * PathPlannerAuto.getStaringPoseFromAutoFile(name), + * PathPlannerAuto.getPathGroupFromAutoFile(name).get(0). + * getPreviewStartingHolonomicPose(), + * Autoc.pathConstraints), + * new PathPlannerAuto(name)); + */ + } + hasSetupAutos = true; + + // NOTHING + autoCommands.add(0, new PrintCommand("Running NULL Auto!")); + // RAW FORWARD command + autoCommands.add(1, new SequentialCommandGroup( + new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new WaitCommand(0.5), + new LastResortAuto(drivetrain))); + // dumb PP forward command + autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!")); + } + // force regeneration each auto call + autoCommands.set(2, constructPPSimpleAuto());//overwrite this slot each time auto runs + } + + public Command constructPPSimpleAuto() { + /** + * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd + * (m/s): 4.30 + * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max + * AngAccel: 360 + * (guesses!) + */ + // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER + Pose2d currPos = drivetrain.getPose(); + + //FIXME running red PP file autos seems to break something, so the robot drivetrain drives in the wrong direction. + //running blue PP autos is fine though + //Note: alliance detection and path generation work correctly! + //Solution: Redeploy after auto. + Pose2d endPos = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) + ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) + : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); + + List bezierPoints = PathPlannerPath.bezierFromPoses(currPos, endPos); + + // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, rad/s^2 */ + PathPlannerPath path = new PathPlannerPath(bezierPoints, + Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); + + path.preventFlipping = false;// don't flip, we do that manually already. + + return new SequentialCommandGroup( + new InstantCommand(()->drivetrain.drive(-.0001, 0, 0)),//align drivetrain wheels. + AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); + } + + public Command getAutonomousCommand() { + setupAutos(); + + Integer autoIndex = autoSelector.getSelected(); + + if (autoIndex != null && autoIndex != 0) { + new PrintCommand("Running selected auto: " + autoSelector.toString()); + return autoCommands.get(autoIndex.intValue()); + } + return new PrintCommand("No auto :("); + } + +} diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 836869c..bfd496c 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -5,10 +5,15 @@ package org.carlmontrobotics; //199 files -// import org.carlmontrobotics.subsystems.*; -// import org.carlmontrobotics.commands.*; +import org.carlmontrobotics.subsystems.*; +import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; +import java.io.ObjectInputFilter.Config; +import java.sql.Driver; +import java.util.ArrayList; +import java.util.List; + //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -26,13 +31,58 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; public class RobotContainer { + private static boolean babyMode = false; + + // 1. using GenericHID allows us to use different kinds of controllers + // 2. Use absolute paths from constants to reduce confusion + public final GenericHID driverController = new GenericHID(Driver.port); + public final GenericHID manipulatorController = new GenericHID(Manipulator.port); + + private final Drivetrain drivetrain = new Drivetrain(); - //1. using GenericHID allows us to use different kinds of controllers - //2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(OI.Driver.port); - public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.port); + /* These are assumed to be equal to the AUTO ames in pathplanner */ + /* These must be equal to the pathPlanner path names from the GUI! */ + // Order matters - but the first one is index 1 on the physical selector - index + // 0 is reserved for + // null command. + // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 + // is simplePz + // straight + private List autoCommands; + private SendableChooser autoSelector = new SendableChooser(); + + private boolean hasSetupAutos = false; + private final String[] autoNames = new String[] {}; public RobotContainer() { + { + // Put any configuration overrides to the dashboard and the terminal + SmartDashboard.putData("CONFIG overrides", Config.CONFIG); + System.out.println(Config.CONFIG); + + SmartDashboard.putData("BuildConstants", BuildInfo.getInstance()); + + SmartDashboard.setDefaultBoolean("babymode", babyMode); + SmartDashboard.setPersistent("babymode"); + // safe auto setup... stuff in setupAutos() is not safe to run here - will break + // robot + registerAutoCommands(); + SmartDashboard.putData(autoSelector); + SmartDashboard.setPersistent("SendableChooser[0]"); + + autoSelector.addOption("Nothing", 0); + autoSelector.addOption("Raw Forward", 1); + autoSelector.addOption("PP Simple Forward", 2);// index corresponds to index in autoCommands[] + + int i = 3; + for (String n : autoNames) { + autoSelector.addOption(n, i); + i++; + } + + ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); + autoSelectorTab.add(autoSelector).withSize(2, 1); + } setDefaultCommands(); setBindingsDriver(); @@ -40,53 +90,170 @@ public RobotContainer() { } private void setDefaultCommands() { - // drivetrain.setDefaultCommand(new TeleopDrive( - // drivetrain, - // () -> ProcessedAxisValue(driverController, Axis.kLeftY)), - // () -> ProcessedAxisValue(driverController, Axis.kLeftX)), - // () -> ProcessedAxisValue(driverController, Axis.kRightX)), - // () -> driverController.getRawButton(OI.Driver.slowDriveButton) - // )); + drivetrain.setDefaultCommand(new TeleopDrive(drivetrain, + () -> ProcessedAxisValue(driverController, Axis.kLeftY), + () -> ProcessedAxisValue(driverController, Axis.kLeftX), + () -> ProcessedAxisValue(driverController, Axis.kRightX), + () -> driverController.getRawButton(Driver.slowDriveButton))); } private void setBindingsDriver() {} private void setBindingsManipulator() {} - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); + /** + * Flips an axis' Y coordinates upside down, but only if the select axis is a + * joystick axis + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis + * @return The processed value. + */ + private double getStickValue(GenericHID hid, Axis axis) { + return hid.getRawAxis(axis.value) + * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); } /** - * Flips an axis' Y coordinates upside down, but only if the select axis is a joystick axis - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis + * Processes an input from the joystick into a value between -1 and 1, + * sinusoidally instead of + * linearly + * + * @param value The value to be processed. * @return The processed value. */ - private double getStickValue(GenericHID hid, Axis axis) { - return hid.getRawAxis(axis.value) * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); + private double inputProcessing(double value) { + double processedInput; + // processedInput = + // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); + processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) + * ((1 - Math.cos(value * Math.PI)) / 2), value); + return processedInput; } + /** - * Processes an input from the joystick into a value between -1 and 1, sinusoidally instead of linearly - * - * @param value The value to be processed. + * Combines both getStickValue and inputProcessing into a single function for + * processing joystick + * outputs + * + * @param hid The controller/plane joystick the axis is on + * @param axis The processed axis * @return The processed value. */ - private double inputProcessing(double value) { - double processedInput; - // processedInput = - // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); - processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) * ((1 - Math.cos(value * Math.PI)) / 2), - value); - return processedInput; + private double ProcessedAxisValue(GenericHID hid, Axis axis) { + return DeadzonedAxis(inputProcessing(getStickValue(hid, axis))); } + /** - * Combines both getStickValue and inputProcessing into a single function for processing joystick outputs - * - * @param hid The controller/plane joystick the axis is on + * Returns zero if a axis input is inside the deadzone + * + * @param hid The controller/plane joystick the axis is on * @param axis The processed axis * @return The processed value. */ - private double ProcessedAxisValue(GenericHID hid, Axis axis){ - return inputProcessing(getStickValue(hid, axis)); + private double DeadzonedAxis(double axOut) { + return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut; } + + /** + * Returns a new instance of Trigger based on the given Joystick and Axis + * objects. The Trigger is + * triggered when the absolute value of the stick value on the specified axis + * exceeds a minimum + * threshold value. + * + * @param stick The Joystick object to retrieve stick value from. + * @param axis The Axis object to retrieve value from the Joystick. + * @return A new instance of Trigger based on the given Joystick and Axis + * objects. * @throws + * NullPointerException if either stick or axis is null. + */ + private Trigger axisTrigger(GenericHID controller, Axis axis) { + return new Trigger(() -> Math + .abs(getStickValue(controller, axis)) > OI.MIN_AXIS_TRIGGER_VALUE); + } + + + private void registerAutoCommands() { + //// AUTO-USABLE COMMANDS + // NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); + } + + private void setupAutos() { + //// CREATING PATHS from files + if (!hasSetupAutos) { + autoCommands=new ArrayList();//clear old/nonexistent autos + + for (int i = 0; i < autoNames.length; i++) { + String name = autoNames[i]; + + autoCommands.add(new PathPlannerAuto(name)); + + /* + * // Charles' opinion: we shouldn't have it path find to the starting pose at the start of match + * new SequentialCommandGroup( + * AutoBuilder.pathfindToPose( + * PathPlannerAuto.getStaringPoseFromAutoFile(name), + * PathPlannerAuto.getPathGroupFromAutoFile(name).get(0). + * getPreviewStartingHolonomicPose(), + * Autoc.pathConstraints), + * new PathPlannerAuto(name)); + */ + } + hasSetupAutos = true; + + // NOTHING + autoCommands.add(0, new PrintCommand("Running NULL Auto!")); + // RAW FORWARD command + autoCommands.add(1, new SequentialCommandGroup( + new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new WaitCommand(0.5), + new LastResortAuto(drivetrain))); + // dumb PP forward command + autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!")); + } + // force regeneration each auto call + autoCommands.set(2, constructPPSimpleAuto());//overwrite this slot each time auto runs + } + + public Command constructPPSimpleAuto() { + /** + * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd + * (m/s): 4.30 + * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max + * AngAccel: 360 + * (guesses!) + */ + // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER + Pose2d currPos = drivetrain.getPose(); + + //FIXME running red PP file autos seems to break something, so the robot drivetrain drives in the wrong direction. + //running blue PP autos is fine though + //Note: alliance detection and path generation work correctly! + //Solution: Redeploy after auto. + Pose2d endPos = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) + ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) + : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); + + List bezierPoints = PathPlannerPath.bezierFromPoses(currPos, endPos); + + // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, rad/s^2 */ + PathPlannerPath path = new PathPlannerPath(bezierPoints, + Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); + + path.preventFlipping = false;// don't flip, we do that manually already. + + return new SequentialCommandGroup( + new InstantCommand(()->drivetrain.drive(-.0001, 0, 0)),//align drivetrain wheels. + AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); + } + + public Command getAutonomousCommand() { + setupAutos(); + + Integer autoIndex = autoSelector.getSelected(); + + if (autoIndex != null && autoIndex != 0) { + new PrintCommand("Running selected auto: " + autoSelector.toString()); + return autoCommands.get(autoIndex.intValue()); + } + return new PrintCommand("No auto :("); + } } diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java new file mode 100644 index 0000000..db12776 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -0,0 +1,152 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Robot; +import org.carlmontrobotics.subsystems.Drivetrain; +import static org.carlmontrobotics.RobotContainer.*; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class TeleopDrive extends Command { + + private static double robotPeriod = Robot.robot.getPeriod(); + private final Drivetrain drivetrain; + private DoubleSupplier fwd; + private DoubleSupplier str; + private DoubleSupplier rcw; + private BooleanSupplier slow; + private double currentForwardVel = 0; + private double currentStrafeVel = 0; + private double prevTimestamp; + private boolean babyMode; + /** + * Creates a new TeleopDrive. + */ + public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow) { + addRequirements(this.drivetrain = drivetrain); + this.fwd = fwd; + this.str = str; + this.rcw = rcw; + this.slow = slow; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // SmartDashboard.putNumber("slow turn const", kSlowDriveRotation); + // SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed); + // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); + // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); + prevTimestamp = Timer.getFPGATimestamp(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentTime = Timer.getFPGATimestamp(); + robotPeriod = currentTime - prevTimestamp; + double[] speeds = getRequestedSpeeds(); + // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); + prevTimestamp = currentTime; + babyMode = SmartDashboard.getBoolean("babymode", false); + // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); + // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); + // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); + // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); + + // SmartDashboard.putNumber("fwd", speeds[0]); + // SmartDashboard.putNumber("strafe", speeds[1]); + // SmartDashboard.putNumber("turn", speeds[2]); + drivetrain.drive(speeds[0], speeds[1], speeds[2]); + } + + public double[] getRequestedSpeeds() { + // Sets all values less than or equal to a very small value (determined by the + // idle joystick state) to zero. + // Used to make sure that the robot does not try to change its angle unless it + // is moving, + double forward = fwd.getAsDouble(); + double strafe = str.getAsDouble(); + double rotateClockwise = rcw.getAsDouble(); + // SmartDashboard.putNumber("fwdIN", forward); + // SmartDashboard.putNumber("strafeIN", strafe); + // SmartDashboard.putNumber("turnIN", rotateClockwise); + if (Math.abs(forward) <= Constants.OI.JOY_THRESH) + forward = 0.0; + else + forward *= maxForward; + if (Math.abs(strafe) <= Constants.OI.JOY_THRESH) + strafe = 0.0; + else + strafe *= maxStrafe; + if (Math.abs(rotateClockwise) <= Constants.OI.JOY_THRESH) + rotateClockwise = 0.0; + else + rotateClockwise *= maxRCW; + + double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; + double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; + if(babyMode == true){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } + // double driveMultiplier = kNormalDriveSpeed; + // double rotationMultiplier = kNormalDriveRotation; + + forward *= driveMultiplier; + strafe *= driveMultiplier; + rotateClockwise *= rotationMultiplier; + + // Limit acceleration of the robot + double accelerationX = (forward - currentForwardVel) / robotPeriod; + double accelerationY = (strafe - currentStrafeVel) / robotPeriod; + double translationalAcceleration = Math.hypot(accelerationX, accelerationY); + // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration); + if (translationalAcceleration > autoMaxAccelMps2 && false) { + Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, + Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); + Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); + currentForwardVel += limitedVelocityVector.getX(); + currentStrafeVel += limitedVelocityVector.getY(); + } else { + currentForwardVel = forward; + currentStrafeVel = strafe; + } + // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel)); + + // ATM, there is no rotational acceleration limit + // currentForwardVel = forward; + // currentStrafeVel = strafe; + // If the above math works, no velocity should be greater than the max velocity, + // so we don't need to limit it. + + return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; + } + + public boolean hasDriverInput() { + return Math.abs(fwd.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(str.getAsDouble()) > Constants.OI.JOY_THRESH + || Math.abs(rcw.getAsDouble()) > Constants.OI.JOY_THRESH; + } + + // 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; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java new file mode 100644 index 0000000..52f10b8 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -0,0 +1,1090 @@ +package org.carlmontrobotics.subsystems; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; +import static org.carlmontrobotics.Constants.Limelightc.*; + +import java.util.Arrays; +import java.util.Map; +import java.util.function.Supplier; + +import org.carlmontrobotics.Constants.Drivetrainc.Autoc; +import org.carlmontrobotics.Robot; +// import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; +import org.carlmontrobotics.commands.TeleopDrive; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SensorFactory; +import org.carlmontrobotics.lib199.swerve.SwerveModule; +import static org.carlmontrobotics.Config.CONFIG; + +import com.ctre.phoenix6.hardware.CANcoder; +// import com.kauailabs.navx.frc.AHRS; +import com.kauailabs.navx.frc.AHRS; +import com.pathplanner.lib.auto.AutoBuilder; +// import com.pathplanner.lib.util.HolonomicPathFollowerConfig; +// import com.pathplanner.lib.util.PIDConstants; +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; +import com.revrobotics.spark.SparkMax; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +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.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.units.Angle; +import edu.wpi.first.units.Distance; +import edu.wpi.first.units.Measure; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.Velocity; +import edu.wpi.first.units.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.SerialPort; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotations; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Meters; +public class Drivetrain extends SubsystemBase { + private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 + private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); + // ^used by PathPlanner for chaining paths + + private SwerveDriveKinematics kinematics = null; + // private SwerveDriveOdometry odometry = null; + + private SwerveDrivePoseEstimator poseEstimator = null; + + private SwerveModule modules[]; + private boolean fieldOriented = true; + private double fieldOffset = 0; + // FIXME not for permanent use!! + private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; + private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; + private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; + + // gyro + public final float initPitch; + public final float initRoll; + + // debug purposes + private SwerveModule moduleFL; + private SwerveModule moduleFR; + private SwerveModule moduleBL; + private SwerveModule moduleBR; + + private final Field2d field = new Field2d(); + + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); + + private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; + + public Drivetrain() { + // SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); + // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); + // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", + // lastSetTheta); + + // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); + // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); + + // Calibrate Gyro + { + + double initTimestamp = Timer.getFPGATimestamp(); + double currentTimestamp = initTimestamp; + while (gyro.isCalibrating() && currentTimestamp - initTimestamp < 10) { + currentTimestamp = Timer.getFPGATimestamp(); + try { + Thread.sleep(1000);// 1 second + } catch (InterruptedException e) { + e.printStackTrace(); + break; + } + System.out.println("Calibrating the gyro..."); + } + gyro.reset(); + // this.resetFieldOrientation(); + System.out.println("NavX-MXP firmware version: " + gyro.getFirmwareVersion()); + System.out.println("Magnetometer is calibrated: " + gyro.isMagnetometerCalibrated()); + } + + // Setup Kinematics + { + // Define the corners of the robot relative to the center of the robot using + // Translation2d objects. + // Positive x-values represent moving toward the front of the robot whereas + // positive y-values represent moving toward the left of the robot. + Translation2d locationFL = new Translation2d(wheelBase / 2, trackWidth / 2); + Translation2d locationFR = new Translation2d(wheelBase / 2, -trackWidth / 2); + Translation2d locationBL = new Translation2d(-wheelBase / 2, trackWidth / 2); + Translation2d locationBR = new Translation2d(-wheelBase / 2, -trackWidth / 2); + + kinematics = new SwerveDriveKinematics(locationFL, locationFR, locationBL, locationBR); + } + + // Initialize modules + { + // initPitch = 0; + // initRoll = 0; + Supplier pitchSupplier = () -> 0F; + Supplier rollSupplier = () -> 0F; + initPitch = gyro.getPitch(); + initRoll = gyro.getRoll(); + // Supplier pitchSupplier = () -> gyro.getPitch(); + // Supplier rollSupplier = () -> gyro.getRoll(); + + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), + turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, + pitchSupplier, rollSupplier); + // Forward-Right + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), + turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, + pitchSupplier, rollSupplier); + + // Backward-Left + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + driveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), + turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, + pitchSupplier, rollSupplier); + // Backward-Right + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + driveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), + turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, + pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + + if (RobotBase.isSimulation()) { + moduleSims = new SwerveModuleSim[] { + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + }; + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + } + + for (SparkMax driveMotor : driveMotors) { + driveMotor.setOpenLoopRampRate(secsPer12Volts); + driveMotor.getEncoder().setPositionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveMotor.getEncoder().setVelocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveMotor.getEncoder().setAverageDepth(2); + driveMotor.getEncoder().setMeasurementPeriod(16); + driveMotor.setSmartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + } + for (SparkMax turnMotor : turnMotors) { + turnMotor.getEncoder().setPositionConversionFactor(360 / turnGearing); + turnMotor.getEncoder().setVelocityConversionFactor(360 / turnGearing / 60); + turnMotor.getEncoder().setAverageDepth(2); + turnMotor.getEncoder().setMeasurementPeriod(16); + } + for (CANcoder coder : turnEncoders) { + coder.getAbsolutePosition().setUpdateFrequency(500); + coder.getPosition().setUpdateFrequency(500); + coder.getVelocity().setUpdateFrequency(500); + + } + + SmartDashboard.putData("Field", field); + + // for(SparkMax driveMotor : driveMotors) + // driveMotor.setSmartCurrentLimit(80); + + // Must call this method for SysId to run + if (CONFIG.isSysIdTesting()) { + sysIdSetup(); + } + } + + // odometry = new SwerveDriveOdometry(kinematics, + // Rotation2d.fromDegrees(getHeading()), getModulePositions(), + // new Pose2d()); + + poseEstimator = new SwerveDrivePoseEstimator( + getKinematics(), + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d()); + + // Setup autopath builder + configurePPLAutoBuilder(); + // SmartDashboard.putNumber("chassis speeds x", 0); + // SmartDashboard.putNumber("chassis speeds y", 0); + + // SmartDashboard.putNumber("chassis speeds theta", 0); + // SmartDashboard.putData(this); + + } + + @Override + public void simulationPeriodic() { + for (var moduleSim : moduleSims) { + moduleSim.update(); + } + SwerveModuleState[] measuredStates = + new SwerveModuleState[] { + moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState() + }; + ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates); + + double dtSecs = simTimer.get(); + simTimer.restart(); + + Pose2d simPose = field.getRobotPose(); + simPose = simPose.exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + double newAngleDeg = simPose.getRotation().getDegrees(); + // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. + newAngleDeg -= simGyroOffset.getDegrees(); + newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); + gyroYawSim.set(newAngleDeg); + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + @Override + public void periodic() { + // for (CANcoder coder : turnEncoders) { + // SignalLogger.writeDouble("Regular position " + coder.toString(), + // coder.getPosition().getValue()); + // SignalLogger.writeDouble("Velocity " + coder.toString(), + // coder.getVelocity().getValue()); + // SignalLogger.writeDouble("Absolute position " + coder.toString(), + // coder.getAbsolutePosition().getValue()); + // } + // lobotomized to prevent ucontrollabe swerve behavior + // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); + // moduleFL.periodic(); + // moduleFR.periodic(); + // moduleBL.periodic(); + // moduleBR.periodic(); + // double goal = SmartDashboard.getNumber("bigoal", 0); + for (SwerveModule module : modules) { + module.periodic(); + // module.move(0, goal); + } + + // field.setRobotPose(odometry.getPoseMeters()); + + field.setRobotPose(poseEstimator.getEstimatedPosition()); + + // odometry.update(gyro.getRotation2d(), getModulePositions()); + + poseEstimator.update(gyro.getRotation2d(), getModulePositions()); + //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); + + updateMT2PoseEstimator(); + + // double currSetX = + // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); + // double currSetY = + // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); + // double currSetTheta = SmartDashboard + // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); + + // if (lastSetX != currSetX || lastSetY != currSetY + // || lastSetTheta != currSetTheta) { + // setPose(new Pose2d(currSetX, currSetY, + // Rotation2d.fromDegrees(currSetTheta))); + // } + + // setPose(new Pose2d(getPose().getTranslation().getX(), + // getPose().getTranslation().getY(), + // Rotation2d.fromDegrees(getHeading()))); + + // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); + // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); + // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); + // SmartDashboard.putNumber("Robot Heading", getHeading()); + // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); + // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); + // SmartDashboard.putBoolean("Field Oriented", fieldOriented); + // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); + // SmartDashboard.putNumber("Compass Offset", compassOffset); + // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); + // SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); + // SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); + // SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); + // SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + for (SwerveModule module : modules) + SendableRegistry.addChild(this, module); + + builder.addBooleanProperty("Magnetic Field Disturbance", + gyro::isMagneticDisturbance, null); + builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); + builder.addBooleanProperty("Field Oriented", () -> fieldOriented, + fieldOriented -> this.fieldOriented = fieldOriented); + builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), + null); + builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(), + null); + builder.addDoubleProperty("Pose Estimator Theta", + () -> + getPose().getRotation().getDegrees(), null); + builder.addDoubleProperty("Robot Heading", () -> getHeading(), null); + builder.addDoubleProperty("Raw Gyro Angle", gyro::getAngle, null); + builder.addDoubleProperty("Pitch", gyro::getPitch, null); + builder.addDoubleProperty("Roll", gyro::getRoll, null); + builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> + this.fieldOffset = fieldOffset); + builder.addDoubleProperty("FL Turn Encoder (Deg)", + () -> moduleFL.getModuleAngle(), null); + builder.addDoubleProperty("FR Turn Encoder (Deg)", + () -> moduleFR.getModuleAngle(), null); + builder.addDoubleProperty("BL Turn Encoder (Deg)", + () -> moduleBL.getModuleAngle(), null); + builder.addDoubleProperty("BR Turn Encoder (Deg)", + () -> moduleBR.getModuleAngle(), null); + + } + + // #region Drive Methods + + /** + * Drives the robot using the given x, y, and rotation speed + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive + */ + public void drive(double forward, double strafe, double rotation) { + drive(getSwerveStates(forward, strafe, rotation)); + } + + public void drive(SwerveModuleState[] moduleStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, maxSpeed); + for (int i = 0; i < 4; i++) { + // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + moduleStates[i] = SwerveModuleState.optimize(moduleStates[i], + Rotation2d.fromDegrees(modules[i].getModuleAngle())); + // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + + modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); + } + } + + public void configurePPLAutoBuilder() { + /** + * PATHPLANNER SETTINGS + * Robot Width (m): .91 + * Robot Length(m): .94 + * Max Module Spd (m/s): 4.30 + * Default Constraints + * Max Vel: 1.54, Max Accel: 6.86 + * Max Angvel: 360, Max AngAccel: 180 (guesses!) + */ + AutoBuilder.configureHolonomic( + this::getPose, + this::setPose, + this::getSpeeds, + (ChassisSpeeds cs) -> { + //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; + // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); + // SmartDashboard.putNumber("chassis speeds y", cs.vyMetersPerSecond); + // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); + + drive(kinematics.toSwerveModuleStates(cs)); + }, + new HolonomicPathFollowerConfig( + new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals + new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals + maxSpeed, + swerveRadius, + Autoc.replanningConfig, + Robot.robot.getPeriod()//robot period + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) + return alliance.get() == DriverStation.Alliance.Red; + //else: + return false; + }, + this + ); + + /* + AutoBuilder.configureHolonomic( + () -> getPose().plus(new Transform2d(autoGyroOffset.getTranslation(),autoGyroOffset.getRotation())),//position supplier + (Pose2d pose) -> { autoGyroOffset=pose.times(-1); }, //position reset (by subtracting current pos) + this::getSpeeds, //chassisSpeed supplier + (ChassisSpeeds cs) -> drive( + cs.vxMetersPerSecond, + -cs.vyMetersPerSecond, + //flipped because drive assumes up is negative, but PPlanner assumes up is positive + cs.omegaRadiansPerSecond + ), + new HolonomicPathFollowerConfig( + new PIDConstants(drivekP[0], drivekI[0], drivekD[0], driveIzone), //translation (drive) pid vals + new PIDConstants(turnkP_avg, 0., 0., turnIzone), //rotation pid vals + maxSpeed, + swerveRadius, + Autoc.replanningConfig, + Robot.robot.getPeriod()//robot period + ), + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) + return alliance.get() == DriverStation.Alliance.Red; + //else: + return false; + }, + this + ); + */ + } + + public void autoCancelDtCommand() { + if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; + + // Use hasDriverInput to get around acceleration limiting on slowdown + if (((TeleopDrive) getDefaultCommand()).hasDriverInput()) { + Command currentDtCommand = getCurrentCommand(); + if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof RotateToFieldRelativeAngle) + && currentDtCommand != null) { + currentDtCommand.cancel(); + } + } + } + + public void stop() { + for (SwerveModule module : modules) + module.move(0, 0); + } + + public boolean isStopped() { + return Math.abs(getSpeeds().vxMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().vyMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().omegaRadiansPerSecond) < 0.1; + } + + /** + * Constructs and returns a ChassisSpeeds objects using forward, strafe, and + * rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A ChassisSpeeds object. + */ + private ChassisSpeeds getChassisSpeeds(double forward, double strafe, double rotation) { + ChassisSpeeds speeds; + if (fieldOriented) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, rotation, + Rotation2d.fromDegrees(getHeading())); + } else { + speeds = new ChassisSpeeds(forward, strafe, rotation); + } + return speeds; + } + + /** + * Constructs and returns four SwerveModuleState objects, one for each side, + * using forward, strafe, and rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A SwerveModuleState array, one for each side of the drivetrain (FL, + * FR, etc.). + */ + private SwerveModuleState[] getSwerveStates(double forward, double strafe, double rotation) { + return kinematics.toSwerveModuleStates(getChassisSpeeds(forward, -strafe, rotation)); + } + + // #endregion + + // #region Getters and Setters + + // returns a value from -180 to 180 + public double getHeading() { + double x = gyro.getAngle(); + if (fieldOriented) + x -= fieldOffset; + return Math.IEEEremainder(x * (isGyroReversed ? -1.0 : 1.0), 360); + } + + public double getHeadingDeg() { + return getHeading();//...wait. + } + + public SwerveModulePosition[] getModulePositions() { + return Arrays.stream(modules).map(SwerveModule::getCurrentPosition).toArray(SwerveModulePosition[]::new); + } + + public Pose2d getPose() { + // return odometry.getPoseMeters(); + return poseEstimator.getEstimatedPosition(); + } + + private Rotation2d simGyroOffset = new Rotation2d(); + public void setPose(Pose2d initialPose) { + Rotation2d gyroRotation = gyro.getRotation2d(); + // odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); + + poseEstimator.resetPosition(gyroRotation, getModulePositions(), initialPose); + // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future + // We need the offset so that we can compensate for it during simulationPeriodic(). + simGyroOffset = initialPose.getRotation().minus(gyroRotation); + //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); + } + + // Resets the gyro, so that the direction the robotic currently faces is + // considered "forward" + public void resetHeading() { + gyro.reset(); + } + + public double getPitch() { + return gyro.getPitch(); + } + + public double getRoll() { + return gyro.getRoll(); + } + + public boolean getFieldOriented() { + return fieldOriented; + } + + public void setFieldOriented(boolean fieldOriented) { + this.fieldOriented = fieldOriented; + } + + public void resetFieldOrientation() { + fieldOffset = gyro.getAngle(); + } + + public void resetPoseEstimator() { + // odometry.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + + poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + gyro.reset(); + } + + public SwerveDriveKinematics getKinematics() { + return kinematics; + } + + public ChassisSpeeds getSpeeds() { + return kinematics.toChassisSpeeds(Arrays.stream(modules).map(SwerveModule::getCurrentState) + .toArray(SwerveModuleState[]::new)); + } + + public void toggleMode() { + for (SwerveModule module : modules) + module.toggleMode(); + } + + public void brake() { + for (SwerveModule module : modules) + module.brake(); + } + + public void coast() { + for (SwerveModule module : modules) + module.coast(); + } + + public double[][] getPIDConstants() { + return new double[][] { + xPIDController, + yPIDController, + thetaPIDController + }; + } + + // #region SysId Code + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutableMeasure[] m_appliedVoltage = new MutableMeasure[8]; + + // Mutable holder for unit-safe linear distance values, persisted to avoid + // reallocation. + private final MutableMeasure[] m_distance = new MutableMeasure[4]; + // Mutable holder for unit-safe linear velocity values, persisted to avoid + // reallocation. + private final MutableMeasure>[] m_velocity = new MutableMeasure[4]; + // edu.wpi.first.math.util.Units.Rotations beans; + private final MutableMeasure[] m_revs = new MutableMeasure[4]; + private final MutableMeasure>[] m_revs_vel = new MutableMeasure[4]; + + private enum SysIdTest { + FRONT_DRIVE, + BACK_DRIVE, + ALL_DRIVE, + // FLBR_TURN, + // FRBL_TURN, + // ALL_TURN + FL_ROT, + FR_ROT, + BL_ROT, + BR_ROT + } + + private SendableChooser sysIdChooser = new SendableChooser<>(); + + // ROUTINES FOR SYSID + // private SysIdRoutine.Config defaultSysIdConfig = new + // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(5)); + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds.of(1)), + Volts.of(2.891), Seconds.of(10)); + + // DRIVE + private void motorLogShort_drive(SysIdRoutineLog log, int id) { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + driveMotors[id].getBusVoltage() * driveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(driveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(driveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } + + // Create a new SysId routine for characterizing the drive. + private SysIdRoutine frontOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + // Tell SysId how to give the driving voltage to the motors. + (Measure volts) -> { + driveMotors[0].setVoltage(volts.in(Volts)); + driveMotors[1].setVoltage(volts.in(Volts)); + modules[2].coast(); + modules[3].coast(); + }, + log -> {// FRONT + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + }, + this)); + + private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Measure volts) -> { + modules[0].coast(); + modules[1].coast(); + modules[2].brake(); + modules[3].brake(); + driveMotors[2].setVoltage(volts.in(Volts)); + driveMotors[3].setVoltage(volts.in(Volts)); + }, + log -> {// BACK + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Measure volts) -> { + for (SparkMax dm : driveMotors) { + dm.setVoltage(volts.in(Volts)); + } + }, + log -> { + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine sysidroutshort_turn(int id, String logname) { + return new SysIdRoutine( + defaultSysIdConfig, + // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(3)), + new SysIdRoutine.Mechanism( + (Measure volts) -> turnMotors[id].setVoltage(volts.in(Volts)), + log -> log.motor(logname + "_turn") + .voltage(m_appliedVoltage[id + 4].mut_replace( + // ^because drivemotors take up the first 4 slots of the unit holders + turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) + .angularPosition( + m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue(), Rotations)) + .angularVelocity(m_revs_vel[id].mut_replace( + turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), + this)); + } + + // as always, fl/fr/bl/br + private SysIdRoutine[] rotateRoutine = new SysIdRoutine[] { + sysidroutshort_turn(0, "fl"), // woaw, readable code??? + sysidroutshort_turn(1, "fr"), + sysidroutshort_turn(2, "bl"), + sysidroutshort_turn(3, "br") + }; + + private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Drivetrain SysID"); + + // void sysidtabshorthand(String name, SysIdRoutine.Direction dir, int width, + // int height){ + // sysIdTab.add(name, dir).withSize(width, height); + // } + void sysidtabshorthand_qsi(String name, SysIdRoutine.Direction dir) { + sysIdTab.add(name, sysIdQuasistatic(dir)).withSize(2, 1); + } + + void sysidtabshorthand_dyn(String name, SysIdRoutine.Direction dir) { + sysIdTab.add(name, sysIdDynamic(dir)).withSize(2, 1); + } + + private void sysIdSetup() { + // SysId Setup + { + Supplier stopNwait = () -> new SequentialCommandGroup( + new InstantCommand(this::stop), new WaitCommand(2)); + + /* + * Alex's old sysId tests + * sysIdTab.add("All sysid tests", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - FRONT wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - BACK wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()) + * )); + */ + + sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); + sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); + sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); + sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); + + sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); + sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); + sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); + // sysIdChooser.addOption("fl-br Turn", SysIdTest.FLBR_TURN); + // sysIdChooser.addOption("fr-bl Turn", SysIdTest.FRBL_TURN); + // sysIdChooser.addOption("All Turn", SysIdTest.ALL_TURN); + sysIdChooser.addOption("FL Rotate", SysIdTest.FL_ROT); + sysIdChooser.addOption("FR Rotate", SysIdTest.FR_ROT); + sysIdChooser.addOption("BL Rotate", SysIdTest.BL_ROT); + sysIdChooser.addOption("BR Rotate", SysIdTest.BR_ROT); + + sysIdTab + .add(sysIdChooser) + .withSize(2, 1); + + sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? + .withSize(2, 1); + + sysIdTab.add(this); + + for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors + m_appliedVoltage[i] = mutable(Volts.of(0)); + } + for (int i = 0; i < 4; i++) { + m_distance[i] = mutable(Meters.of(0)); + m_velocity[i] = mutable(MetersPerSecond.of(0)); + + m_revs[i] = mutable(Rotations.of(0)); + m_revs_vel[i] = mutable(RotationsPerSecond.of(0)); + } + + // SmartDashboard.putNumber("Desired Angle", 0); + + // SmartDashboard.putNumber("kS", 0); + } + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + private SysIdTest selector() { + SysIdTest test = sysIdChooser.getSelected(); + System.out.println("Test Selected: " + test); + return test; + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only quasistatic forward") + : new PrintCommand("Running front only quasistatic backward"), + frontOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only quasistatic forward") + : new PrintCommand("Running back only quasistatic backward"), + backOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all drive quasistatic forward") + : new PrintCommand("Running all drive quasistatic backward"), + allWheelsDriveRoutine.quasistatic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate quasistatic forward") + : new PrintCommand("Running FL rotate quasistatic backward"), + rotateRoutine[0].quasistatic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate quasistatic forward") + : new PrintCommand("Running FR rotate quasistatic backward"), + rotateRoutine[1].quasistatic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate quasistatic forward") + : new PrintCommand("Running BL rotate quasistatic backward"), + rotateRoutine[2].quasistatic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate quasistatic forward") + : new PrintCommand("Running BR rotate quasistatic backward"), + rotateRoutine[3].quasistatic(direction))) + + // //TURN + // Map.entry(SysIdTest.FLBR_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fL-bR turn quasistatic forward") : + // new PrintCommand("Running fL-bR turn quasistatic backward"), + // flbrTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.FRBL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fR-bL turn quasistatic forward") : + // new PrintCommand("Running fR-bL turn quasistatic backward"), + // frblTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.ALL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running all turn quasistatic forward") : + // new PrintCommand("Running all turn quasistatic backward"), + // allWheelsTurn.quasistatic(direction) + // )) + ), + this::selector); + } + + // public Command sysIdDynamic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyDrive.dynamic(direction); + // case 1: + // return backOnlyDrive.dynamic(direction); + // case 2: + // return allWheelsDrive.dynamic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + private Command allTheSYSID(SysIdRoutine.Direction direction) { + return new SequentialCommandGroup( + frontOnlyDriveRoutine.dynamic(direction), + backOnlyDriveRoutine.dynamic(direction), + allWheelsDriveRoutine.dynamic(direction), + rotateRoutine[0].dynamic(direction), + rotateRoutine[1].dynamic(direction), + rotateRoutine[2].dynamic(direction), + rotateRoutine[3].dynamic(direction), + + frontOnlyDriveRoutine.quasistatic(direction), + backOnlyDriveRoutine.quasistatic(direction), + allWheelsDriveRoutine.quasistatic(direction), + rotateRoutine[0].quasistatic(direction), + rotateRoutine[1].quasistatic(direction), + rotateRoutine[2].quasistatic(direction), + rotateRoutine[3].quasistatic(direction)); + } + + public Command allTheSYSID() { + return new SequentialCommandGroup( + allTheSYSID(SysIdRoutine.Direction.kForward), + allTheSYSID(SysIdRoutine.Direction.kReverse)); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only dynamic forward") + : new PrintCommand("Running front only dynamic backward"), + frontOnlyDriveRoutine.dynamic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only dynamic forward") + : new PrintCommand("Running back only dynamic backward"), + backOnlyDriveRoutine.dynamic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all wheels dynamic forward") + : new PrintCommand("Running all wheels dynamic backward"), + allWheelsDriveRoutine.dynamic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate dynamic forward") + : new PrintCommand("Running FL rotate dynamic backward"), + rotateRoutine[0].dynamic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate dynamic forward") + : new PrintCommand("Running FR rotate dynamic backward"), + rotateRoutine[1].dynamic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate dynamic forward") + : new PrintCommand("Running BL rotate dynamic backward"), + rotateRoutine[2].dynamic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate dynamic forward") + : new PrintCommand("Running BR rotate dynamic backward"), + rotateRoutine[3].dynamic(direction)))), + this::selector); + } + + public void keepRotateMotorsAtDegrees(int angle) { + for (SwerveModule module : modules) { + module.turnPeriodic(); + module.move(0.0000000000001, angle); + } + } + + // pose estimator stuff + + public void updateMT2PoseEstimator() { + boolean rejectVisionUpdate = false; + + LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME, + poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers + .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); + + if (Math.abs(getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second + rejectVisionUpdate = true; + } + + if (visionPoseEstimate.tagCount == 0) { + rejectVisionUpdate = true; + } + + if (!rejectVisionUpdate) { + poseEstimator + .setVisionMeasurementStdDevs( + VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, + STD_DEV_HEADING_RADS)); + poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds); + } + } + + public double getGyroRate() { + return gyro.getRate(); + } + // #endregion +} From 97e63d261eae6e3f39ccc26e701ab92114fb0ca5 Mon Sep 17 00:00:00 2001 From: Tim <73599525+timtogan@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:05:44 -0800 Subject: [PATCH 02/39] bug fixes --- .../org/carlmontrobotics/RobotContainer.java | 22 +++++++++++++++++-- .../subsystems/Drivetrain.java | 12 ++++++---- 2 files changed, 28 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index bfd496c..af6d906 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -6,6 +6,16 @@ //199 files import org.carlmontrobotics.subsystems.*; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.commands.PathPlannerAuto; +import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.PathPlannerPath; + +import org.carlmontrobotics.Constants.Drivetrainc.Autoc; +import org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Manipulator; +import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; @@ -14,17 +24,25 @@ import java.util.ArrayList; import java.util.List; +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.wpilibj.DriverStation; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; - +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - +import edu.wpi.first.wpilibj2.command.WaitCommand; //control bindings import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 52f10b8..bd4c38f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -40,12 +40,16 @@ import edu.wpi.first.math.kinematics.SwerveDriveOdometry; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.units.Angle; -import edu.wpi.first.units.Distance; +// import edu.wpi.first.units.Angle; +// import edu.wpi.first.units.Distance; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; -import edu.wpi.first.units.Velocity; -import edu.wpi.first.units.Voltage; +// import edu.wpi.first.units.Velocity; +// import edu.wpi.first.units.Voltage; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.DriverStation; From 3d72d22fd03eb0cb14802911e6a2a53c0c9f04b0 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 18 Jan 2025 11:14:11 -0800 Subject: [PATCH 03/39] //BabyMode Slashed babymode as it has no constants. Need to add the constants and then we can put back. 2 Errors removed --- .../java/org/carlmontrobotics/commands/TeleopDrive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java index db12776..393db98 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -96,10 +96,10 @@ public double[] getRequestedSpeeds() { double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; - if(babyMode == true){ - driveMultiplier = kBabyDriveSpeed; - rotationMultiplier = kBabyDriveRotation; - } + //if(babyMode == true){ + //driveMultiplier = kBabyDriveSpeed; also gone, need to be setup + //rotationMultiplier = kBabyDriveRotation; kBabyRotation is gone need set it up + //} // double driveMultiplier = kNormalDriveSpeed; // double rotationMultiplier = kNormalDriveRotation; From 3d4ce23341a06ef7096304e8ff4797c06f8dfb3c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 18 Jan 2025 11:21:17 -0800 Subject: [PATCH 04/39] kSlowNormalBaby Drive and Rot Made constants for slow normal and baby modes, have to be set up. --- src/main/java/org/carlmontrobotics/Constants.java | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index c3776c3..6d9cf4c 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -149,6 +149,17 @@ public static final class Drivetrainc { kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, driveInversion, reversed, driveModifier, turnInversion); + //Constants for Normaldrive + public static final double kNormalDriveRotation = 0; + public static final double kNormalDriveSpeed = 0; + //Constants for slowDrive + public static final double kSlowDriveRotation = 0; + public static final double kSlowDriveSpeed = 0; + //Constants for babymode + public static final double kBabyDriveRotation = 0; + public static final double kBabyDriveSpeed = 0; + + public static final class Autoc { } } From 6add2e8bfc1548e66465185d90691090bc57cdc8 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 18 Jan 2025 11:21:47 -0800 Subject: [PATCH 05/39] BabyMode un// Baby mode un slashed as constants have been made for them now. --- .../java/org/carlmontrobotics/commands/TeleopDrive.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java index 393db98..26abc7c 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -96,10 +96,10 @@ public double[] getRequestedSpeeds() { double driveMultiplier = slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed; double rotationMultiplier = slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation; - //if(babyMode == true){ - //driveMultiplier = kBabyDriveSpeed; also gone, need to be setup - //rotationMultiplier = kBabyDriveRotation; kBabyRotation is gone need set it up - //} + if(babyMode == true){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } // double driveMultiplier = kNormalDriveSpeed; // double rotationMultiplier = kNormalDriveRotation; From 54f1b769eb22d57cb4738b02191a12bce995cc0c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 22 Jan 2025 08:29:30 -0800 Subject: [PATCH 06/39] Got missing stuff added, plus overall cleaner code --- .../java/org/carlmontrobotics/Constants.java | 296 +++++++++++------- 1 file changed, 177 insertions(+), 119 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d9cf4c..02b80c5 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -5,6 +5,9 @@ package org.carlmontrobotics; +import org.carlmontrobotics.lib199.swerve.SwerveConfig; +import static org.carlmontrobotics.Config.CONFIG; + import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -43,127 +46,182 @@ public static final class Armc { } public static final class Drivetrainc { - - // #region Subsystem Constants - - public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) - : Units.inchesToMeters(16.75); - public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) - : Units.inchesToMeters(23.75); - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); - // The gearing reduction from the drive motor controller to the wheels - // Gearing for the Swerve Modules is 6.75 : 1 - public static final double driveGearing = 6.75; - // Turn motor shaft to "module shaft" - public static final double turnGearing = 150 / 7; - - public static final double driveModifier = 1; - public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* - * empirical - * correction - */; - public static final double mu = 1; /* 70/83.2; */ - - public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s - // Angular speed to translational speed --> v = omega * r / gearing - public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; - public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out - public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out - // seconds it takes to go from 0 to 12 volts(aka MAX) - public static final double secsPer12Volts = 0.1; - - // maxRCW is the angular velocity of the robot. - // Calculated by looking at one of the motors and treating it as a point mass - // moving around in a circle. - // Tangential speed of this point mass is maxSpeed and the radius of the circle - // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) - // Angular velocity = Tangential speed / radius - public static final double maxRCW = maxSpeed / swerveRadius; - - public static final boolean[] reversed = { false, false, false, false }; - // public static final boolean[] reversed = {true, true, true, true}; - // Determine correct turnZero constants (FL, FR, BL, BR) - public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } - : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } - : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ - - // kP, kI, and kD constants for turn motor controllers in the order of - // front-left, front-right, back-left, back-right. - // Determine correct turn PID constants - public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, - // 0.00374}; - public static final double[] turnkI = { 0, 0, 0, 0 }; - public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d - // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; - - // V = kS + kV * v + kA * a - // 12 = 0.2 + 0.00463 * v - // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 }; - public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 }; - - // kP is an average of the forward and backward kP values - // Forward: 1.72, 1.71, 1.92, 1.94 - // Backward: 1.92, 1.92, 2.11, 1.89 - // Order of modules: (FL, FR, BL, BR) - public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } - : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, - // 1.915/100}; - public static final double[] drivekI = { 0, 0, 0, 0 }; - public static final double[] drivekD = { 0, 0, 0, 0 }; - public static final boolean[] driveInversion = (CONFIG.isSwimShady() - ? new boolean[] { false, false, false, false } - : new boolean[] { true, false, true, false }); - public static final boolean[] turnInversion = { true, true, true, true }; - // kS - public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kBackwardVolts = kForwardVolts; - - public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kBackwardVels = kForwardVels; - public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kBackwardAccels = kForwardAccels; - - public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second - public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 - public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java - // The maximum acceleration the robot can achieve is equal to the coefficient of - // static friction times the gravitational acceleration - // a = mu * 9.8 m/s^2 - public static final double autoCentripetalAccel = mu * g * 2; - - public static final boolean isGyroReversed = true; - - // PID values are listed in the order kP, kI, and kD - public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } - : new double[] { 2, 0.0, 0.0 }; - public static final double[] yPIDController = xPIDController; - public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } - : new double[] {0.05, 0.0, 0.00}; - - public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, - autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, - kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, - driveInversion, reversed, driveModifier, turnInversion); - - //Constants for Normaldrive - public static final double kNormalDriveRotation = 0; - public static final double kNormalDriveSpeed = 0; - //Constants for slowDrive - public static final double kSlowDriveRotation = 0; - public static final double kSlowDriveSpeed = 0; - //Constants for babymode - public static final double kBabyDriveRotation = 0; - public static final double kBabyDriveSpeed = 0; - - - public static final class Autoc { + // #region Subsystem Constants + public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : Units.inchesToMeters(16.75); + public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : Units.inchesToMeters(23.75); + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); + // The gearing reduction from the drive motor controller to the wheels + // Gearing for the Swerve Modules is 6.75 : 1 + public static final double driveGearing = 6.75; + // Turn motor shaft to "module shaft" + public static final double turnGearing = 150 / 7; + + public static final double driveModifier = 1; + public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* + * empirical + * correction + */; + public static final double mu = 1; /* 70/83.2; */ + + public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s + // Angular speed to translational speed --> v = omega * r / gearing + public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; + public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out + public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out + // seconds it takes to go from 0 to 12 volts(aka MAX) + public static final double secsPer12Volts = 0.1; + + // maxRCW is the angular velocity of the robot. + // Calculated by looking at one of the motors and treating it as a point mass + // moving around in a circle. + // Tangential speed of this point mass is maxSpeed and the radius of the circle + // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) + // Angular velocity = Tangential speed / radius + public static final double maxRCW = maxSpeed / swerveRadius; + + public static final boolean[] reversed = { false, false, false, false }; + // public static final boolean[] reversed = {true, true, true, true}; + // Determine correct turnZero constants (FL, FR, BL, BR) + public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } + : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } + : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ + + // kP, kI, and kD constants for turn motor controllers in the order of + // front-left, front-right, back-left, back-right. + // Determine correct turn PID constants + public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, + // 0.00374}; + public static final double[] turnkI = { 0, 0, 0, 0 }; + public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d + // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; + public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; + + // V = kS + kV * v + kA * a + // 12 = 0.2 + 0.00463 * v + // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s + public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 }; + public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 }; + + // kP is an average of the forward and backward kP values + // Forward: 1.72, 1.71, 1.92, 1.94 + // Backward: 1.92, 1.92, 2.11, 1.89 + // Order of modules: (FL, FR, BL, BR) + public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } + : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, + // 1.915/100}; + public static final double[] drivekI = { 0, 0, 0, 0 }; + public static final double[] drivekD = { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isSwimShady() + ? new boolean[] { false, false, false, false } + : new boolean[] { true, false, true, false }); + public static final boolean[] turnInversion = { true, true, true, true }; + // kS + public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kBackwardVolts = kForwardVolts; + + public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kBackwardVels = kForwardVels; + public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kBackwardAccels = kForwardAccels; + + public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second + public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 + public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java + // The maximum acceleration the robot can achieve is equal to the coefficient of + // static friction times the gravitational acceleration + // a = mu * 9.8 m/s^2 + public static final double autoCentripetalAccel = mu * g * 2; + + public static final boolean isGyroReversed = true; + + // PID values are listed in the order kP, kI, and kD + public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } + : new double[] { 2, 0.0, 0.0 }; + public static final double[] yPIDController = xPIDController; + public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } + : new double[] {0.05, 0.0, 0.00}; + + public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, + autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, + kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, + driveInversion, reversed, driveModifier, turnInversion); + + // public static final Limelight.Transform limelightTransformForPoseEstimation = + // Transform.BOTPOSE_WPIBLUE; + + // #endregion + + // #region Ports + //I think all of these are right + public static final int driveFrontLeftPort = 1; + public static final int driveFrontRightPort = 2; + public static final int driveBackLeftPort = 3; + public static final int driveBackRightPort = 4; + + public static final int turnFrontLeftPort = 11; + public static final int turnFrontRightPort = 12; + public static final int turnBackLeftPort = 13; + public static final int turnBackRightPort = 14; + //to be configured + public static final int canCoderPortFL = 0; + public static final int canCoderPortFR = 3; + public static final int canCoderPortBL = 2; + public static final int canCoderPortBR = 1; + + // #endregion + + // #region Command Constants + + public static double kNormalDriveSpeed = 1; // Percent Multiplier + public static double kNormalDriveRotation = 0.5; // Percent Multiplier + public static double kSlowDriveSpeed = 0.4; // Percent Multiplier + public static double kSlowDriveRotation = 0.250; // Percent Multiplier + + //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use + public static double kBabyDriveSpeed = 0.3; + public static double kBabyDriveRotation = 0.2; + public static double kAlignMultiplier = 1D / 3D; + public static final double kAlignForward = 0.6; + + public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to + // orient the wheels to the correct angle. This + // should be very small to avoid actually moving the + // robot. + + public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, + // Meters, + // Degrees + public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, + // Meters, + // Degrees/Second + + // #endregion + // #region Subsystem Constants + + // "swerveRadius" is the distance from the center of the robot to one of the + // modules + public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; + public static final double turnIzone = .1; + + public static final double driveIzone = .1; + + public static final class Autoc { + public static final ReplanningConfig replanningConfig = new ReplanningConfig( /* + * put in + * Constants.Drivetrain.Auto + */ + false, // replan at start of path if robot not at start of path? + false, // replan if total error surpasses total error/spike threshold? + 1.5, // total error threshold in meters that will cause the path to be replanned + .8 // error spike threshold, in meters, that will cause the path to be replanned + ); + public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, + 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the + // angular constraints have no effect. } } - public static final class Limelightc { public static final class Apriltag { From 1e55ae0941cb5e5b97732f9786c824fb122543fb Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 25 Jan 2025 21:15:19 -0800 Subject: [PATCH 07/39] Fixed AHRS --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index bd4c38f..8d48f4b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -19,7 +19,7 @@ import com.ctre.phoenix6.hardware.CANcoder; // import com.kauailabs.navx.frc.AHRS; -import com.kauailabs.navx.frc.AHRS; +import com.studica.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; // import com.pathplanner.lib.util.HolonomicPathFollowerConfig; // import com.pathplanner.lib.util.PIDConstants; From 89fc78e0a43e6e7a8a17e3ea08bd7454791b1ad4 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Tue, 28 Jan 2025 17:10:16 -0800 Subject: [PATCH 08/39] Adhit Das Commit: Fixed ~30 Drivetrain errors --- .devcontainer/devcontainer.json | 39 +++++++ .devcontainer/installWPILibExtension.sh | 7 ++ .../java/org/carlmontrobotics/Constants.java | 7 +- .../subsystems/Drivetrain.java | 105 ++++++++++-------- 4 files changed, 112 insertions(+), 46 deletions(-) create mode 100644 .devcontainer/devcontainer.json create mode 100644 .devcontainer/installWPILibExtension.sh diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json new file mode 100644 index 0000000..9ecad41 --- /dev/null +++ b/.devcontainer/devcontainer.json @@ -0,0 +1,39 @@ +// For format details, see https://aka.ms/devcontainer.json. For config options, see the +// README at: https://github.com/devcontainers/templates/tree/main/src/java +{ + "name": "Java", + // Or use a Dockerfile or Docker Compose file. More info: https://containers.dev/guide/dockerfile + "image": "mcr.microsoft.com/devcontainers/java:1-17-bookworm", + + "features": { + "ghcr.io/devcontainers/features/java:1": { + "version": "none", + "installMaven": "false", + "installGradle": "true" + } + }, + + // Use 'forwardPorts' to make a list of ports inside the container available locally. + // "forwardPorts": [], + + // Use 'postCreateCommand' to run commands after the container is created. + // "postCreateCommand": "", + + // Configure tool-specific properties. + "customizations": { + "vscode": { + "extensions": [ + "redhat.java", + "ms-vscode.cpptools", + "vcjava.vscode-java-debug", + "ms-toolsai.jupyter", + "vcjava.vscode-java-dependency" + ] + } + }, + + "postAttachCommand": "./.devcontainer/installWPILibExtension.sh && exit" + + // Uncomment to connect as root instead. More info: https://aka.ms/dev-containers-non-root. + // "remoteUser": "root" +} diff --git a/.devcontainer/installWPILibExtension.sh b/.devcontainer/installWPILibExtension.sh new file mode 100644 index 0000000..f789b8b --- /dev/null +++ b/.devcontainer/installWPILibExtension.sh @@ -0,0 +1,7 @@ +# Modified from ChatGPT suggestions +WPILIB_VSIX_URL=$(curl -s "https://api.github.com/repos/wpilibsuite/vscode-wpilib/releases/latest" | jq -r '.assets[] | select(.name | test(".vsix$")) | .browser_download_url') +INSTALL_LOCATION="/tmp/wpilib-extension/latest.vsix" +echo "$WPILIB_VSIX_URL" +curl --create-dirs -L -o "$INSTALL_LOCATION" "$WPILIB_VSIX_URL" +code --install-extension "$INSTALL_LOCATION" + diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 02b80c5..ce6150a 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -6,6 +6,9 @@ package org.carlmontrobotics; import org.carlmontrobotics.lib199.swerve.SwerveConfig; + +import com.pathplanner.lib.config.RobotConfig; + import static org.carlmontrobotics.Config.CONFIG; import edu.wpi.first.math.util.Units; @@ -208,14 +211,14 @@ public static final class Drivetrainc { public static final double driveIzone = .1; public static final class Autoc { - public static final ReplanningConfig replanningConfig = new ReplanningConfig( /* + public static final RobotConfig robotConfig = new RobotConfig( /* * put in * Constants.Drivetrain.Auto */ false, // replan at start of path if robot not at start of path? false, // replan if total error surpasses total error/spike threshold? 1.5, // total error threshold in meters that will cause the path to be replanned - .8 // error spike threshold, in meters, that will cause the path to be replanned + 0.8 // error spike threshold, in meters, that will cause the path to be replanned ); public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 8d48f4b..100328c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -1,3 +1,5 @@ +//Resolve 71 errors starting from Holonomics + package org.carlmontrobotics.subsystems; import static org.carlmontrobotics.Constants.Drivetrainc.*; @@ -13,6 +15,7 @@ import org.carlmontrobotics.commands.TeleopDrive; import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.MotorErrors; import org.carlmontrobotics.lib199.SensorFactory; import org.carlmontrobotics.lib199.swerve.SwerveModule; import static org.carlmontrobotics.Config.CONFIG; @@ -21,10 +24,18 @@ // import com.kauailabs.navx.frc.AHRS; import com.studica.frc.AHRS; import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; + // import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -// import com.pathplanner.lib.util.PIDConstants; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; + import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; @@ -78,7 +89,8 @@ import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.MutableMeasure.mutable; +// Make sure this code is extraneous +// import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; public class Drivetrain extends SubsystemBase { private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 @@ -172,27 +184,27 @@ public Drivetrain() { // Supplier rollSupplier = () -> gyro.getRoll(); moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), - turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorErrors.TemperatureLimit.NEO), + turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorErrors.TemperatureLimit.NEO), turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); // Forward-Right moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), - turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorErrors.TemperatureLimit.NEO), + turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorErrors.TemperatureLimit.NEO), turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); // Backward-Left moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, - driveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), - turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + driveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorErrors.TemperatureLimit.NEO), + turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorErrors.TemperatureLimit.NEO), turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); // Backward-Right moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, - driveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), - turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + driveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorErrors.TemperatureLimit.NEO), + turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorErrors.TemperatureLimit.NEO), turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; @@ -203,21 +215,28 @@ public Drivetrain() { }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } + + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); for (SparkMax driveMotor : driveMotors) { - driveMotor.setOpenLoopRampRate(secsPer12Volts); - driveMotor.getEncoder().setPositionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); - driveMotor.getEncoder().setVelocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); - driveMotor.getEncoder().setAverageDepth(2); - driveMotor.getEncoder().setMeasurementPeriod(16); - driveMotor.setSmartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.positionConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + for (SparkMax turnMotor : turnMotors) { - turnMotor.getEncoder().setPositionConversionFactor(360 / turnGearing); - turnMotor.getEncoder().setVelocityConversionFactor(360 / turnGearing / 60); - turnMotor.getEncoder().setAverageDepth(2); - turnMotor.getEncoder().setMeasurementPeriod(16); + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } + for (CANcoder coder : turnEncoders) { coder.getAbsolutePosition().setUpdateFrequency(500); coder.getPosition().setUpdateFrequency(500); @@ -434,10 +453,10 @@ public void configurePPLAutoBuilder() { * Max Vel: 1.54, Max Accel: 6.86 * Max Angvel: 360, Max AngAccel: 180 (guesses!) */ - AutoBuilder.configureHolonomic( - this::getPose, - this::setPose, - this::getSpeeds, + AutoBuilder.configure( + this::getPose, // :D + this::setPose, // :D + this::getSpeeds, // :D (ChassisSpeeds cs) -> { //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); @@ -445,26 +464,24 @@ public void configurePPLAutoBuilder() { // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); drive(kinematics.toSwerveModuleStates(cs)); - }, - new HolonomicPathFollowerConfig( - new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals - new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals - maxSpeed, - swerveRadius, - Autoc.replanningConfig, - Robot.robot.getPeriod()//robot period - ), - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) - return alliance.get() == DriverStation.Alliance.Red; - //else: - return false; - }, - this + }, // :D + new PPHolonomicDriveController( + new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals + new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals + Robot.kDefaultPeriod//robot period + ), + Autoc.robotConfig, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) + return alliance.get() == DriverStation.Alliance.Red; + //else: + return false; + }, // :D + this // :D ); /* From b4fa9532c36998dea126e2a87bf5a968fabd5b18 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:06:13 -0800 Subject: [PATCH 09/39] Fixed SysId and part of measure --- .../carlmontrobotics/subsystems/Drivetrain.java | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 100328c..5c2374b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -55,6 +55,7 @@ // import edu.wpi.first.units.Distance; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; // import edu.wpi.first.units.Velocity; @@ -683,16 +684,16 @@ public double[][] getPIDConstants() { // #region SysId Code // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutableMeasure[] m_appliedVoltage = new MutableMeasure[8]; + private final Voltage[] m_appliedVoltage = new Voltage[8]; // Mutable holder for unit-safe linear distance values, persisted to avoid // reallocation. - private final MutableMeasure[] m_distance = new MutableMeasure[4]; + private final Distance[] m_distance = new Distance[4]; // Mutable holder for unit-safe linear velocity values, persisted to avoid // reallocation. - private final MutableMeasure>[] m_velocity = new MutableMeasure[4]; + private final LinearVelocity[] m_velocity = new LinearVelocity[4]; // edu.wpi.first.math.util.Units.Rotations beans; - private final MutableMeasure[] m_revs = new MutableMeasure[4]; + private final Angle[] m_revs = new Angle[4]; private final MutableMeasure>[] m_revs_vel = new MutableMeasure[4]; private enum SysIdTest { @@ -734,7 +735,7 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { defaultSysIdConfig, new SysIdRoutine.Mechanism( // Tell SysId how to give the driving voltage to the motors. - (Measure volts) -> { + (Voltage volts) -> { driveMotors[0].setVoltage(volts.in(Volts)); driveMotors[1].setVoltage(volts.in(Volts)); modules[2].coast(); @@ -749,7 +750,7 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( defaultSysIdConfig, new SysIdRoutine.Mechanism( - (Measure volts) -> { + (Voltage volts) -> { modules[0].coast(); modules[1].coast(); modules[2].brake(); @@ -766,7 +767,7 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( defaultSysIdConfig, new SysIdRoutine.Mechanism( - (Measure volts) -> { + (Voltage volts) -> { for (SparkMax dm : driveMotors) { dm.setVoltage(volts.in(Volts)); } @@ -785,7 +786,7 @@ private SysIdRoutine sysidroutshort_turn(int id, String logname) { // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), // Seconds.of(3)), new SysIdRoutine.Mechanism( - (Measure volts) -> turnMotors[id].setVoltage(volts.in(Volts)), + (Voltage volts) -> turnMotors[id].setVoltage(volts.in(Volts)), log -> log.motor(logname + "_turn") .voltage(m_appliedVoltage[id + 4].mut_replace( // ^because drivemotors take up the first 4 slots of the unit holders From a6f55cb962f1bca0580d1f38ce83b5f4c0912b03 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:19:42 -0800 Subject: [PATCH 10/39] Mutable Measure fixed --- .../subsystems/Drivetrain.java | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 5c2374b..4973949 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -54,8 +54,14 @@ // import edu.wpi.first.units.Angle; // import edu.wpi.first.units.Distance; import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutDistance; +import edu.wpi.first.units.measure.MutLinearVelocity; +import edu.wpi.first.units.measure.MutVoltage; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; // import edu.wpi.first.units.Velocity; @@ -684,17 +690,17 @@ public double[][] getPIDConstants() { // #region SysId Code // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final Voltage[] m_appliedVoltage = new Voltage[8]; + private final MutVoltage[] m_appliedVoltage = new MutVoltage[8]; // Mutable holder for unit-safe linear distance values, persisted to avoid // reallocation. - private final Distance[] m_distance = new Distance[4]; + private final MutDistance[] m_distance = new MutDistance[4]; // Mutable holder for unit-safe linear velocity values, persisted to avoid // reallocation. - private final LinearVelocity[] m_velocity = new LinearVelocity[4]; + private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[4]; // edu.wpi.first.math.util.Units.Rotations beans; - private final Angle[] m_revs = new Angle[4]; - private final MutableMeasure>[] m_revs_vel = new MutableMeasure[4]; + private final MutAngle[] m_revs = new MutAngle[4]; + private final MutAngularVelocity[] m_revs_vel = new MutAngularVelocity[4]; private enum SysIdTest { FRONT_DRIVE, @@ -792,7 +798,7 @@ private SysIdRoutine sysidroutshort_turn(int id, String logname) { // ^because drivemotors take up the first 4 slots of the unit holders turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) .angularPosition( - m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue(), Rotations)) + m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) .angularVelocity(m_revs_vel[id].mut_replace( turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), this)); From ed1346fed112f0af1ff5bde4155bd07725134e36 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:22:33 -0800 Subject: [PATCH 11/39] Fixed Drivetrain Port for Gyro --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 5c2374b..c6c6cdb 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -23,6 +23,7 @@ import com.ctre.phoenix6.hardware.CANcoder; // import com.kauailabs.navx.frc.AHRS; import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -94,7 +95,7 @@ // import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; public class Drivetrain extends SubsystemBase { - private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); // Also try kUSB and kUSB2 private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); // ^used by PathPlanner for chaining paths From 47c642a35731d0f2902139d46806a1338a526c09 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:28:18 -0800 Subject: [PATCH 12/39] Fixed .per thing for time on Drivetrain --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index d9b556b..d06d5b5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -722,7 +722,7 @@ private enum SysIdTest { // private SysIdRoutine.Config defaultSysIdConfig = new // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), // Seconds.of(5)); - private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds.of(1)), + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), Volts.of(2.891), Seconds.of(10)); // DRIVE From aaa5e3f01f57764b3069fd1d8638db9adc894c42 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:30:33 -0800 Subject: [PATCH 13/39] mutable fully fixed --- .../org/carlmontrobotics/subsystems/Drivetrain.java | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 4973949..24bbf8c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -93,9 +93,12 @@ import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Meter; // Make sure this code is extraneous // import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; @@ -898,14 +901,14 @@ private void sysIdSetup() { sysIdTab.add(this); for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors - m_appliedVoltage[i] = mutable(Volts.of(0)); + m_appliedVoltage[i] = Volt.mutable(0); } for (int i = 0; i < 4; i++) { - m_distance[i] = mutable(Meters.of(0)); - m_velocity[i] = mutable(MetersPerSecond.of(0)); + m_distance[i] = Meter.mutable(0); + m_velocity[i] = MetersPerSecond.mutable(0); - m_revs[i] = mutable(Rotations.of(0)); - m_revs_vel[i] = mutable(RotationsPerSecond.of(0)); + m_revs[i] = Rotation.mutable(0); + m_revs_vel[i] = RotationsPerSecond.mutable(0); } // SmartDashboard.putNumber("Desired Angle", 0); From 419fb82a300224750708623d68ff9c85ab6efef4 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:33:37 -0800 Subject: [PATCH 14/39] Update Drivetrain.java --- .../java/org/carlmontrobotics/subsystems/Drivetrain.java | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 5f46d79..07e2260 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -21,9 +21,8 @@ import static org.carlmontrobotics.Config.CONFIG; import com.ctre.phoenix6.hardware.CANcoder; -// import com.kauailabs.navx.frc.AHRS; +//import com.kauailabs.navx.frc.AHRS; import com.studica.frc.AHRS; -import com.studica.frc.AHRS.NavXComType; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -104,7 +103,7 @@ // import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; public class Drivetrain extends SubsystemBase { - private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); // Also try kUSB and kUSB2 + private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); // ^used by PathPlanner for chaining paths @@ -725,7 +724,7 @@ private enum SysIdTest { // private SysIdRoutine.Config defaultSysIdConfig = new // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), // Seconds.of(5)); - private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds.of(1)), Volts.of(2.891), Seconds.of(10)); // DRIVE From 4533dcf9a69c62b249f964b81b273a122b463a6c Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:39:23 -0800 Subject: [PATCH 15/39] Port gyro fixed --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 07e2260..d8e27f3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -23,6 +23,7 @@ import com.ctre.phoenix6.hardware.CANcoder; //import com.kauailabs.navx.frc.AHRS; import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.controllers.PPHolonomicDriveController; @@ -103,7 +104,7 @@ // import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; public class Drivetrain extends SubsystemBase { - private final AHRS gyro = new AHRS(SerialPort.Port.kMXP); // Also try kUSB and kUSB2 + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); // ^used by PathPlanner for chaining paths From f02f32b32de99e7d6433294810c68271cfbd73c2 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:41:20 -0800 Subject: [PATCH 16/39] Fixed the .per timeunit line --- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index d8e27f3..e58e4a5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -725,7 +725,7 @@ private enum SysIdTest { // private SysIdRoutine.Config defaultSysIdConfig = new // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), // Seconds.of(5)); - private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds.of(1)), + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), Volts.of(2.891), Seconds.of(10)); // DRIVE From 2f0fc06fb326df156f5856598cc1aecd2ff00452 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 31 Jan 2025 17:48:26 -0800 Subject: [PATCH 17/39] Created limelighthelper subystem and rotateRelative command and added limelight c constants :) with the help of jonathan --- .../java/org/carlmontrobotics/Constants.java | 35 +- .../commands/RotateToFieldRelativeAngle.java | 51 + .../subsystems/Drivetrain.java | 3 +- .../subsystems/LimelightHelpers.java | 993 ++++++++++++++++++ 4 files changed, 1079 insertions(+), 3 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ce6150a..6881091 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -8,6 +8,7 @@ import org.carlmontrobotics.lib199.swerve.SwerveConfig; import com.pathplanner.lib.config.RobotConfig; +import com.pathplanner.lib.path.PathConstraints; import static org.carlmontrobotics.Config.CONFIG; @@ -226,9 +227,39 @@ public static final class Autoc { } } public static final class Limelightc { - + public static final String INTAKE_LL_NAME = "limelight-intake"; + public static final String SHOOTER_LL_NAME = "limelight-shooter"; + + public static final int[] VALID_IDS = { 4, 7 }; + + public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high + // to represent unreliable heading + public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity + + public static final double ERROR_TOLERANCE_RAD = 0.1; + public static final double HORIZONTAL_FOV_DEG = 0; // unused + public static final double RESOLUTION_WIDTH_PIX = 640; // unused + public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; + public static final double MOUNT_ANGLE_DEG_INTAKE = -29; + public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); + public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = + Units.inchesToMeters(13); + public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused + public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused + public static final double MIN_MOVEMENT_METERSPSEC = 1.5; + public static final double MIN_MOVEMENT_RADSPSEC = 0.5; + public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); + public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); + public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); + public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused public static final class Apriltag { - + public static final int RED_SPEAKER_CENTER_TAG_ID = 4; + public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; + public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); + public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); + public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); } } diff --git a/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java new file mode 100644 index 0000000..d1fb25b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/RotateToFieldRelativeAngle.java @@ -0,0 +1,51 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; + +import org.carlmontrobotics.subsystems.Drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj2.command.Command; + +public class RotateToFieldRelativeAngle extends Command { + + public final TeleopDrive teleopDrive; + public final Drivetrain drivetrain; + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); + + public RotateToFieldRelativeAngle(Rotation2d angle, Drivetrain drivetrain) { + this.drivetrain = drivetrain; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + rotationPID.setSetpoint(MathUtil.inputModulus(angle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + // SmartDashboard.pu + + addRequirements(drivetrain); + } + + @Override + public void execute() { + if (teleopDrive == null) + drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], + rotationPID.calculate(drivetrain.getHeading())); + } + } + + @Override + public boolean isFinished() { + //SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + //SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index e58e4a5..386f477 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -100,6 +100,8 @@ import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volt; import static edu.wpi.first.units.Units.Meter; +import org.carlmontrobotics.subsystems.LimelightHelpers; +import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; // Make sure this code is extraneous // import static edu.wpi.first.units.MutableMeasure.mutable; import static edu.wpi.first.units.Units.Meters; @@ -110,7 +112,6 @@ public class Drivetrain extends SubsystemBase { private SwerveDriveKinematics kinematics = null; // private SwerveDriveOdometry odometry = null; - private SwerveDrivePoseEstimator poseEstimator = null; private SwerveModule modules[]; diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java new file mode 100644 index 0000000..2c03899 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -0,0 +1,993 @@ +//LimelightHelpers v1.5.0 (March 27, 2024) + +package org.carlmontrobotics.subsystems; + +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; + +public class LimelightHelpers { + + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + public static class LimelightTarget_Barcode { + + } + + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Detector() { + } + } + + public static class Results { + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public Results() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + } + + public static class LimelightResults { + @JsonProperty("Results") + public Results targetingResults; + + public String error; + + public LimelightResults() { + targetingResults = new Results(); + error = ""; + } + + } + + public static class RawFiducial { + public int id; + public double txnc; + public double tync; + public double ta; + public double distToCamera; + public double distToRobot; + public double ambiguity; + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + } + } + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + private static Pose3d toPose3D(double[] inData) { + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + private static Pose2d toPose2D(double[] inData) { + if(inData.length < 6) + { + // System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + private static double extractBotPoseEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractBotPoseEntry(poseArray, 6); + int tagCount = (int) extractBotPoseEntry(poseArray, 7); + double tagSpan = extractBotPoseEntry(poseArray, 8); + double tagDist = extractBotPoseEntry(poseArray, 9); + double tagArea = extractBotPoseEntry(poseArray, 10); + // getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the BLUE + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator + * (addVisionMeasurement) when you are on the RED + * alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * The LEDs will be controlled by Limelight pipeline settings, and not by robot + * code. + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + public static void setCameraMode_Processor(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 0); + } + public static void setCameraMode_Driver(String limelightName) { + setLimelightNTDouble(limelightName, "camMode", 1); + } + + + /** + * Sets the crop window. The crop window in the UI must be completely open for + * dynamic cropping to work. + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Parses Limelight's JSON results dump into a LimelightResults Object + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.targetingResults.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} \ No newline at end of file From 397764b45f091ecd44b003872d1f82b96899d40d Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 31 Jan 2025 18:07:27 -0800 Subject: [PATCH 18/39] Fixed some things in robot container --- .pathplanner/settings.json | 14 ++++++ .../java/org/carlmontrobotics/BuildInfo.java | 45 +++++++++++++++++++ .../java/org/carlmontrobotics/Config.java | 2 +- .../org/carlmontrobotics/RobotContainer.java | 5 +-- 4 files changed, 62 insertions(+), 4 deletions(-) create mode 100644 .pathplanner/settings.json create mode 100644 src/main/java/org/carlmontrobotics/BuildInfo.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json new file mode 100644 index 0000000..26bb21b --- /dev/null +++ b/.pathplanner/settings.json @@ -0,0 +1,14 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [ + "Center Limelight 4 Piece" + ], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "maxModuleSpeed": 4.5 + } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/BuildInfo.java b/src/main/java/org/carlmontrobotics/BuildInfo.java new file mode 100644 index 0000000..2973583 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/BuildInfo.java @@ -0,0 +1,45 @@ +package org.carlmontrobotics; + +import java.util.Properties; +import java.io.File; +import java.io.InputStream; +import java.nio.file.Path; +import java.nio.file.Files; + +import edu.wpi.first.util.sendable.Sendable; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Filesystem; + +public class BuildInfo implements Sendable { + private Properties props = new Properties(); + + static private BuildInfo instance = null; + + public static BuildInfo getInstance() { + if (instance == null) { + instance = new BuildInfo(); + } + return instance; + } + + private BuildInfo() { + Path path = Path + .of(Filesystem.getDeployDirectory().getAbsolutePath() + File.separator + "BuildInfo.properties"); + try (InputStream is = Files.newInputStream(path)) { + props.load(is); + } catch (Exception ex) { + System.err.println("Error reading build properties from %s".formatted(path)); + } + } + + @Override + public void initSendable(SendableBuilder builder) { + props.stringPropertyNames().forEach(name -> { + var value = props.getProperty(name); + // Workaround bug (https://github.com/lessthanoptimal/gversion-plugin/pull/14) + // where the gversion plugin surrounds values with quotes. + value = value.replaceAll("\"", ""); + builder.publishConstString(name, value); + }); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index f8a0362..9a68135 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -116,4 +116,4 @@ public String toString() { } return stringBuilder.toString(); } -} +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index af6d906..cd5fb5a 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -47,13 +47,12 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; - public class RobotContainer { private static boolean babyMode = false; // 1. using GenericHID allows us to use different kinds of controllers // 2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(Driver.port); + public final GenericHID driverController = new GenericHID(OI.Driver.port); public final GenericHID manipulatorController = new GenericHID(Manipulator.port); private final Drivetrain drivetrain = new Drivetrain(); @@ -112,7 +111,7 @@ private void setDefaultCommands() { () -> ProcessedAxisValue(driverController, Axis.kLeftY), () -> ProcessedAxisValue(driverController, Axis.kLeftX), () -> ProcessedAxisValue(driverController, Axis.kRightX), - () -> driverController.getRawButton(Driver.slowDriveButton))); + () -> driverController.getRawButton(OI.Driver.slowDriveButton))); } private void setBindingsDriver() {} private void setBindingsManipulator() {} From bacfacc89a152474309f6612449b6822e25803dc Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 18:35:18 -0800 Subject: [PATCH 19/39] Final fixes --- .../java/org/carlmontrobotics/Constants.java | 34 ++----------------- .../org/carlmontrobotics/RobotContainer.java | 14 ++++---- .../commands/TeleopDrive.java | 2 +- 3 files changed, 10 insertions(+), 40 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6881091..7e84cae 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -227,39 +227,9 @@ public static final class Autoc { } } public static final class Limelightc { - public static final String INTAKE_LL_NAME = "limelight-intake"; - public static final String SHOOTER_LL_NAME = "limelight-shooter"; - - public static final int[] VALID_IDS = { 4, 7 }; - - public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field - public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field - public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high - // to represent unreliable heading - public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity - - public static final double ERROR_TOLERANCE_RAD = 0.1; - public static final double HORIZONTAL_FOV_DEG = 0; // unused - public static final double RESOLUTION_WIDTH_PIX = 640; // unused - public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; - public static final double MOUNT_ANGLE_DEG_INTAKE = -29; - public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); - public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = - Units.inchesToMeters(13); - public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused - public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused - public static final double MIN_MOVEMENT_METERSPSEC = 1.5; - public static final double MIN_MOVEMENT_RADSPSEC = 0.5; - public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); - public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); - public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); - public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused + public static final class Apriltag { - public static final int RED_SPEAKER_CENTER_TAG_ID = 4; - public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; - public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); - public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); - public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); + } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index cd5fb5a..f1ee3ec 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -10,7 +10,9 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.commands.PathPlannerAuto; import com.pathplanner.lib.path.GoalEndState; +import com.pathplanner.lib.path.IdealStartingState; import com.pathplanner.lib.path.PathPlannerPath; +import com.pathplanner.lib.path.Waypoint; import org.carlmontrobotics.Constants.Drivetrainc.Autoc; import org.carlmontrobotics.Constants.OI; @@ -19,8 +21,6 @@ import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; -import java.io.ObjectInputFilter.Config; -import java.sql.Driver; import java.util.ArrayList; import java.util.List; @@ -220,9 +220,9 @@ private void setupAutos() { // NOTHING autoCommands.add(0, new PrintCommand("Running NULL Auto!")); // RAW FORWARD command - autoCommands.add(1, new SequentialCommandGroup( - new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new WaitCommand(0.5), - new LastResortAuto(drivetrain))); + // autoCommands.add(1, new SequentialCommandGroup( + // new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new WaitCommand(0.5), + // new LastResortAuto(drivetrain))); // dumb PP forward command autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!")); } @@ -249,11 +249,11 @@ public Command constructPPSimpleAuto() { ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); - List bezierPoints = PathPlannerPath.bezierFromPoses(currPos, endPos); + List bezierPoints = PathPlannerPath.waypointsFromPoses(currPos, endPos); // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, rad/s^2 */ PathPlannerPath path = new PathPlannerPath(bezierPoints, - Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); + Autoc.pathConstraints, null, new GoalEndState(0, currPos.getRotation())); path.preventFlipping = false;// don't flip, we do that manually already. diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java index 26abc7c..1689d27 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -18,7 +18,7 @@ public class TeleopDrive extends Command { - private static double robotPeriod = Robot.robot.getPeriod(); + private static double robotPeriod = Robot.kDefaultPeriod; private final Drivetrain drivetrain; private DoubleSupplier fwd; private DoubleSupplier str; From 78b6ae7984a26d13deb77117225bff9d55682837 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 18:52:28 -0800 Subject: [PATCH 20/39] Wheel base and trackwidth added --- .../java/org/carlmontrobotics/Constants.java | 22 +-- .../subsystems/Drivetrain.java | 134 +++++++++--------- 2 files changed, 78 insertions(+), 78 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7e84cae..12da64e 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -51,8 +51,8 @@ public static final class Armc { public static final class Drivetrainc { // #region Subsystem Constants - public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : Units.inchesToMeters(16.75); - public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : Units.inchesToMeters(23.75); + public static final double wheelBase = 24.75; //CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) : Units.inchesToMeters(16.75); + public static final double trackWidth = 24.75;//CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) : Units.inchesToMeters(23.75); // "swerveRadius" is the distance from the center of the robot to one of the // modules public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); @@ -212,15 +212,15 @@ public static final class Drivetrainc { public static final double driveIzone = .1; public static final class Autoc { - public static final RobotConfig robotConfig = new RobotConfig( /* - * put in - * Constants.Drivetrain.Auto - */ - false, // replan at start of path if robot not at start of path? - false, // replan if total error surpasses total error/spike threshold? - 1.5, // total error threshold in meters that will cause the path to be replanned - 0.8 // error spike threshold, in meters, that will cause the path to be replanned - ); + // public static final RobotConfig robotConfig = new RobotConfig( /* + // * put in + // * Constants.Drivetrain.Auto + // */ + // false, // replan at start of path if robot not at start of path? + // false, // replan if total error surpasses total error/spike threshold? + // 1.5, // total error threshold in meters that will cause the path to be replanned + // 0.8 // error spike threshold, in meters, that will cause the path to be replanned + // ); public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the // angular constraints have no effect. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 386f477..8e6fc41 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -278,7 +278,7 @@ public Drivetrain() { new Pose2d()); // Setup autopath builder - configurePPLAutoBuilder(); + //configurePPLAutoBuilder(); // SmartDashboard.putNumber("chassis speeds x", 0); // SmartDashboard.putNumber("chassis speeds y", 0); @@ -358,7 +358,7 @@ public void periodic() { poseEstimator.update(gyro.getRotation2d(), getModulePositions()); //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); - updateMT2PoseEstimator(); + // updateMT2PoseEstimator(); // double currSetX = // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); @@ -455,46 +455,46 @@ public void drive(SwerveModuleState[] moduleStates) { } } - public void configurePPLAutoBuilder() { - /** - * PATHPLANNER SETTINGS - * Robot Width (m): .91 - * Robot Length(m): .94 - * Max Module Spd (m/s): 4.30 - * Default Constraints - * Max Vel: 1.54, Max Accel: 6.86 - * Max Angvel: 360, Max AngAccel: 180 (guesses!) - */ - AutoBuilder.configure( - this::getPose, // :D - this::setPose, // :D - this::getSpeeds, // :D - (ChassisSpeeds cs) -> { - //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; - // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); - // SmartDashboard.putNumber("chassis speeds y", cs.vyMetersPerSecond); - // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); - - drive(kinematics.toSwerveModuleStates(cs)); - }, // :D - new PPHolonomicDriveController( - new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals - new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals - Robot.kDefaultPeriod//robot period - ), - Autoc.robotConfig, - () -> { - // Boolean supplier that controls when the path will be mirrored for the red alliance - // This will flip the path being followed to the red side of the field. - // THE ORIGIN WILL REMAIN ON THE BLUE SIDE - var alliance = DriverStation.getAlliance(); - if (alliance.isPresent()) - return alliance.get() == DriverStation.Alliance.Red; - //else: - return false; - }, // :D - this // :D - ); +// public void configurePPLAutoBuilder() { +// /** +// * PATHPLANNER SETTINGS +// * Robot Width (m): .91 +// * Robot Length(m): .94 +// * Max Module Spd (m/s): 4.30 +// * Default Constraints +// * Max Vel: 1.54, Max Accel: 6.86 +// * Max Angvel: 360, Max AngAccel: 180 (guesses!) +// */ +// AutoBuilder.configure( +// this::getPose, // :D +// this::setPose, // :D +// this::getSpeeds, // :D +// (ChassisSpeeds cs) -> { +// //cs.vxMetersPerSecond = -cs.vxMetersPerSecond; +// // SmartDashboard.putNumber("chassis speeds x", cs.vxMetersPerSecond); +// // SmartDashboard.putNumber("chassis speeds y", cs.vyMetersPerSecond); +// // SmartDashboard.putNumber("chassis speeds theta", cs.omegaRadiansPerSecond); + +// drive(kinematics.toSwerveModuleStates(cs)); +// }, // :D +// new PPHolonomicDriveController( +// new PIDConstants(xPIDController[0], xPIDController[1], xPIDController[2], 0), //translation (drive) pid vals +// new PIDConstants(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2], 0), //rotation pid vals +// Robot.kDefaultPeriod//robot period +// ), +// Autoc.robotConfig, +// () -> { +// // Boolean supplier that controls when the path will be mirrored for the red alliance +// // This will flip the path being followed to the red side of the field. +// // THE ORIGIN WILL REMAIN ON THE BLUE SIDE +// var alliance = DriverStation.getAlliance(); +// if (alliance.isPresent()) +// return alliance.get() == DriverStation.Alliance.Red; +// //else: +// return false; +// }, // :D +// this // :D +// ); /* AutoBuilder.configureHolonomic( @@ -528,7 +528,7 @@ public void configurePPLAutoBuilder() { this ); */ - } +// } public void autoCancelDtCommand() { if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; @@ -1091,30 +1091,30 @@ public void keepRotateMotorsAtDegrees(int angle) { // pose estimator stuff - public void updateMT2PoseEstimator() { - boolean rejectVisionUpdate = false; - - LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME, - poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); - LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers - .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); - - if (Math.abs(getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second - rejectVisionUpdate = true; - } - - if (visionPoseEstimate.tagCount == 0) { - rejectVisionUpdate = true; - } - - if (!rejectVisionUpdate) { - poseEstimator - .setVisionMeasurementStdDevs( - VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, - STD_DEV_HEADING_RADS)); - poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds); - } - } + // public void updateMT2PoseEstimator() { + // boolean rejectVisionUpdate = false; + + // LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME, + // poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); + // LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers + // .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); + + // if (Math.abs(getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second + // rejectVisionUpdate = true; + // } + + // if (visionPoseEstimate.tagCount == 0) { + // rejectVisionUpdate = true; + // } + + // if (!rejectVisionUpdate) { + // poseEstimator + // .setVisionMeasurementStdDevs( + // VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, + // STD_DEV_HEADING_RADS)); + // poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds); + // } + // } public double getGyroRate() { return gyro.getRate(); From b5aa93ea62dfbd6e38e3a2473656bf56ce86d551 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Fri, 31 Jan 2025 20:25:41 -0800 Subject: [PATCH 21/39] Changed motors to new rev lib ones --- .../java/org/carlmontrobotics/Constants.java | 6 +- .../subsystems/Drivetrain.java | 56 ++++++++++--------- 2 files changed, 32 insertions(+), 30 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 12da64e..b3029e5 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -95,12 +95,12 @@ public static final class Drivetrainc { // kP, kI, and kD constants for turn motor controllers in the order of // front-left, front-right, back-left, back-right. // Determine correct turn PID constants - public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = { 0, 0, 0, 0 }; // {0.00374, 0.00374, 0.00374, // 0.00374}; public static final double[] turnkI = { 0, 0, 0, 0 }; - public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d + public static final double[] turnkD = { 0/* dont edit */, 0, 0, 0}; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; + public static final double[] turnkS = { 0, 0, 0, 0 }; // V = kS + kV * v + kA * a // 12 = 0.2 + 0.00463 * v diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 8e6fc41..cda4b6c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -9,6 +9,7 @@ import java.util.Map; import java.util.function.Supplier; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.Constants.Drivetrainc.Autoc; import org.carlmontrobotics.Robot; // import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; @@ -35,6 +36,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; @@ -109,7 +111,7 @@ public class Drivetrain extends SubsystemBase { private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); // ^used by PathPlanner for chaining paths - + private SwerveDriveKinematics kinematics = null; // private SwerveDriveOdometry odometry = null; private SwerveDrivePoseEstimator poseEstimator = null; @@ -121,7 +123,7 @@ public class Drivetrain extends SubsystemBase { private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; - + // gyro public final float initPitch; public final float initRoll; @@ -141,6 +143,7 @@ public class Drivetrain extends SubsystemBase { private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; public Drivetrain() { + // SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", @@ -195,30 +198,26 @@ public Drivetrain() { // Supplier pitchSupplier = () -> gyro.getPitch(); // Supplier rollSupplier = () -> gyro.getRoll(); - moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorErrors.TemperatureLimit.NEO), - turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorErrors.TemperatureLimit.NEO), - turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, - pitchSupplier, rollSupplier); - // Forward-Right - moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorErrors.TemperatureLimit.NEO), - turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorErrors.TemperatureLimit.NEO), - turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, - pitchSupplier, rollSupplier); - - // Backward-Left - moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, - driveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorErrors.TemperatureLimit.NEO), - turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorErrors.TemperatureLimit.NEO), - turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, - pitchSupplier, rollSupplier); - // Backward-Right - moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, - driveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorErrors.TemperatureLimit.NEO), - turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorErrors.TemperatureLimit.NEO), - turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, - pitchSupplier, rollSupplier); + + moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, + driveMotors[0] = new SparkMax(0, MotorType.kBrushless), + turnMotors[0] = new SparkMax(1, MotorType.kBrushless), + turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + + moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, + driveMotors[1] = new SparkMax(2, MotorType.kBrushless), + turnMotors[1] = new SparkMax(3, MotorType.kBrushless), + turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, + driveMotors[2] = new SparkMax(4, MotorType.kBrushless), + turnMotors[2] = new SparkMax(5, MotorType.kBrushless), + turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, + driveMotors[3] = new SparkMax(6, MotorType.kBrushless), + turnMotors[3] = new SparkMax(7, MotorType.kBrushless), + turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; if (RobotBase.isSimulation()) { @@ -329,6 +328,9 @@ public void simulationPeriodic() { @Override public void periodic() { + SmartDashboard.getNumber("Velocity FL: ", turnEncoders[0].getVelocity().getValueAsDouble()); + turnMotors[0].setVoltage(SmartDashboard.getNumber("goal Velocity", 0)); + // for (CANcoder coder : turnEncoders) { // SignalLogger.writeDouble("Regular position " + coder.toString(), // coder.getPosition().getValue()); @@ -450,7 +452,7 @@ public void drive(SwerveModuleState[] moduleStates) { moduleStates[i] = SwerveModuleState.optimize(moduleStates[i], Rotation2d.fromDegrees(modules[i].getModuleAngle())); // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); - + modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); } } From 2f141a8fcdf53a329660f5a4b6d82d21ac1a7a3a Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Fri, 31 Jan 2025 20:46:32 -0800 Subject: [PATCH 22/39] Added temporary pid testing code to guess and check later --- .../subsystems/Drivetrain.java | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index cda4b6c..07c2cb9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -7,9 +7,11 @@ import java.util.Arrays; import java.util.Map; +import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Drivetrainc; import org.carlmontrobotics.Constants.Drivetrainc.Autoc; import org.carlmontrobotics.Robot; // import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; @@ -33,12 +35,17 @@ import com.pathplanner.lib.config.RobotConfig; import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; @@ -135,15 +142,19 @@ public class Drivetrain extends SubsystemBase { private SwerveModule moduleBR; private final Field2d field = new Field2d(); - + private final SparkClosedLoopController pidController = turnMotors[0].getClosedLoopController(); private SwerveModuleSim[] moduleSims; private SimDouble gyroYawSim; private Timer simTimer = new Timer(); private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; - + double kP = 0; + double kI = 0; + double kD = 0; public Drivetrain() { - + SparkMaxConfig c = new SparkMaxConfig(); + c.closedLoop.pid(kP, kI, kP).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + turnMotors[0].configure(c, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); // SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", @@ -329,7 +340,12 @@ public void simulationPeriodic() { @Override public void periodic() { SmartDashboard.getNumber("Velocity FL: ", turnEncoders[0].getVelocity().getValueAsDouble()); - turnMotors[0].setVoltage(SmartDashboard.getNumber("goal Velocity", 0)); + double velocity = SmartDashboard.getNumber("Goal Velocity", 0); + + kP = SmartDashboard.getNumber("kP", 0); + kI = SmartDashboard.getNumber("kI", 0); + kD = SmartDashboard.getNumber("kD", 0); + pidController.setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0); // for (CANcoder coder : turnEncoders) { // SignalLogger.writeDouble("Regular position " + coder.toString(), From 451085c5afc4bce636b0ed66a1086a700fec9e74 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 1 Feb 2025 17:24:53 -0800 Subject: [PATCH 23/39] Updated LimelightHelpers. --- .../subsystems/LimelightHelpers.java | 854 ++++++++++++++++-- 1 file changed, 754 insertions(+), 100 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java index 2c03899..b7b3a0e 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -1,10 +1,14 @@ -//LimelightHelpers v1.5.0 (March 27, 2024) +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) package org.carlmontrobotics.subsystems; +import edu.wpi.first.networktables.DoubleArrayEntry; import edu.wpi.first.networktables.NetworkTable; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import org.carlmontrobotics.subsystems.LimelightHelpers.LimelightResults; +import org.carlmontrobotics.subsystems.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -17,6 +21,7 @@ import java.net.HttpURLConnection; import java.net.MalformedURLException; import java.net.URL; +import java.util.Map; import java.util.concurrent.CompletableFuture; import com.fasterxml.jackson.annotation.JsonFormat; @@ -25,9 +30,19 @@ import com.fasterxml.jackson.core.JsonProcessingException; import com.fasterxml.jackson.databind.DeserializationFeature; import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ public class LimelightHelpers { + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ public static class LimelightTarget_Retro { @JsonProperty("t6c_ts") @@ -92,16 +107,22 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("tx") public double tx; + + @JsonProperty("ty") + public double ty; @JsonProperty("txp") public double tx_pixels; - @JsonProperty("ty") - public double ty; - @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + @JsonProperty("ts") public double ts; @@ -115,6 +136,9 @@ public LimelightTarget_Retro() { } + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ public static class LimelightTarget_Fiducial { @JsonProperty("fID") @@ -186,15 +210,21 @@ public Pose2d getTargetPose_RobotSpace2D() @JsonProperty("tx") public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") public double ty; + @JsonProperty("txp") + public double tx_pixels; + @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + @JsonProperty("ts") public double ts; @@ -207,10 +237,58 @@ public LimelightTarget_Fiducial() { } } + /** + * Represents a Barcode Target Result extracted from JSON Output + */ public static class LimelightTarget_Barcode { + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } } + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ public static class LimelightTarget_Classifier { @JsonProperty("class") @@ -241,6 +319,9 @@ public LimelightTarget_Classifier() { } } + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ public static class LimelightTarget_Detector { @JsonProperty("class") @@ -258,21 +339,32 @@ public static class LimelightTarget_Detector { @JsonProperty("tx") public double tx; - @JsonProperty("txp") - public double tx_pixels; - @JsonProperty("ty") public double ty; + @JsonProperty("txp") + public double tx_pixels; + @JsonProperty("typ") public double ty_pixels; + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + public LimelightTarget_Detector() { } } - public static class Results { - + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + @JsonProperty("pID") public double pipelineID; @@ -305,13 +397,13 @@ public static class Results { @JsonProperty("botpose_tagcount") public double botpose_tagcount; - + @JsonProperty("botpose_span") public double botpose_span; - + @JsonProperty("botpose_avgdist") public double botpose_avgdist; - + @JsonProperty("botpose_avgarea") public double botpose_avgarea; @@ -357,7 +449,7 @@ public Pose2d getBotPose2d_wpiBlue() { @JsonProperty("Barcode") public LimelightTarget_Barcode[] targets_Barcode; - public Results() { + public LimelightResults() { botpose = new double[6]; botpose_wpired = new double[6]; botpose_wpiblue = new double[6]; @@ -369,32 +461,24 @@ public Results() { targets_Barcode = new LimelightTarget_Barcode[0]; } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public String error; - public LimelightResults() { - targetingResults = new Results(); - error = ""; - } } + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, - double ambiguity) { + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { this.id = id; this.txnc = txnc; this.tync = tync; @@ -405,6 +489,47 @@ public RawFiducial(int id, double txnc, double tync, double ta, double distToCam } } + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ public static class PoseEstimate { public Pose2d pose; public double timestampSeconds; @@ -413,11 +538,28 @@ public static class PoseEstimate { public double tagSpan; public double avgTagDist; public double avgTagArea; - public RawFiducial[] rawFiducials; - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials) { + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { this.pose = pose; this.timestampSeconds = timestampSeconds; @@ -427,9 +569,45 @@ public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, this.avgTagDist = avgTagDist; this.avgTagArea = avgTagArea; this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; } + } + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + private static ObjectMapper mapper; /** @@ -444,7 +622,13 @@ static final String sanitizeName(String name) { return name; } - private static Pose3d toPose3D(double[] inData) { + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ if(inData.length < 6) { //System.err.println("Bad LL 3D Pose Data!"); @@ -456,10 +640,17 @@ private static Pose3d toPose3D(double[] inData) { Units.degreesToRadians(inData[5]))); } - private static Pose2d toPose2D(double[] inData) { + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ if(inData.length < 6) { - // System.err.println("Bad LL 2D Pose Data!"); + //System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -467,35 +658,83 @@ private static Pose2d toPose2D(double[] inData) { return new Pose2d(tran2d, r2d); } - private static double extractBotPoseEntry(double[] inData, int position) { - if (inData.length < position + 1) { + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { return 0; } return inData[position]; } - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + var pose = toPose2D(poseArray); - double latency = extractBotPoseEntry(poseArray, 6); - int tagCount = (int) extractBotPoseEntry(poseArray, 7); - double tagSpan = extractBotPoseEntry(poseArray, 8); - double tagDist = extractBotPoseEntry(poseArray, 9); - double tagArea = extractBotPoseEntry(poseArray, 10); - // getlastchange() in microseconds, ll latency in milliseconds - var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); - + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; int valsPerFiducial = 7; int expectedTotalVals = 11 + valsPerFiducial * tagCount; - + if (poseArray.length != expectedTotalVals) { // Don't populate fiducials } else { - for (int i = 0; i < tagCount; i++) { + for(int i = 0; i < tagCount; i++) { int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) poseArray[baseIndex]; + int id = (int)poseArray[baseIndex]; double txnc = poseArray[baseIndex + 1]; double tync = poseArray[baseIndex + 2]; double ta = poseArray[baseIndex + 3]; @@ -505,16 +744,94 @@ private static PoseEstimate getBotPoseEstimate(String limelightName, String entr rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); } } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } - return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; } - private static void printPoseEstimate(PoseEstimate pose) { + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { if (pose == null) { System.out.println("No PoseEstimate available."); return; } - + System.out.printf("Pose Estimate Information:%n"); System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); System.out.printf("Latency: %.3f ms%n", pose.latency); @@ -522,13 +839,14 @@ private static void printPoseEstimate(PoseEstimate pose) { System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); System.out.println(); - + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { System.out.println("No RawFiducials data available."); return; } - + System.out.println("Raw Fiducials Details:"); for (int i = 0; i < pose.rawFiducials.length; i++) { RawFiducial fiducial = pose.rawFiducials[i]; @@ -544,14 +862,30 @@ private static void printPoseEstimate(PoseEstimate pose) { } } + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { return getLimelightNTTable(tableName).getEntry(entryName); } + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + public static double getLimelightNTDouble(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); } @@ -568,10 +902,16 @@ public static double[] getLimelightNTDoubleArray(String tableName, String entryN return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); } + public static String getLimelightNTString(String tableName, String entryName) { return getLimelightNTTableEntry(tableName, entryName).getString(""); } + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + public static URL getLimelightURLString(String tableName, String request) { String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; URL url; @@ -586,30 +926,171 @@ public static URL getLimelightURLString(String tableName, String request) { ///// ///// + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ public static double getTX(String limelightName) { return getLimelightNTDouble(limelightName, "tx"); } + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ public static double getTY(String limelightName) { return getLimelightNTDouble(limelightName, "ty"); } + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ public static double getTA(String limelightName) { return getLimelightNTDouble(limelightName, "ta"); } + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ public static double getLatency_Pipeline(String limelightName) { return getLimelightNTDouble(limelightName, "tl"); } + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ public static double getLatency_Capture(String limelightName) { return getLimelightNTDouble(limelightName, "cl"); } + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ public static double getCurrentPipelineIndex(String limelightName) { return getLimelightNTDouble(limelightName, "getpipe"); } + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ public static String getJSONDump(String limelightName) { return getLimelightNTString(limelightName, "json"); } @@ -687,6 +1168,10 @@ public static String getNeuralClassID(String limelightName) { return getLimelightNTString(limelightName, "tclass"); } + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + ///// ///// @@ -695,36 +1180,71 @@ public static Pose3d getBotPose3d(String limelightName) { return toPose3D(poseArray); } + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ public static Pose3d getBotPose3d_wpiRed(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); return toPose3D(poseArray); } + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ public static Pose3d getBotPose3d_wpiBlue(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); return toPose3D(poseArray); } + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ public static Pose3d getBotPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); return toPose3D(poseArray); } + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); return toPose3D(poseArray); } + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); return toPose3D(poseArray); } + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); return toPose3D(poseArray); } + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); return toPose3D(poseArray); @@ -744,27 +1264,24 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the BLUE - * alliance + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the BLUE - * alliance + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); } /** @@ -782,27 +1299,23 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the RED + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED * alliance - * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); + return getBotPoseEstimate(limelightName, "botpose_wpired", false); } /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the RED + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED * alliance - * * @param limelightName * @return */ public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); } /** @@ -818,9 +1331,21 @@ public static Pose2d getBotPose2d(String limelightName) { return toPose2D(result); } - - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); } ///// @@ -830,13 +1355,14 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + public static void setPriorityTagID(String limelightName, int ID) { setLimelightNTDouble(limelightName, "priorityid", ID); } /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot - * code. + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera */ public static void setLEDMode_PipelineControl(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 0); @@ -854,29 +1380,38 @@ public static void setLEDMode_ForceOn(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 3); } + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_Standard(String limelightName) { setLimelightNTDouble(limelightName, "stream", 0); } + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_PiPMain(String limelightName) { setLimelightNTDouble(limelightName, "stream", 1); } + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ public static void setStreamMode_PiPSecondary(String limelightName) { setLimelightNTDouble(limelightName, "stream", 2); } - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) */ public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { double[] entries = new double[4]; @@ -886,10 +1421,44 @@ public static void setCropWindow(String limelightName, double cropXMin, double c entries[3] = cropYMax; setLimelightNTDoubleArray(limelightName, "crop", entries); } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { double[] entries = new double[6]; entries[0] = yaw; @@ -899,16 +1468,99 @@ public static void SetRobotOrientation(String limelightName, double yaw, double entries[4] = roll; entries[5] = rollRate; setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); } + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { double[] validIDsDouble = new double[validIDs.length]; for (int i = 0; i < validIDs.length; i++) { validIDsDouble[i] = validIDs[i]; - } + } setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); } + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { double[] entries = new double[6]; entries[0] = forward; @@ -965,7 +1617,9 @@ private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) } /** - * Parses Limelight's JSON results dump into a LimelightResults Object + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data */ public static LimelightResults getLatestResults(String limelightName) { @@ -983,7 +1637,7 @@ public static LimelightResults getLatestResults(String limelightName) { long end = System.nanoTime(); double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; + results.latency_jsonParse = millis; if (profileJSON) { System.out.printf("lljson: %.2f\r\n", millis); } From 1cd5156b9a87f8842dc1d008ff18f43f7bf932a5 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 1 Feb 2025 18:22:01 -0800 Subject: [PATCH 24/39] Limelight yeeted no need for limelight in driverstation, code that did need is also deleted, will be added later in the limelight branch. --- .../subsystems/Drivetrain.java | 31 - .../subsystems/LimelightHelpers.java | 993 ------------------ 2 files changed, 1024 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 07c2cb9..8fe50dc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -3,11 +3,9 @@ package org.carlmontrobotics.subsystems; import static org.carlmontrobotics.Constants.Drivetrainc.*; -import static org.carlmontrobotics.Constants.Limelightc.*; import java.util.Arrays; import java.util.Map; -import java.util.function.DoubleSupplier; import java.util.function.Supplier; import org.carlmontrobotics.Constants; @@ -104,12 +102,10 @@ import static edu.wpi.first.units.Units.Volts; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Rotation; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Seconds; import static edu.wpi.first.units.Units.Volt; import static edu.wpi.first.units.Units.Meter; -import org.carlmontrobotics.subsystems.LimelightHelpers; import org.carlmontrobotics.commands.RotateToFieldRelativeAngle; // Make sure this code is extraneous // import static edu.wpi.first.units.MutableMeasure.mutable; @@ -1107,33 +1103,6 @@ public void keepRotateMotorsAtDegrees(int angle) { } } - // pose estimator stuff - - // public void updateMT2PoseEstimator() { - // boolean rejectVisionUpdate = false; - - // LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME, - // poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0); - // LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers - // .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME); - - // if (Math.abs(getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second - // rejectVisionUpdate = true; - // } - - // if (visionPoseEstimate.tagCount == 0) { - // rejectVisionUpdate = true; - // } - - // if (!rejectVisionUpdate) { - // poseEstimator - // .setVisionMeasurementStdDevs( - // VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, - // STD_DEV_HEADING_RADS)); - // poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds); - // } - // } - public double getGyroRate() { return gyro.getRate(); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java deleted file mode 100644 index 2c03899..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java +++ /dev/null @@ -1,993 +0,0 @@ -//LimelightHelpers v1.5.0 (March 27, 2024) - -package org.carlmontrobotics.subsystems; - -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.geometry.Translation2d; - -import java.io.IOException; -import java.net.HttpURLConnection; -import java.net.MalformedURLException; -import java.net.URL; -import java.util.concurrent.CompletableFuture; - -import com.fasterxml.jackson.annotation.JsonFormat; -import com.fasterxml.jackson.annotation.JsonFormat.Shape; -import com.fasterxml.jackson.annotation.JsonProperty; -import com.fasterxml.jackson.core.JsonProcessingException; -import com.fasterxml.jackson.databind.DeserializationFeature; -import com.fasterxml.jackson.databind.ObjectMapper; - -public class LimelightHelpers { - - public static class LimelightTarget_Retro { - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Retro() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - - } - - public static class LimelightTarget_Fiducial { - - @JsonProperty("fID") - public double fiducialID; - - @JsonProperty("fam") - public String fiducialFamily; - - @JsonProperty("t6c_ts") - private double[] cameraPose_TargetSpace; - - @JsonProperty("t6r_fs") - private double[] robotPose_FieldSpace; - - @JsonProperty("t6r_ts") - private double[] robotPose_TargetSpace; - - @JsonProperty("t6t_cs") - private double[] targetPose_CameraSpace; - - @JsonProperty("t6t_rs") - private double[] targetPose_RobotSpace; - - public Pose3d getCameraPose_TargetSpace() - { - return toPose3D(cameraPose_TargetSpace); - } - public Pose3d getRobotPose_FieldSpace() - { - return toPose3D(robotPose_FieldSpace); - } - public Pose3d getRobotPose_TargetSpace() - { - return toPose3D(robotPose_TargetSpace); - } - public Pose3d getTargetPose_CameraSpace() - { - return toPose3D(targetPose_CameraSpace); - } - public Pose3d getTargetPose_RobotSpace() - { - return toPose3D(targetPose_RobotSpace); - } - - public Pose2d getCameraPose_TargetSpace2D() - { - return toPose2D(cameraPose_TargetSpace); - } - public Pose2d getRobotPose_FieldSpace2D() - { - return toPose2D(robotPose_FieldSpace); - } - public Pose2d getRobotPose_TargetSpace2D() - { - return toPose2D(robotPose_TargetSpace); - } - public Pose2d getTargetPose_CameraSpace2D() - { - return toPose2D(targetPose_CameraSpace); - } - public Pose2d getTargetPose_RobotSpace2D() - { - return toPose2D(targetPose_RobotSpace); - } - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - @JsonProperty("ts") - public double ts; - - public LimelightTarget_Fiducial() { - cameraPose_TargetSpace = new double[6]; - robotPose_FieldSpace = new double[6]; - robotPose_TargetSpace = new double[6]; - targetPose_CameraSpace = new double[6]; - targetPose_RobotSpace = new double[6]; - } - } - - public static class LimelightTarget_Barcode { - - } - - public static class LimelightTarget_Classifier { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("zone") - public double zone; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Classifier() { - } - } - - public static class LimelightTarget_Detector { - - @JsonProperty("class") - public String className; - - @JsonProperty("classID") - public double classID; - - @JsonProperty("conf") - public double confidence; - - @JsonProperty("ta") - public double ta; - - @JsonProperty("tx") - public double tx; - - @JsonProperty("txp") - public double tx_pixels; - - @JsonProperty("ty") - public double ty; - - @JsonProperty("typ") - public double ty_pixels; - - public LimelightTarget_Detector() { - } - } - - public static class Results { - - @JsonProperty("pID") - public double pipelineID; - - @JsonProperty("tl") - public double latency_pipeline; - - @JsonProperty("cl") - public double latency_capture; - - public double latency_jsonParse; - - @JsonProperty("ts") - public double timestamp_LIMELIGHT_publish; - - @JsonProperty("ts_rio") - public double timestamp_RIOFPGA_capture; - - @JsonProperty("v") - @JsonFormat(shape = Shape.NUMBER) - public boolean valid; - - @JsonProperty("botpose") - public double[] botpose; - - @JsonProperty("botpose_wpired") - public double[] botpose_wpired; - - @JsonProperty("botpose_wpiblue") - public double[] botpose_wpiblue; - - @JsonProperty("botpose_tagcount") - public double botpose_tagcount; - - @JsonProperty("botpose_span") - public double botpose_span; - - @JsonProperty("botpose_avgdist") - public double botpose_avgdist; - - @JsonProperty("botpose_avgarea") - public double botpose_avgarea; - - @JsonProperty("t6c_rs") - public double[] camerapose_robotspace; - - public Pose3d getBotPose3d() { - return toPose3D(botpose); - } - - public Pose3d getBotPose3d_wpiRed() { - return toPose3D(botpose_wpired); - } - - public Pose3d getBotPose3d_wpiBlue() { - return toPose3D(botpose_wpiblue); - } - - public Pose2d getBotPose2d() { - return toPose2D(botpose); - } - - public Pose2d getBotPose2d_wpiRed() { - return toPose2D(botpose_wpired); - } - - public Pose2d getBotPose2d_wpiBlue() { - return toPose2D(botpose_wpiblue); - } - - @JsonProperty("Retro") - public LimelightTarget_Retro[] targets_Retro; - - @JsonProperty("Fiducial") - public LimelightTarget_Fiducial[] targets_Fiducials; - - @JsonProperty("Classifier") - public LimelightTarget_Classifier[] targets_Classifier; - - @JsonProperty("Detector") - public LimelightTarget_Detector[] targets_Detector; - - @JsonProperty("Barcode") - public LimelightTarget_Barcode[] targets_Barcode; - - public Results() { - botpose = new double[6]; - botpose_wpired = new double[6]; - botpose_wpiblue = new double[6]; - camerapose_robotspace = new double[6]; - targets_Retro = new LimelightTarget_Retro[0]; - targets_Fiducials = new LimelightTarget_Fiducial[0]; - targets_Classifier = new LimelightTarget_Classifier[0]; - targets_Detector = new LimelightTarget_Detector[0]; - targets_Barcode = new LimelightTarget_Barcode[0]; - - } - } - - public static class LimelightResults { - @JsonProperty("Results") - public Results targetingResults; - - public String error; - - public LimelightResults() { - targetingResults = new Results(); - error = ""; - } - - } - - public static class RawFiducial { - public int id; - public double txnc; - public double tync; - public double ta; - public double distToCamera; - public double distToRobot; - public double ambiguity; - - public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, - double ambiguity) { - this.id = id; - this.txnc = txnc; - this.tync = tync; - this.ta = ta; - this.distToCamera = distToCamera; - this.distToRobot = distToRobot; - this.ambiguity = ambiguity; - } - } - - public static class PoseEstimate { - public Pose2d pose; - public double timestampSeconds; - public double latency; - public int tagCount; - public double tagSpan; - public double avgTagDist; - public double avgTagArea; - public RawFiducial[] rawFiducials; - - public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, - int tagCount, double tagSpan, double avgTagDist, - double avgTagArea, RawFiducial[] rawFiducials) { - - this.pose = pose; - this.timestampSeconds = timestampSeconds; - this.latency = latency; - this.tagCount = tagCount; - this.tagSpan = tagSpan; - this.avgTagDist = avgTagDist; - this.avgTagArea = avgTagArea; - this.rawFiducials = rawFiducials; - } - } - - private static ObjectMapper mapper; - - /** - * Print JSON Parse time to the console in milliseconds - */ - static boolean profileJSON = false; - - static final String sanitizeName(String name) { - if (name == "" || name == null) { - return "limelight"; - } - return name; - } - - private static Pose3d toPose3D(double[] inData) { - if(inData.length < 6) - { - //System.err.println("Bad LL 3D Pose Data!"); - return new Pose3d(); - } - return new Pose3d( - new Translation3d(inData[0], inData[1], inData[2]), - new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), - Units.degreesToRadians(inData[5]))); - } - - private static Pose2d toPose2D(double[] inData) { - if(inData.length < 6) - { - // System.err.println("Bad LL 2D Pose Data!"); - return new Pose2d(); - } - Translation2d tran2d = new Translation2d(inData[0], inData[1]); - Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); - return new Pose2d(tran2d, r2d); - } - - private static double extractBotPoseEntry(double[] inData, int position) { - if (inData.length < position + 1) { - return 0; - } - return inData[position]; - } - - private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { - var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); - var poseArray = poseEntry.getDoubleArray(new double[0]); - var pose = toPose2D(poseArray); - double latency = extractBotPoseEntry(poseArray, 6); - int tagCount = (int) extractBotPoseEntry(poseArray, 7); - double tagSpan = extractBotPoseEntry(poseArray, 8); - double tagDist = extractBotPoseEntry(poseArray, 9); - double tagArea = extractBotPoseEntry(poseArray, 10); - // getlastchange() in microseconds, ll latency in milliseconds - var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); - - RawFiducial[] rawFiducials = new RawFiducial[tagCount]; - int valsPerFiducial = 7; - int expectedTotalVals = 11 + valsPerFiducial * tagCount; - - if (poseArray.length != expectedTotalVals) { - // Don't populate fiducials - } else { - for (int i = 0; i < tagCount; i++) { - int baseIndex = 11 + (i * valsPerFiducial); - int id = (int) poseArray[baseIndex]; - double txnc = poseArray[baseIndex + 1]; - double tync = poseArray[baseIndex + 2]; - double ta = poseArray[baseIndex + 3]; - double distToCamera = poseArray[baseIndex + 4]; - double distToRobot = poseArray[baseIndex + 5]; - double ambiguity = poseArray[baseIndex + 6]; - rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); - } - } - - return new PoseEstimate(pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); - } - - private static void printPoseEstimate(PoseEstimate pose) { - if (pose == null) { - System.out.println("No PoseEstimate available."); - return; - } - - System.out.printf("Pose Estimate Information:%n"); - System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); - System.out.printf("Latency: %.3f ms%n", pose.latency); - System.out.printf("Tag Count: %d%n", pose.tagCount); - System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); - System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); - System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); - System.out.println(); - - if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { - System.out.println("No RawFiducials data available."); - return; - } - - System.out.println("Raw Fiducials Details:"); - for (int i = 0; i < pose.rawFiducials.length; i++) { - RawFiducial fiducial = pose.rawFiducials[i]; - System.out.printf(" Fiducial #%d:%n", i + 1); - System.out.printf(" ID: %d%n", fiducial.id); - System.out.printf(" TXNC: %.2f%n", fiducial.txnc); - System.out.printf(" TYNC: %.2f%n", fiducial.tync); - System.out.printf(" TA: %.2f%n", fiducial.ta); - System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); - System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); - System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); - System.out.println(); - } - } - - public static NetworkTable getLimelightNTTable(String tableName) { - return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); - } - - public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { - return getLimelightNTTable(tableName).getEntry(entryName); - } - - public static double getLimelightNTDouble(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); - } - - public static void setLimelightNTDouble(String tableName, String entryName, double val) { - getLimelightNTTableEntry(tableName, entryName).setDouble(val); - } - - public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { - getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); - } - - public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); - } - - public static String getLimelightNTString(String tableName, String entryName) { - return getLimelightNTTableEntry(tableName, entryName).getString(""); - } - - public static URL getLimelightURLString(String tableName, String request) { - String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; - URL url; - try { - url = new URL(urlString); - return url; - } catch (MalformedURLException e) { - System.err.println("bad LL URL"); - } - return null; - } - ///// - ///// - - public static double getTX(String limelightName) { - return getLimelightNTDouble(limelightName, "tx"); - } - - public static double getTY(String limelightName) { - return getLimelightNTDouble(limelightName, "ty"); - } - - public static double getTA(String limelightName) { - return getLimelightNTDouble(limelightName, "ta"); - } - - public static double getLatency_Pipeline(String limelightName) { - return getLimelightNTDouble(limelightName, "tl"); - } - - public static double getLatency_Capture(String limelightName) { - return getLimelightNTDouble(limelightName, "cl"); - } - - public static double getCurrentPipelineIndex(String limelightName) { - return getLimelightNTDouble(limelightName, "getpipe"); - } - - public static String getJSONDump(String limelightName) { - return getLimelightNTString(limelightName, "json"); - } - - /** - * Switch to getBotPose - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - /** - * Switch to getBotPose_wpiRed - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - /** - * Switch to getBotPose_wpiBlue - * - * @param limelightName - * @return - */ - @Deprecated - public static double[] getBotpose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose"); - } - - public static double[] getBotPose_wpiRed(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - } - - public static double[] getBotPose_wpiBlue(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - } - - public static double[] getBotPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - } - - public static double[] getCameraPose_TargetSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - } - - public static double[] getTargetPose_CameraSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - } - - public static double[] getTargetPose_RobotSpace(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - } - - public static double[] getTargetColor(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "tc"); - } - - public static double getFiducialID(String limelightName) { - return getLimelightNTDouble(limelightName, "tid"); - } - - public static String getNeuralClassID(String limelightName) { - return getLimelightNTString(limelightName, "tclass"); - } - - ///// - ///// - - public static Pose3d getBotPose3d(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiRed(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_wpiBlue(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); - return toPose3D(poseArray); - } - - public static Pose3d getBotPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); - return toPose3D(poseArray); - } - - public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); - return toPose3D(poseArray); - } - - public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { - double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); - return toPose3D(poseArray); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiBlue(String limelightName) { - - double[] result = getBotPose_wpiBlue(limelightName); - return toPose2D(result); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the BLUE - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpiblue"); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the BLUE - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d_wpiRed(String limelightName) { - - double[] result = getBotPose_wpiRed(limelightName); - return toPose2D(result); - - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the RED - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_wpired"); - } - - /** - * Gets the Pose2d and timestamp for use with WPILib pose estimator - * (addVisionMeasurement) when you are on the RED - * alliance - * - * @param limelightName - * @return - */ - public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { - return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); - } - - /** - * Gets the Pose2d for easy use with Odometry vision pose estimator - * (addVisionMeasurement) - * - * @param limelightName - * @return - */ - public static Pose2d getBotPose2d(String limelightName) { - - double[] result = getBotPose(limelightName); - return toPose2D(result); - - } - - public static boolean getTV(String limelightName) { - return 1.0 == getLimelightNTDouble(limelightName, "tv"); - } - - ///// - ///// - - public static void setPipelineIndex(String limelightName, int pipelineIndex) { - setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); - } - - public static void setPriorityTagID(String limelightName, int ID) { - setLimelightNTDouble(limelightName, "priorityid", ID); - } - - /** - * The LEDs will be controlled by Limelight pipeline settings, and not by robot - * code. - */ - public static void setLEDMode_PipelineControl(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 0); - } - - public static void setLEDMode_ForceOff(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 1); - } - - public static void setLEDMode_ForceBlink(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 2); - } - - public static void setLEDMode_ForceOn(String limelightName) { - setLimelightNTDouble(limelightName, "ledMode", 3); - } - - public static void setStreamMode_Standard(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 0); - } - - public static void setStreamMode_PiPMain(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 1); - } - - public static void setStreamMode_PiPSecondary(String limelightName) { - setLimelightNTDouble(limelightName, "stream", 2); - } - - public static void setCameraMode_Processor(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 0); - } - public static void setCameraMode_Driver(String limelightName) { - setLimelightNTDouble(limelightName, "camMode", 1); - } - - - /** - * Sets the crop window. The crop window in the UI must be completely open for - * dynamic cropping to work. - */ - public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { - double[] entries = new double[4]; - entries[0] = cropXMin; - entries[1] = cropXMax; - entries[2] = cropYMin; - entries[3] = cropYMax; - setLimelightNTDoubleArray(limelightName, "crop", entries); - } - - public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, - double pitch, double pitchRate, - double roll, double rollRate) { - - double[] entries = new double[6]; - entries[0] = yaw; - entries[1] = yawRate; - entries[2] = pitch; - entries[3] = pitchRate; - entries[4] = roll; - entries[5] = rollRate; - setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); - } - - public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { - double[] validIDsDouble = new double[validIDs.length]; - for (int i = 0; i < validIDs.length; i++) { - validIDsDouble[i] = validIDs[i]; - } - setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); - } - - public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { - double[] entries = new double[6]; - entries[0] = forward; - entries[1] = side; - entries[2] = up; - entries[3] = roll; - entries[4] = pitch; - entries[5] = yaw; - setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); - } - - ///// - ///// - - public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { - setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); - } - - public static double[] getPythonScriptData(String limelightName) { - return getLimelightNTDoubleArray(limelightName, "llpython"); - } - - ///// - ///// - - /** - * Asynchronously take snapshot. - */ - public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { - return CompletableFuture.supplyAsync(() -> { - return SYNCH_TAKESNAPSHOT(tableName, snapshotName); - }); - } - - private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { - URL url = getLimelightURLString(tableName, "capturesnapshot"); - try { - HttpURLConnection connection = (HttpURLConnection) url.openConnection(); - connection.setRequestMethod("GET"); - if (snapshotName != null && snapshotName != "") { - connection.setRequestProperty("snapname", snapshotName); - } - - int responseCode = connection.getResponseCode(); - if (responseCode == 200) { - return true; - } else { - System.err.println("Bad LL Request"); - } - } catch (IOException e) { - System.err.println(e.getMessage()); - } - return false; - } - - /** - * Parses Limelight's JSON results dump into a LimelightResults Object - */ - public static LimelightResults getLatestResults(String limelightName) { - - long start = System.nanoTime(); - LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); - if (mapper == null) { - mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); - } - - try { - results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); - } catch (JsonProcessingException e) { - results.error = "lljson error: " + e.getMessage(); - } - - long end = System.nanoTime(); - double millis = (end - start) * .000001; - results.targetingResults.latency_jsonParse = millis; - if (profileJSON) { - System.out.printf("lljson: %.2f\r\n", millis); - } - - return results; - } -} \ No newline at end of file From fa7c6ac9242b286a6cab87df65837f4a81c91f3e Mon Sep 17 00:00:00 2001 From: DriverStationComputer Date: Tue, 4 Feb 2025 17:39:27 -0800 Subject: [PATCH 25/39] Very short commit, just changed helpers --- .../subsystems/Drivetrain.java | 5 + .../subsystems/Limelight.java | 157 ++ .../subsystems/LimelightHelpers.java | 1648 +++++++++++++++++ 3 files changed, 1810 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Limelight.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java new file mode 100644 index 0000000..1cdbfc5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -0,0 +1,5 @@ +package org.carlmontrobotics.subsystems; + +public class Drivetrain { + +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java new file mode 100644 index 0000000..4850044 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -0,0 +1,157 @@ + +package org.carlmontrobotics.subsystems; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; +import static org.carlmontrobotics.Constants.Limelightc.*; +import static org.carlmontrobotics.Constants.Limelightc.Apriltag.*; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Limelight extends SubsystemBase { + // Useless drivetrain created + private final Drivetrain drivetrain; + + private final InterpolatingDoubleTreeMap shooterMap; + + public Limelight(Drivetrain drivetrain) { + this.drivetrain = drivetrain; + + LimelightHelpers.SetFiducialIDFiltersOverride(SHOOTER_LL_NAME, VALID_IDS); + + shooterMap = new InterpolatingDoubleTreeMap(); // add values after testing + // key is distance (meters), value is angle (rads) + shooterMap.put(2.06, .01); + shooterMap.put(3.05, 0.26); + shooterMap.put(2.5, 0.23); + shooterMap.put(1.39, -0.18); + shooterMap.put(3.45, 0.3); + shooterMap.put(3.1, 0.3); + + SmartDashboard.putBoolean("shooter sees note", LimelightHelpers.getTV(SHOOTER_LL_NAME)); + SmartDashboard.putBoolean("intake sees note", LimelightHelpers.getTV(INTAKE_LL_NAME)); + + // ASSUMING SHOOTING AT 4000 RPM + // changing speed multipliers for auto intaking note + // SmartDashboard.putNumber("forward speed multiplier", 1.5); + // SmartDashboard.putNumber("strafe speed multiplier", 1.5); + // SmartDashboard.putNumber("rotational speed multiplier", 2); + + // tuning apriltag alignment pid and tolerances + // SmartDashboard.putNumber("rotation to align", getRotateAngleRadMT2()); + + // SmartDashboard.putNumber("apriltag align kp", thetaPIDController[0]); + // SmartDashboard.putNumber("apriltag align ki", thetaPIDController[1]); + // SmartDashboard.putNumber("apriltag align kd", thetaPIDController[2]); + + // SmartDashboard.putNumber("apriltag align pos tolerance", + // positionTolerance[2]); + // SmartDashboard.putNumber("apriltag align vel tolerance", + // velocityTolerance[2]); + + } + + @Override + public void periodic() { + + SmartDashboard.putBoolean("shooter sees note", LimelightHelpers.getTV(SHOOTER_LL_NAME)); + SmartDashboard.putBoolean("intake sees note", LimelightHelpers.getTV(INTAKE_LL_NAME)); + + // intake limelight testing + // SmartDashboard.putBoolean("see note", + // LimelightHelpers.getTV(INTAKE_LL_NAME)); + // SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); + // SmartDashboard.putNumber("intake tx", + // LimelightHelpers.getTX(INTAKE_LL_NAME)); + + // shooter limelight testing + // SmartDashboard.putNumber("distance to speaker (meters)", + // getDistanceToSpeakerMetersMT2()); + // SmartDashboard.putNumber("optimized arm angle", + // getOptimizedArmAngleRadsMT2()); + } + + public double getTXDeg(String limelightName) { + return (limelightName == INTAKE_LL_NAME) + ? LimelightHelpers.getTX(INTAKE_LL_NAME) + : -LimelightHelpers.getTY(SHOOTER_LL_NAME); + } + + public double getTYDeg(String limelightName) { + return (limelightName == INTAKE_LL_NAME) + ? LimelightHelpers.getTY(INTAKE_LL_NAME) + : LimelightHelpers.getTX(SHOOTER_LL_NAME); + } + + public double getDistanceToSpeakerMeters() { + if (LimelightHelpers + .getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID + || LimelightHelpers + .getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER) + .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); // because limelight is mounted + // horizontally + double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) + / angleToGoal.getTan(); + // SmartDashboard.putNumber("limelight distance", distance); + return distance; + } else { + // SmartDashboard.putNumber("limelight distance", -1); + return -1; + } + } + + public double getDistanceToNoteMeters() { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE) + .plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME))); + if (angleToGoal.getDegrees() <= 0) { + double distance = (HEIGHT_FROM_GROUND_METERS_INTAKE - NOTE_HEIGHT) + / Math.tan(Math.abs(angleToGoal.getRadians())); + // SmartDashboard.putNumber("limelight distance", distance); + return distance; + } else { + // SmartDashboard.putNumber("limelight distance", -1); + return -1; + } + } + + public double getArmAngleToShootSpeakerRad() { + double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS; + double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH; + return END_EFFECTOR_BASE_ANGLE_RADS - Math + .atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters); + } + + public double getRotateAngleRadMT2() { + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target + + double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the + // robot and target + double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset + + double targetOffsetRads = MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI); + + return targetOffsetRads; + } + + public double getDistanceToSpeakerMetersMT2() { + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); + + double x = targetPoseRobotSpace.getX(); + double z = targetPoseRobotSpace.getZ(); + + return Math.hypot(x, z); + } + + public double getOptimizedArmAngleRadsMT2() { + return shooterMap.get(getDistanceToSpeakerMetersMT2()); + } + + public boolean seesTag() { + return LimelightHelpers.getTV(SHOOTER_LL_NAME); + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java new file mode 100644 index 0000000..1b885bc --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -0,0 +1,1648 @@ + +//LimelightHelpers v1.11 (REQUIRES LLOS 2025.0 OR LATER) + +package org.carlmontrobotics.subsystems; + +import edu.wpi.first.networktables.DoubleArrayEntry; +import edu.wpi.first.networktables.NetworkTable; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.TimestampedDoubleArray; +import org.carlmontrobotics.subsystems.LimelightHelpers.LimelightResults; +import org.carlmontrobotics.subsystems.LimelightHelpers.PoseEstimate; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.geometry.Translation2d; + +import java.io.IOException; +import java.net.HttpURLConnection; +import java.net.MalformedURLException; +import java.net.URL; +import java.util.Map; +import java.util.concurrent.CompletableFuture; + +import com.fasterxml.jackson.annotation.JsonFormat; +import com.fasterxml.jackson.annotation.JsonFormat.Shape; +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.DeserializationFeature; +import com.fasterxml.jackson.databind.ObjectMapper; +import java.util.concurrent.ConcurrentHashMap; + +/** + * LimelightHelpers provides static methods and classes for interfacing with Limelight vision cameras in FRC. + * This library supports all Limelight features including AprilTag tracking, Neural Networks, and standard color/retroreflective tracking. + */ +public class LimelightHelpers { + + private static final Map doubleArrayEntries = new ConcurrentHashMap<>(); + + /** + * Represents a Color/Retroreflective Target Result extracted from JSON Output + */ + public static class LimelightTarget_Retro { + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Retro() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + + } + + /** + * Represents an AprilTag/Fiducial Target Result extracted from JSON Output + */ + public static class LimelightTarget_Fiducial { + + @JsonProperty("fID") + public double fiducialID; + + @JsonProperty("fam") + public String fiducialFamily; + + @JsonProperty("t6c_ts") + private double[] cameraPose_TargetSpace; + + @JsonProperty("t6r_fs") + private double[] robotPose_FieldSpace; + + @JsonProperty("t6r_ts") + private double[] robotPose_TargetSpace; + + @JsonProperty("t6t_cs") + private double[] targetPose_CameraSpace; + + @JsonProperty("t6t_rs") + private double[] targetPose_RobotSpace; + + public Pose3d getCameraPose_TargetSpace() + { + return toPose3D(cameraPose_TargetSpace); + } + public Pose3d getRobotPose_FieldSpace() + { + return toPose3D(robotPose_FieldSpace); + } + public Pose3d getRobotPose_TargetSpace() + { + return toPose3D(robotPose_TargetSpace); + } + public Pose3d getTargetPose_CameraSpace() + { + return toPose3D(targetPose_CameraSpace); + } + public Pose3d getTargetPose_RobotSpace() + { + return toPose3D(targetPose_RobotSpace); + } + + public Pose2d getCameraPose_TargetSpace2D() + { + return toPose2D(cameraPose_TargetSpace); + } + public Pose2d getRobotPose_FieldSpace2D() + { + return toPose2D(robotPose_FieldSpace); + } + public Pose2d getRobotPose_TargetSpace2D() + { + return toPose2D(robotPose_TargetSpace); + } + public Pose2d getTargetPose_CameraSpace2D() + { + return toPose2D(targetPose_CameraSpace); + } + public Pose2d getTargetPose_RobotSpace2D() + { + return toPose2D(targetPose_RobotSpace); + } + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ts") + public double ts; + + public LimelightTarget_Fiducial() { + cameraPose_TargetSpace = new double[6]; + robotPose_FieldSpace = new double[6]; + robotPose_TargetSpace = new double[6]; + targetPose_CameraSpace = new double[6]; + targetPose_RobotSpace = new double[6]; + } + } + + /** + * Represents a Barcode Target Result extracted from JSON Output + */ + public static class LimelightTarget_Barcode { + + /** + * Barcode family type (e.g. "QR", "DataMatrix", etc.) + */ + @JsonProperty("fam") + public String family; + + /** + * Gets the decoded data content of the barcode + */ + @JsonProperty("data") + public String data; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("pts") + public double[][] corners; + + public LimelightTarget_Barcode() { + } + + public String getFamily() { + return family; + } + } + + /** + * Represents a Neural Classifier Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Classifier { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("zone") + public double zone; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("typ") + public double ty_pixels; + + public LimelightTarget_Classifier() { + } + } + + /** + * Represents a Neural Detector Pipeline Result extracted from JSON Output + */ + public static class LimelightTarget_Detector { + + @JsonProperty("class") + public String className; + + @JsonProperty("classID") + public double classID; + + @JsonProperty("conf") + public double confidence; + + @JsonProperty("ta") + public double ta; + + @JsonProperty("tx") + public double tx; + + @JsonProperty("ty") + public double ty; + + @JsonProperty("txp") + public double tx_pixels; + + @JsonProperty("typ") + public double ty_pixels; + + @JsonProperty("tx_nocross") + public double tx_nocrosshair; + + @JsonProperty("ty_nocross") + public double ty_nocrosshair; + + public LimelightTarget_Detector() { + } + } + + /** + * Limelight Results object, parsed from a Limelight's JSON results output. + */ + public static class LimelightResults { + + public String error; + + @JsonProperty("pID") + public double pipelineID; + + @JsonProperty("tl") + public double latency_pipeline; + + @JsonProperty("cl") + public double latency_capture; + + public double latency_jsonParse; + + @JsonProperty("ts") + public double timestamp_LIMELIGHT_publish; + + @JsonProperty("ts_rio") + public double timestamp_RIOFPGA_capture; + + @JsonProperty("v") + @JsonFormat(shape = Shape.NUMBER) + public boolean valid; + + @JsonProperty("botpose") + public double[] botpose; + + @JsonProperty("botpose_wpired") + public double[] botpose_wpired; + + @JsonProperty("botpose_wpiblue") + public double[] botpose_wpiblue; + + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + + @JsonProperty("t6c_rs") + public double[] camerapose_robotspace; + + public Pose3d getBotPose3d() { + return toPose3D(botpose); + } + + public Pose3d getBotPose3d_wpiRed() { + return toPose3D(botpose_wpired); + } + + public Pose3d getBotPose3d_wpiBlue() { + return toPose3D(botpose_wpiblue); + } + + public Pose2d getBotPose2d() { + return toPose2D(botpose); + } + + public Pose2d getBotPose2d_wpiRed() { + return toPose2D(botpose_wpired); + } + + public Pose2d getBotPose2d_wpiBlue() { + return toPose2D(botpose_wpiblue); + } + + @JsonProperty("Retro") + public LimelightTarget_Retro[] targets_Retro; + + @JsonProperty("Fiducial") + public LimelightTarget_Fiducial[] targets_Fiducials; + + @JsonProperty("Classifier") + public LimelightTarget_Classifier[] targets_Classifier; + + @JsonProperty("Detector") + public LimelightTarget_Detector[] targets_Detector; + + @JsonProperty("Barcode") + public LimelightTarget_Barcode[] targets_Barcode; + + public LimelightResults() { + botpose = new double[6]; + botpose_wpired = new double[6]; + botpose_wpiblue = new double[6]; + camerapose_robotspace = new double[6]; + targets_Retro = new LimelightTarget_Retro[0]; + targets_Fiducials = new LimelightTarget_Fiducial[0]; + targets_Classifier = new LimelightTarget_Classifier[0]; + targets_Detector = new LimelightTarget_Detector[0]; + targets_Barcode = new LimelightTarget_Barcode[0]; + + } + + + } + + /** + * Represents a Limelight Raw Fiducial result from Limelight's NetworkTables output. + */ + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + + public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + /** + * Represents a Limelight Raw Neural Detector result from Limelight's NetworkTables output. + */ + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + + public RawDetection(int classId, double txnc, double tync, double ta, + double corner0_X, double corner0_Y, + double corner1_X, double corner1_Y, + double corner2_X, double corner2_Y, + double corner3_X, double corner3_Y ) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + /** + * Represents a 3D Pose Estimate. + */ + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + + public RawFiducial[] rawFiducials; + public boolean isMegaTag2; + + /** + * Instantiates a PoseEstimate object with default values + */ + public PoseEstimate() { + this.pose = new Pose2d(); + this.timestampSeconds = 0; + this.latency = 0; + this.tagCount = 0; + this.tagSpan = 0; + this.avgTagDist = 0; + this.avgTagArea = 0; + this.rawFiducials = new RawFiducial[]{}; + this.isMegaTag2 = false; + } + + public PoseEstimate(Pose2d pose, double timestampSeconds, double latency, + int tagCount, double tagSpan, double avgTagDist, + double avgTagArea, RawFiducial[] rawFiducials, boolean isMegaTag2) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; + this.isMegaTag2 = isMegaTag2; + } + + } + + /** + * Encapsulates the state of an internal Limelight IMU. + */ + public static class IMUData { + public double robotYaw = 0.0; + public double Roll = 0.0; + public double Pitch = 0.0; + public double Yaw = 0.0; + public double gyroX = 0.0; + public double gyroY = 0.0; + public double gyroZ = 0.0; + public double accelX = 0.0; + public double accelY = 0.0; + public double accelZ = 0.0; + + public IMUData() {} + + public IMUData(double[] imuData) { + if (imuData != null && imuData.length >= 10) { + this.robotYaw = imuData[0]; + this.Roll = imuData[1]; + this.Pitch = imuData[2]; + this.Yaw = imuData[3]; + this.gyroX = imuData[4]; + this.gyroY = imuData[5]; + this.gyroZ = imuData[6]; + this.accelX = imuData[7]; + this.accelY = imuData[8]; + this.accelZ = imuData[9]; + } + } + } + + + private static ObjectMapper mapper; + + /** + * Print JSON Parse time to the console in milliseconds + */ + static boolean profileJSON = false; + + static final String sanitizeName(String name) { + if (name == "" || name == null) { + return "limelight"; + } + return name; + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose3d object. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose3d object representing the pose, or empty Pose3d if invalid data + */ + public static Pose3d toPose3D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 3D Pose Data!"); + return new Pose3d(); + } + return new Pose3d( + new Translation3d(inData[0], inData[1], inData[2]), + new Rotation3d(Units.degreesToRadians(inData[3]), Units.degreesToRadians(inData[4]), + Units.degreesToRadians(inData[5]))); + } + + /** + * Takes a 6-length array of pose data and converts it to a Pose2d object. + * Uses only x, y, and yaw components, ignoring z, roll, and pitch. + * Array format: [x, y, z, roll, pitch, yaw] where angles are in degrees. + * @param inData Array containing pose data [x, y, z, roll, pitch, yaw] + * @return Pose2d object representing the pose, or empty Pose2d if invalid data + */ + public static Pose2d toPose2D(double[] inData){ + if(inData.length < 6) + { + //System.err.println("Bad LL 2D Pose Data!"); + return new Pose2d(); + } + Translation2d tran2d = new Translation2d(inData[0], inData[1]); + Rotation2d r2d = new Rotation2d(Units.degreesToRadians(inData[5])); + return new Pose2d(tran2d, r2d); + } + + /** + * Converts a Pose3d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * + * @param pose The Pose3d object to convert + * @return A 6-element array containing [x, y, z, roll, pitch, yaw] + */ + public static double[] pose3dToArray(Pose3d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = pose.getTranslation().getZ(); + result[3] = Units.radiansToDegrees(pose.getRotation().getX()); + result[4] = Units.radiansToDegrees(pose.getRotation().getY()); + result[5] = Units.radiansToDegrees(pose.getRotation().getZ()); + return result; + } + + /** + * Converts a Pose2d object to an array of doubles in the format [x, y, z, roll, pitch, yaw]. + * Translation components are in meters, rotation components are in degrees. + * Note: z, roll, and pitch will be 0 since Pose2d only contains x, y, and yaw. + * + * @param pose The Pose2d object to convert + * @return A 6-element array containing [x, y, 0, 0, 0, yaw] + */ + public static double[] pose2dToArray(Pose2d pose) { + double[] result = new double[6]; + result[0] = pose.getTranslation().getX(); + result[1] = pose.getTranslation().getY(); + result[2] = 0; + result[3] = Units.radiansToDegrees(0); + result[4] = Units.radiansToDegrees(0); + result[5] = Units.radiansToDegrees(pose.getRotation().getRadians()); + return result; + } + + private static double extractArrayEntry(double[] inData, int position){ + if(inData.length < position+1) + { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName, boolean isMegaTag2) { + DoubleArrayEntry poseEntry = LimelightHelpers.getLimelightDoubleArrayEntry(limelightName, entryName); + + TimestampedDoubleArray tsValue = poseEntry.getAtomic(); + double[] poseArray = tsValue.value; + long timestamp = tsValue.timestamp; + + if (poseArray.length == 0) { + // Handle the case where no data is available + return null; // or some default PoseEstimate + } + + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int)extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + + // Convert server timestamp from microseconds to seconds and adjust for latency + double adjustedTimestamp = (timestamp / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for(int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int)poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate(pose, adjustedTimestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials, isMegaTag2); + } + + /** + * Gets the latest raw fiducial/AprilTag detection results from NetworkTables. + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawFiducial objects containing detection details + */ + public static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + /** + * Gets the latest raw neural detector results from NetworkTables + * + * @param limelightName Name/identifier of the Limelight + * @return Array of RawDetection objects containing detection details + */ + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 12; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = new RawDetection(classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + /** + * Prints detailed information about a PoseEstimate to standard output. + * Includes timestamp, latency, tag count, tag span, average tag distance, + * average tag area, and detailed information about each detected fiducial. + * + * @param pose The PoseEstimate object to print. If null, prints "No PoseEstimate available." + */ + public static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.printf("Is MegaTag2: %b%n", pose.isMegaTag2); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + + public static Boolean validPoseEstimate(PoseEstimate pose) { + return pose != null && pose.rawFiducials != null && pose.rawFiducials.length != 0; + } + + public static NetworkTable getLimelightNTTable(String tableName) { + return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); + } + + public static void Flush() { + NetworkTableInstance.getDefault().flush(); + } + + public static NetworkTableEntry getLimelightNTTableEntry(String tableName, String entryName) { + return getLimelightNTTable(tableName).getEntry(entryName); + } + + public static DoubleArrayEntry getLimelightDoubleArrayEntry(String tableName, String entryName) { + String key = tableName + "/" + entryName; + return doubleArrayEntries.computeIfAbsent(key, k -> { + NetworkTable table = getLimelightNTTable(tableName); + return table.getDoubleArrayTopic(entryName).getEntry(new double[0]); + }); + } + + public static double getLimelightNTDouble(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDouble(0.0); + } + + public static void setLimelightNTDouble(String tableName, String entryName, double val) { + getLimelightNTTableEntry(tableName, entryName).setDouble(val); + } + + public static void setLimelightNTDoubleArray(String tableName, String entryName, double[] val) { + getLimelightNTTableEntry(tableName, entryName).setDoubleArray(val); + } + + public static double[] getLimelightNTDoubleArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getDoubleArray(new double[0]); + } + + + public static String getLimelightNTString(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getString(""); + } + + public static String[] getLimelightNTStringArray(String tableName, String entryName) { + return getLimelightNTTableEntry(tableName, entryName).getStringArray(new String[0]); + } + + + public static URL getLimelightURLString(String tableName, String request) { + String urlString = "http://" + sanitizeName(tableName) + ".local:5807/" + request; + URL url; + try { + url = new URL(urlString); + return url; + } catch (MalformedURLException e) { + System.err.println("bad LL URL"); + } + return null; + } + ///// + ///// + + /** + * Does the Limelight have a valid target? + * @param limelightName Name of the Limelight camera ("" for default) + * @return True if a valid target is present, false otherwise + */ + public static boolean getTV(String limelightName) { + return 1.0 == getLimelightNTDouble(limelightName, "tv"); + } + + /** + * Gets the horizontal offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTX(String limelightName) { + return getLimelightNTDouble(limelightName, "tx"); + } + + /** + * Gets the vertical offset from the crosshair to the target in degrees. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTY(String limelightName) { + return getLimelightNTDouble(limelightName, "ty"); + } + + /** + * Gets the horizontal offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Horizontal offset angle in degrees + */ + public static double getTXNC(String limelightName) { + return getLimelightNTDouble(limelightName, "txnc"); + } + + /** + * Gets the vertical offset from the principal pixel/point to the target in degrees. This is the most accurate 2d metric if you are using a calibrated camera and you don't need adjustable crosshair functionality. + * @param limelightName Name of the Limelight camera ("" for default) + * @return Vertical offset angle in degrees + */ + public static double getTYNC(String limelightName) { + return getLimelightNTDouble(limelightName, "tync"); + } + + /** + * Gets the target area as a percentage of the image (0-100%). + * @param limelightName Name of the Limelight camera ("" for default) + * @return Target area percentage (0-100) + */ + public static double getTA(String limelightName) { + return getLimelightNTDouble(limelightName, "ta"); + } + + /** + * T2D is an array that contains several targeting metrcis + * @param limelightName Name of the Limelight camera + * @return Array containing [targetValid, targetCount, targetLatency, captureLatency, tx, ty, txnc, tync, ta, tid, targetClassIndexDetector, + * targetClassIndexClassifier, targetLongSidePixels, targetShortSidePixels, targetHorizontalExtentPixels, targetVerticalExtentPixels, targetSkewDegrees] + */ + public static double[] getT2DArray(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "t2d"); + } + + /** + * Gets the number of targets currently detected. + * @param limelightName Name of the Limelight camera + * @return Number of detected targets + */ + public static int getTargetCount(String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[1]; + } + return 0; + } + + /** + * Gets the classifier class index from the currently running neural classifier pipeline + * @param limelightName Name of the Limelight camera + * @return Class index from classifier pipeline + */ + public static int getClassifierClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[10]; + } + return 0; + } + + /** + * Gets the detector class index from the primary result of the currently running neural detector pipeline. + * @param limelightName Name of the Limelight camera + * @return Class index from detector pipeline + */ + public static int getDetectorClassIndex (String limelightName) { + double[] t2d = getT2DArray(limelightName); + if(t2d.length == 17) + { + return (int)t2d[11]; + } + return 0; + } + + /** + * Gets the current neural classifier result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from classifier pipeline + */ + public static String getClassifierClass (String limelightName) { + return getLimelightNTString(limelightName, "tcclass"); + } + + /** + * Gets the primary neural detector result class name. + * @param limelightName Name of the Limelight camera + * @return Class name string from detector pipeline + */ + public static String getDetectorClass (String limelightName) { + return getLimelightNTString(limelightName, "tdclass"); + } + + /** + * Gets the pipeline's processing latency contribution. + * @param limelightName Name of the Limelight camera + * @return Pipeline latency in milliseconds + */ + public static double getLatency_Pipeline(String limelightName) { + return getLimelightNTDouble(limelightName, "tl"); + } + + /** + * Gets the capture latency. + * @param limelightName Name of the Limelight camera + * @return Capture latency in milliseconds + */ + public static double getLatency_Capture(String limelightName) { + return getLimelightNTDouble(limelightName, "cl"); + } + + /** + * Gets the active pipeline index. + * @param limelightName Name of the Limelight camera + * @return Current pipeline index (0-9) + */ + public static double getCurrentPipelineIndex(String limelightName) { + return getLimelightNTDouble(limelightName, "getpipe"); + } + + /** + * Gets the current pipeline type. + * @param limelightName Name of the Limelight camera + * @return Pipeline type string (e.g. "retro", "apriltag", etc) + */ + public static String getCurrentPipelineType(String limelightName) { + return getLimelightNTString(limelightName, "getpipetype"); + } + + /** + * Gets the full JSON results dump. + * @param limelightName Name of the Limelight camera + * @return JSON string containing all current results + */ + public static String getJSONDump(String limelightName) { + return getLimelightNTString(limelightName, "json"); + } + + /** + * Switch to getBotPose + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + /** + * Switch to getBotPose_wpiRed + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + /** + * Switch to getBotPose_wpiBlue + * + * @param limelightName + * @return + */ + @Deprecated + public static double[] getBotpose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose"); + } + + public static double[] getBotPose_wpiRed(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + } + + public static double[] getBotPose_wpiBlue(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + } + + public static double[] getBotPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + } + + public static double[] getCameraPose_TargetSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + } + + public static double[] getTargetPose_CameraSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + } + + public static double[] getTargetPose_RobotSpace(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + } + + public static double[] getTargetColor(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "tc"); + } + + public static double getFiducialID(String limelightName) { + return getLimelightNTDouble(limelightName, "tid"); + } + + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); + } + + public static String[] getRawBarcodeData(String limelightName) { + return getLimelightNTStringArray(limelightName, "rawbarcodes"); + } + + ///// + ///// + + public static Pose3d getBotPose3d(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose"); + return toPose3D(poseArray); + } + + /** + * (Not Recommended) Gets the robot's 3D pose in the WPILib Red Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Red Alliance field space + */ + public static Pose3d getBotPose3d_wpiRed(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpired"); + return toPose3D(poseArray); + } + + /** + * (Recommended) Gets the robot's 3D pose in the WPILib Blue Alliance Coordinate System. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation in Blue Alliance field space + */ + public static Pose3d getBotPose3d_wpiBlue(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_wpiblue"); + return toPose3D(poseArray); + } + + /** + * Gets the robot's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the robot's position and orientation relative to the target + */ + public static Pose3d getBotPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "botpose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the currently tracked target's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the target + */ + public static Pose3d getCameraPose3d_TargetSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_targetspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the camera's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the camera + */ + public static Pose3d getTargetPose3d_CameraSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_cameraspace"); + return toPose3D(poseArray); + } + + /** + * Gets the target's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the target's position and orientation relative to the robot + */ + public static Pose3d getTargetPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "targetpose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the camera's 3D pose with respect to the robot's coordinate system. + * @param limelightName Name/identifier of the Limelight + * @return Pose3d object representing the camera's position and orientation relative to the robot + */ + public static Pose3d getCameraPose3d_RobotSpace(String limelightName) { + double[] poseArray = getLimelightNTDoubleArray(limelightName, "camerapose_robotspace"); + return toPose3D(poseArray); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiBlue(String limelightName) { + + double[] result = getBotPose_wpiBlue(limelightName); + return toPose2D(result); + } + + /** + * Gets the MegaTag1 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue", false); + } + + /** + * Gets the MegaTag2 Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) in the WPILib Blue alliance coordinate system. + * Make sure you are calling setRobotOrientation() before calling this method. + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d_wpiRed(String limelightName) { + + double[] result = getBotPose_wpiRed(limelightName); + return toPose2D(result); + + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired", false); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED + * alliance + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired", true); + } + + /** + * Gets the Pose2d for easy use with Odometry vision pose estimator + * (addVisionMeasurement) + * + * @param limelightName + * @return + */ + public static Pose2d getBotPose2d(String limelightName) { + + double[] result = getBotPose(limelightName); + return toPose2D(result); + + } + + /** + * Gets the current IMU data from NetworkTables. + * IMU data is formatted as [robotYaw, Roll, Pitch, Yaw, gyroX, gyroY, gyroZ, accelX, accelY, accelZ]. + * Returns all zeros if data is invalid or unavailable. + * + * @param limelightName Name/identifier of the Limelight + * @return IMUData object containing all current IMU data + */ + public static IMUData getIMUData(String limelightName) { + double[] imuData = getLimelightNTDoubleArray(limelightName, "imu"); + if (imuData == null || imuData.length < 10) { + return new IMUData(); // Returns object with all zeros + } + return new IMUData(imuData); + } + + ///// + ///// + + public static void setPipelineIndex(String limelightName, int pipelineIndex) { + setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); + } + + + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + + /** + * Sets LED mode to be controlled by the current pipeline. + * @param limelightName Name of the Limelight camera + */ + public static void setLEDMode_PipelineControl(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 0); + } + + public static void setLEDMode_ForceOff(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 1); + } + + public static void setLEDMode_ForceBlink(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 2); + } + + public static void setLEDMode_ForceOn(String limelightName) { + setLimelightNTDouble(limelightName, "ledMode", 3); + } + + /** + * Enables standard side-by-side stream mode. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_Standard(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 0); + } + + /** + * Enables Picture-in-Picture mode with secondary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPMain(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 1); + } + + /** + * Enables Picture-in-Picture mode with primary stream in the corner. + * @param limelightName Name of the Limelight camera + */ + public static void setStreamMode_PiPSecondary(String limelightName) { + setLimelightNTDouble(limelightName, "stream", 2); + } + + + /** + * Sets the crop window for the camera. The crop window in the UI must be completely open. + * @param limelightName Name of the Limelight camera + * @param cropXMin Minimum X value (-1 to 1) + * @param cropXMax Maximum X value (-1 to 1) + * @param cropYMin Minimum Y value (-1 to 1) + * @param cropYMax Maximum Y value (-1 to 1) + */ + public static void setCropWindow(String limelightName, double cropXMin, double cropXMax, double cropYMin, double cropYMax) { + double[] entries = new double[4]; + entries[0] = cropXMin; + entries[1] = cropXMax; + entries[2] = cropYMin; + entries[3] = cropYMax; + setLimelightNTDoubleArray(limelightName, "crop", entries); + } + + /** + * Sets 3D offset point for easy 3D targeting. + */ + public static void setFiducial3DOffset(String limelightName, double offsetX, double offsetY, double offsetZ) { + double[] entries = new double[3]; + entries[0] = offsetX; + entries[1] = offsetY; + entries[2] = offsetZ; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Sets robot orientation values used by MegaTag2 localization algorithm. + * + * @param limelightName Name/identifier of the Limelight + * @param yaw Robot yaw in degrees. 0 = robot facing red alliance wall in FRC + * @param yawRate (Unnecessary) Angular velocity of robot yaw in degrees per second + * @param pitch (Unnecessary) Robot pitch in degrees + * @param pitchRate (Unnecessary) Angular velocity of robot pitch in degrees per second + * @param roll (Unnecessary) Robot roll in degrees + * @param rollRate (Unnecessary) Angular velocity of robot roll in degrees per second + */ + public static void SetRobotOrientation(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, true); + } + + public static void SetRobotOrientation_NoFlush(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate) { + SetRobotOrientation_INTERNAL(limelightName, yaw, yawRate, pitch, pitchRate, roll, rollRate, false); + } + + private static void SetRobotOrientation_INTERNAL(String limelightName, double yaw, double yawRate, + double pitch, double pitchRate, + double roll, double rollRate, boolean flush) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + if(flush) + { + Flush(); + } + } + + /** + * Configures the IMU mode for MegaTag2 Localization + * + * @param limelightName Name/identifier of the Limelight + * @param mode IMU mode. + */ + public static void SetIMUMode(String limelightName, int mode) { + setLimelightNTDouble(limelightName, "imumode_set", mode); + } + + /** + * Sets the 3D point-of-interest offset for the current fiducial pipeline. + * https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-3d#point-of-interest-tracking + * + * @param limelightName Name/identifier of the Limelight + * @param x X offset in meters + * @param y Y offset in meters + * @param z Z offset in meters + */ + public static void SetFidcuial3DOffset(String limelightName, double x, double y, + double z) { + + double[] entries = new double[3]; + entries[0] = x; + entries[1] = y; + entries[2] = z; + setLimelightNTDoubleArray(limelightName, "fiducial_offset_set", entries); + } + + /** + * Overrides the valid AprilTag IDs that will be used for localization. + * Tags not in this list will be ignored for robot pose estimation. + * + * @param limelightName Name/identifier of the Limelight + * @param validIDs Array of valid AprilTag IDs to track + */ + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + /** + * Sets the downscaling factor for AprilTag detection. + * Increasing downscale can improve performance at the cost of potentially reduced detection range. + * + * @param limelightName Name/identifier of the Limelight + * @param downscale Downscale factor. Valid values: 1.0 (no downscale), 1.5, 2.0, 3.0, 4.0. Set to 0 for pipeline control. + */ + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) + { + int d = 0; // pipeline + if (downscale == 1.0) + { + d = 1; + } + if (downscale == 1.5) + { + d = 2; + } + if (downscale == 2) + { + d = 3; + } + if (downscale == 3) + { + d = 4; + } + if (downscale == 4) + { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + + /** + * Sets the camera pose relative to the robot. + * @param limelightName Name of the Limelight camera + * @param forward Forward offset in meters + * @param side Side offset in meters + * @param up Up offset in meters + * @param roll Roll angle in degrees + * @param pitch Pitch angle in degrees + * @param yaw Yaw angle in degrees + */ + public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) { + double[] entries = new double[6]; + entries[0] = forward; + entries[1] = side; + entries[2] = up; + entries[3] = roll; + entries[4] = pitch; + entries[5] = yaw; + setLimelightNTDoubleArray(limelightName, "camerapose_robotspace_set", entries); + } + + ///// + ///// + + public static void setPythonScriptData(String limelightName, double[] outgoingPythonData) { + setLimelightNTDoubleArray(limelightName, "llrobot", outgoingPythonData); + } + + public static double[] getPythonScriptData(String limelightName) { + return getLimelightNTDoubleArray(limelightName, "llpython"); + } + + ///// + ///// + + /** + * Asynchronously take snapshot. + */ + public static CompletableFuture takeSnapshot(String tableName, String snapshotName) { + return CompletableFuture.supplyAsync(() -> { + return SYNCH_TAKESNAPSHOT(tableName, snapshotName); + }); + } + + private static boolean SYNCH_TAKESNAPSHOT(String tableName, String snapshotName) { + URL url = getLimelightURLString(tableName, "capturesnapshot"); + try { + HttpURLConnection connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + if (snapshotName != null && snapshotName != "") { + connection.setRequestProperty("snapname", snapshotName); + } + + int responseCode = connection.getResponseCode(); + if (responseCode == 200) { + return true; + } else { + System.err.println("Bad LL Request"); + } + } catch (IOException e) { + System.err.println(e.getMessage()); + } + return false; + } + + /** + * Gets the latest JSON results output and returns a LimelightResults object. + * @param limelightName Name of the Limelight camera + * @return LimelightResults object containing all current target data + */ + public static LimelightResults getLatestResults(String limelightName) { + + long start = System.nanoTime(); + LimelightHelpers.LimelightResults results = new LimelightHelpers.LimelightResults(); + if (mapper == null) { + mapper = new ObjectMapper().configure(DeserializationFeature.FAIL_ON_UNKNOWN_PROPERTIES, false); + } + + try { + results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); + } catch (JsonProcessingException e) { + results.error = "lljson error: " + e.getMessage(); + } + + long end = System.nanoTime(); + double millis = (end - start) * .000001; + results.latency_jsonParse = millis; + if (profileJSON) { + System.out.printf("lljson: %.2f\r\n", millis); + } + + return results; + } +} From a68d9486790feb800ff72ab5e7425a16b9b61bee Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 4 Feb 2025 18:38:14 -0800 Subject: [PATCH 26/39] Changed pid c so it matches turn motor configuration --- .../carlmontrobotics/subsystems/Drivetrain.java | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 8fe50dc..404bcf4 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -126,8 +126,7 @@ public class Drivetrain extends SubsystemBase { private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; - - // gyro + private final SparkClosedLoopController pidController; public final float initPitch; public final float initRoll; @@ -138,7 +137,6 @@ public class Drivetrain extends SubsystemBase { private SwerveModule moduleBR; private final Field2d field = new Field2d(); - private final SparkClosedLoopController pidController = turnMotors[0].getClosedLoopController(); private SwerveModuleSim[] moduleSims; private SimDouble gyroYawSim; private Timer simTimer = new Timer(); @@ -148,10 +146,8 @@ public class Drivetrain extends SubsystemBase { double kI = 0; double kD = 0; public Drivetrain() { - SparkMaxConfig c = new SparkMaxConfig(); - c.closedLoop.pid(kP, kI, kP).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - turnMotors[0].configure(c, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - // SmartDashboard.putNumber("Pose Estimator set x (m)", lastSetX); + + // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", // lastSetTheta); @@ -226,7 +222,7 @@ public Drivetrain() { turnMotors[3] = new SparkMax(7, MotorType.kBrushless), turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; - + pidController = turnMotors[0].getClosedLoopController(); if (RobotBase.isSimulation()) { moduleSims = new SwerveModuleSim[] { moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() @@ -243,7 +239,7 @@ public Drivetrain() { driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); for (SparkMax driveMotor : driveMotors) { - driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } SparkMaxConfig turnConfig = new SparkMaxConfig(); turnConfig.encoder.positionConversionFactor(360/turnGearing); @@ -261,7 +257,7 @@ public Drivetrain() { coder.getVelocity().setUpdateFrequency(500); } - + turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); SmartDashboard.putData("Field", field); // for(SparkMax driveMotor : driveMotors) From 0a564b38eba55e242526cd0636f5ccf3a2e16cc8 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 7 Feb 2025 16:39:24 -0800 Subject: [PATCH 27/39] added 4 pid controllers for turn in an array named turnPidControllers --- .../java/org/carlmontrobotics/subsystems/Drivetrain.java | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 404bcf4..9d5d354 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -126,7 +126,7 @@ public class Drivetrain extends SubsystemBase { private SparkMax[] driveMotors = new SparkMax[] { null, null, null, null }; private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; - private final SparkClosedLoopController pidController; + private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; public final float initPitch; public final float initRoll; @@ -222,7 +222,10 @@ public Drivetrain() { turnMotors[3] = new SparkMax(7, MotorType.kBrushless), turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; - pidController = turnMotors[0].getClosedLoopController(); + turnPidControllers[0] = turnMotors[0].getClosedLoopController(); + turnPidControllers[1] = turnMotors[1].getClosedLoopController(); + turnPidControllers[2] = turnMotors[2].getClosedLoopController(); + turnPidControllers[3] = turnMotors[3].getClosedLoopController(); if (RobotBase.isSimulation()) { moduleSims = new SwerveModuleSim[] { moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() @@ -337,7 +340,7 @@ public void periodic() { kP = SmartDashboard.getNumber("kP", 0); kI = SmartDashboard.getNumber("kI", 0); kD = SmartDashboard.getNumber("kD", 0); - pidController.setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + turnPidControllers[0].setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0); // for (CANcoder coder : turnEncoders) { // SignalLogger.writeDouble("Regular position " + coder.toString(), From a5ba3a7fc46c451c17750ead75520c0b06659755 Mon Sep 17 00:00:00 2001 From: AdhitDasDeepBlue <823573@seq.org> Date: Tue, 18 Feb 2025 15:45:29 -0800 Subject: [PATCH 28/39] Mostly ported last year's limelight code into code for just one Limelight this year. Also changed some constants, which need to be recalibrated/tested. --- .wpilib/wpilib_preferences.json | 6 + .../java/org/carlmontrobotics/Constants.java | 17 ++ .../subsystems/Drivetrain.java | 5 - .../subsystems/Limelight.java | 153 +++++------------- 4 files changed, 59 insertions(+), 122 deletions(-) create mode 100644 .wpilib/wpilib_preferences.json delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..f0ee648 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "none", + "teamNumber": 199 +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..56c9ac9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -4,12 +4,29 @@ public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; // } + public static final class Limelightc { + public static final String ROBOT_LL_NAME = 'limelight'; + + public static final int[] VALID_IDS = {3, 16}; // Processors + public static final int RED_PROCESSOR_ID = 3; + public static final int BLUE_PROCESSOR_ID = 16; + public static final double MOUNT_ANGLE_DEG_LL = 45; // Please change + public static final double PROCESSOR_CENTER_HEIGHT_METERS = 0.4318; // 17 inches. 17 wasted seconds of my time. + public static final double HEIGHT_FROM_GROUND_METERS_ROBOT = 0.206502 // Seems to be OK + public static final double ALGAE_HEIGHT = 0.4191; + + public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high + } + public static final class OI { public static final class Driver { public static final int port = 0; } public static final class Manipulator { public static final int port = 1; + } } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java deleted file mode 100644 index 1cdbfc5..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ /dev/null @@ -1,5 +0,0 @@ -package org.carlmontrobotics.subsystems; - -public class Drivetrain { - -} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 4850044..8ae1bbc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -13,17 +13,17 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Limelight extends SubsystemBase { - // Useless drivetrain created private final Drivetrain drivetrain; private final InterpolatingDoubleTreeMap shooterMap; + // NEEDS TO SEE: Barge, Reef, Processor, Coral Dropoff public Limelight(Drivetrain drivetrain) { this.drivetrain = drivetrain; - LimelightHelpers.SetFiducialIDFiltersOverride(SHOOTER_LL_NAME, VALID_IDS); + LimelightHelpers.SetFiducialIDFiltersOverride(ROBOT_LL_NAME, VALID_IDS); - shooterMap = new InterpolatingDoubleTreeMap(); // add values after testing + shooterMap = new InterpolatingDoubleTreeMap(); // ADD VALUES AFTER TESTING!!! // key is distance (meters), value is angle (rads) shooterMap.put(2.06, .01); shooterMap.put(3.05, 0.26); @@ -32,126 +32,45 @@ public Limelight(Drivetrain drivetrain) { shooterMap.put(3.45, 0.3); shooterMap.put(3.1, 0.3); - SmartDashboard.putBoolean("shooter sees note", LimelightHelpers.getTV(SHOOTER_LL_NAME)); - SmartDashboard.putBoolean("intake sees note", LimelightHelpers.getTV(INTAKE_LL_NAME)); + SmartDashboard.putBoolean("Robot sees Processor", , LimelightHelpers.getTV(ROBOT_LL_NAME)); - // ASSUMING SHOOTING AT 4000 RPM - // changing speed multipliers for auto intaking note - // SmartDashboard.putNumber("forward speed multiplier", 1.5); - // SmartDashboard.putNumber("strafe speed multiplier", 1.5); - // SmartDashboard.putNumber("rotational speed multiplier", 2); + public void periodic() { - // tuning apriltag alignment pid and tolerances - // SmartDashboard.putNumber("rotation to align", getRotateAngleRadMT2()); - - // SmartDashboard.putNumber("apriltag align kp", thetaPIDController[0]); - // SmartDashboard.putNumber("apriltag align ki", thetaPIDController[1]); - // SmartDashboard.putNumber("apriltag align kd", thetaPIDController[2]); - - // SmartDashboard.putNumber("apriltag align pos tolerance", - // positionTolerance[2]); - // SmartDashboard.putNumber("apriltag align vel tolerance", - // velocityTolerance[2]); - - } - - @Override - public void periodic() { - - SmartDashboard.putBoolean("shooter sees note", LimelightHelpers.getTV(SHOOTER_LL_NAME)); - SmartDashboard.putBoolean("intake sees note", LimelightHelpers.getTV(INTAKE_LL_NAME)); - - // intake limelight testing - // SmartDashboard.putBoolean("see note", - // LimelightHelpers.getTV(INTAKE_LL_NAME)); - // SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters()); - // SmartDashboard.putNumber("intake tx", - // LimelightHelpers.getTX(INTAKE_LL_NAME)); - - // shooter limelight testing - // SmartDashboard.putNumber("distance to speaker (meters)", - // getDistanceToSpeakerMetersMT2()); - // SmartDashboard.putNumber("optimized arm angle", - // getOptimizedArmAngleRadsMT2()); - } - - public double getTXDeg(String limelightName) { - return (limelightName == INTAKE_LL_NAME) - ? LimelightHelpers.getTX(INTAKE_LL_NAME) - : -LimelightHelpers.getTY(SHOOTER_LL_NAME); - } - - public double getTYDeg(String limelightName) { - return (limelightName == INTAKE_LL_NAME) - ? LimelightHelpers.getTY(INTAKE_LL_NAME) - : LimelightHelpers.getTX(SHOOTER_LL_NAME); - } - - public double getDistanceToSpeakerMeters() { - if (LimelightHelpers - .getFiducialID(SHOOTER_LL_NAME) == RED_SPEAKER_CENTER_TAG_ID - || LimelightHelpers - .getFiducialID(SHOOTER_LL_NAME) == BLUE_SPEAKER_CENTER_TAG_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_SHOOTER) - .plus(Rotation2d.fromDegrees(getTYDeg(SHOOTER_LL_NAME))); // because limelight is mounted - // horizontally - double distance = (SPEAKER_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_SHOOTER) - / angleToGoal.getTan(); - // SmartDashboard.putNumber("limelight distance", distance); - return distance; - } else { - // SmartDashboard.putNumber("limelight distance", -1); - return -1; + SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); } - } - - public double getDistanceToNoteMeters() { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_INTAKE) - .plus(Rotation2d.fromDegrees(getTYDeg(INTAKE_LL_NAME))); - if (angleToGoal.getDegrees() <= 0) { - double distance = (HEIGHT_FROM_GROUND_METERS_INTAKE - NOTE_HEIGHT) - / Math.tan(Math.abs(angleToGoal.getRadians())); - // SmartDashboard.putNumber("limelight distance", distance); - return distance; - } else { - // SmartDashboard.putNumber("limelight distance", -1); - return -1; + // Very helpful accessors + public double getTXDeg() { + return LimelightHelpers.getTX(ROBOT_LL_NAME); } - } - public double getArmAngleToShootSpeakerRad() { - double armRestingHeightToSubwooferMeters = HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS; - double horizontalDistanceMeters = getDistanceToSpeakerMeters() + SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH; - return END_EFFECTOR_BASE_ANGLE_RADS - Math - .atan(armRestingHeightToSubwooferMeters / horizontalDistanceMeters); - } - - public double getRotateAngleRadMT2() { - Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target - - double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the - // robot and target - double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset - - double targetOffsetRads = MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI); - - return targetOffsetRads; - } - - public double getDistanceToSpeakerMetersMT2() { - Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); - - double x = targetPoseRobotSpace.getX(); - double z = targetPoseRobotSpace.getZ(); + public double getTYDeg() { + return LimtelightHelpers.getTY(ROBOT_LL_NAME); + } - return Math.hypot(x, z); - } + // Distance accessors for other subsystems + public double getDistanceToProcessorMeters() { + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_PROCESSOR_ID + || LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_PROCESSOR_ID) { + Rotation2d.angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg(ROBOT_LL_NAME))); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + return distance; + } + else { + return -1; + } + } - public double getOptimizedArmAngleRadsMT2() { - return shooterMap.get(getDistanceToSpeakerMetersMT2()); - } + public double getDistanceToAlgaeMeters() { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg(ROBOT_LL_NAME))); + if (angleToGoal.getDegrees >= 0) { + double distance = (HEIGHT_FROM_GROUND_METERS_ROBOT - ALGAE_HEIGHT)/Math.tan(Math.abs(angleToGoal.getRadians())); + } + else { + return -1; + } + } - public boolean seesTag() { - return LimelightHelpers.getTV(SHOOTER_LL_NAME); - } + public boolean seesTag() { + return LimelightHelpers.getTV(ROBOT_LL_NAME); + } } From 2e5bb15dc6aaf5fd0a9065000bc7601db9221b1c Mon Sep 17 00:00:00 2001 From: AdhitDasDeepBlue <823573@seq.org> Date: Tue, 18 Feb 2025 16:09:09 -0800 Subject: [PATCH 29/39] There were a bunch of syntax/spelling issues in the code, as I was not able to view errors. Many are resolved, only 7 left. --- .../java/org/carlmontrobotics/Constants.java | 21 +++++++++++++++---- .../subsystems/Drivetrain.java | 5 +++++ .../subsystems/Limelight.java | 7 ++++--- 3 files changed, 26 insertions(+), 7 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 56c9ac9..2b2ce2a 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -5,19 +5,32 @@ public final class Constants { // public static final double MAX_SPEED_MPS = 2; // } public static final class Limelightc { - public static final String ROBOT_LL_NAME = 'limelight'; + public static final String ROBOT_LL_NAME = 'limelight'; // Please change the constant public static final int[] VALID_IDS = {3, 16}; // Processors - public static final int RED_PROCESSOR_ID = 3; - public static final int BLUE_PROCESSOR_ID = 16; public static final double MOUNT_ANGLE_DEG_LL = 45; // Please change - public static final double PROCESSOR_CENTER_HEIGHT_METERS = 0.4318; // 17 inches. 17 wasted seconds of my time. public static final double HEIGHT_FROM_GROUND_METERS_ROBOT = 0.206502 // Seems to be OK public static final double ALGAE_HEIGHT = 0.4191; public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high + + public static final class Apriltag { + /*public static final int RED_SPEAKER_CENTER_TAG_ID = 4; + public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; + public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); + public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); + public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875);*/ + + public static final int RED_PROCESSOR_ID = 3; + public static final int BLUE_PROCESSOR_ID = 16; + public static final double PROCESSOR_CENTER_HEIGHT_METERS = 0.4318; // 17 inches. 17 wasted seconds of my time. + } + } + + public static final class Drivetrainc { + // Drivetrain BS that can be added later } public static final class OI { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java new file mode 100644 index 0000000..0ce57b0 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -0,0 +1,5 @@ +package org.carlmontrobotics.subsystems; + +public class Drivetrain { + // Drivetrain BS! +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 8ae1bbc..70b8fc8 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -32,7 +32,7 @@ public Limelight(Drivetrain drivetrain) { shooterMap.put(3.45, 0.3); shooterMap.put(3.1, 0.3); - SmartDashboard.putBoolean("Robot sees Processor", , LimelightHelpers.getTV(ROBOT_LL_NAME)); + SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); public void periodic() { @@ -44,7 +44,7 @@ public double getTXDeg() { } public double getTYDeg() { - return LimtelightHelpers.getTY(ROBOT_LL_NAME); + return LimelightHelpers.getTY(ROBOT_LL_NAME); } // Distance accessors for other subsystems @@ -73,4 +73,5 @@ public double getDistanceToAlgaeMeters() { public boolean seesTag() { return LimelightHelpers.getTV(ROBOT_LL_NAME); } -} + } +} \ No newline at end of file From 5bf589636704b8ac5ee98c857a589f2e7bc586de Mon Sep 17 00:00:00 2001 From: AdhitDasDeepBlue <823573@seq.org> Date: Tue, 18 Feb 2025 16:48:27 -0800 Subject: [PATCH 30/39] Fixed all the errors in Limelight subsystem! There are still issues in build.gradle :( --- .../java/org/carlmontrobotics/Constants.java | 4 +- .../subsystems/Limelight.java | 64 +++++++++---------- 2 files changed, 34 insertions(+), 34 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 2b2ce2a..4d15782 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -5,11 +5,11 @@ public final class Constants { // public static final double MAX_SPEED_MPS = 2; // } public static final class Limelightc { - public static final String ROBOT_LL_NAME = 'limelight'; // Please change the constant + public static final String ROBOT_LL_NAME = "limelight-shooter"; // Please change the constant public static final int[] VALID_IDS = {3, 16}; // Processors public static final double MOUNT_ANGLE_DEG_LL = 45; // Please change - public static final double HEIGHT_FROM_GROUND_METERS_ROBOT = 0.206502 // Seems to be OK + public static final double HEIGHT_FROM_GROUND_METERS_ROBOT = 0.206502; // Seems to be OK public static final double ALGAE_HEIGHT = 0.4191; public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 70b8fc8..5ab064c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -33,45 +33,45 @@ public Limelight(Drivetrain drivetrain) { shooterMap.put(3.1, 0.3); SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); + } + @Override + public void periodic() { + SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); + } + // Very helpful accessors + public double getTXDeg() { + return LimelightHelpers.getTX(ROBOT_LL_NAME); + } - public void periodic() { + public double getTYDeg() { + return LimelightHelpers.getTY(ROBOT_LL_NAME); + } - SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); + // Distance accessors for other subsystems + public double getDistanceToProcessorMeters() { + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_PROCESSOR_ID + || LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_PROCESSOR_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + return distance; } - // Very helpful accessors - public double getTXDeg() { - return LimelightHelpers.getTX(ROBOT_LL_NAME); - } - - public double getTYDeg() { - return LimelightHelpers.getTY(ROBOT_LL_NAME); + else { + return -1; } + } - // Distance accessors for other subsystems - public double getDistanceToProcessorMeters() { - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_PROCESSOR_ID - || LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_PROCESSOR_ID) { - Rotation2d.angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg(ROBOT_LL_NAME))); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - return distance; - } - else { - return -1; - } + public double getDistanceToAlgaeMeters() { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + if (angleToGoal.getDegrees() >= 0) { + double distance = (HEIGHT_FROM_GROUND_METERS_ROBOT - ALGAE_HEIGHT)/Math.tan(Math.abs(angleToGoal.getRadians())); + return distance; } - - public double getDistanceToAlgaeMeters() { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg(ROBOT_LL_NAME))); - if (angleToGoal.getDegrees >= 0) { - double distance = (HEIGHT_FROM_GROUND_METERS_ROBOT - ALGAE_HEIGHT)/Math.tan(Math.abs(angleToGoal.getRadians())); - } - else { - return -1; - } + else { + return -1; } + } - public boolean seesTag() { - return LimelightHelpers.getTV(ROBOT_LL_NAME); - } + public boolean seesTag() { + return LimelightHelpers.getTV(ROBOT_LL_NAME); } } \ No newline at end of file From 8babae08e6795ebeebcd13e305291fc86bcc7205 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 19 Feb 2025 08:24:38 -0800 Subject: [PATCH 31/39] consistent metrics --- src/main/java/org/carlmontrobotics/Constants.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4d15782..75fa074 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,7 @@ package org.carlmontrobotics; +import edu.wpi.first.math.util.Units; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -25,7 +27,7 @@ public static final class Apriltag { public static final int RED_PROCESSOR_ID = 3; public static final int BLUE_PROCESSOR_ID = 16; - public static final double PROCESSOR_CENTER_HEIGHT_METERS = 0.4318; // 17 inches. 17 wasted seconds of my time. + public static final double PROCESSOR_CENTER_HEIGHT_METERS = Units.inchesToMeters(17); //0.4318; 17 inches. 17 wasted seconds of my time. } } From 458cc509b12162b81019f1a74b13f3f7bc104d3b Mon Sep 17 00:00:00 2001 From: AdhitDasDeepBlue <823573@seq.org> Date: Sat, 22 Feb 2025 17:39:57 -0800 Subject: [PATCH 32/39] I made a distance to reef method. I should really improve it, it's messed up (I should use a for loop). --- .../java/org/carlmontrobotics/Constants.java | 31 +++- .../subsystems/Limelight.java | 132 +++++++++++++++++- 2 files changed, 156 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 4d15782..da09bbf 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,7 @@ package org.carlmontrobotics; +import edu.wpi.first.math.util.Units; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -23,10 +25,33 @@ public static final class Apriltag { public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875);*/ + // Red Processor ids public static final int RED_PROCESSOR_ID = 3; + + // Blue Processor ids public static final int BLUE_PROCESSOR_ID = 16; - public static final double PROCESSOR_CENTER_HEIGHT_METERS = 0.4318; // 17 inches. 17 wasted seconds of my time. - } + + // Red Reef ids + public static final int RED_REEF_FAR_RIGHT_ID = 6; + public static final int RED_REEF_FAR_ID = 7; + public static final int RED_REEF_FAR_LEFT_ID = 8; + public static final int RED_REEF_CLOSE_LEFT_ID = 9; + public static final int RED_REEF_CLOSE_ID = 10; + public static final int RED_REEF_CLOSE_RIGHT_ID = 11; + + // Blue Reef ids + public static final int BLUE_REEF_FAR_LEFT_ID = 17; + public static final int BLUE_REEF_FAR_ID = 18; + public static final int BLUE_REEF_FAR_RIGHT_ID = 19; + public static final int BLUE_REEF_CLOSE_RIGHT_ID = 20; + public static final int BLUE_REEF_CLOSE_ID = 21; + public static final int BLUE_REEF_CLOSE_LEFT_ID = 22; + + + // Heights + public static final double PROCESSOR_CENTER_HEIGHT_METERS = Units.inchesToMeters(47.88); // Going to re-assume that this is for Apriltag center. + public static final double REEF_CENTER_HEIGHT_METERS = Units.inchesToMeters(8.75); // Also center of Reef + } } public static final class Drivetrainc { @@ -42,4 +67,4 @@ public static final class Manipulator { } } -} +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 5ab064c..a8f01f8 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -17,10 +17,11 @@ public class Limelight extends SubsystemBase { private final InterpolatingDoubleTreeMap shooterMap; + private final double[] distances = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; + // NEEDS TO SEE: Barge, Reef, Processor, Coral Dropoff public Limelight(Drivetrain drivetrain) { this.drivetrain = drivetrain; - LimelightHelpers.SetFiducialIDFiltersOverride(ROBOT_LL_NAME, VALID_IDS); shooterMap = new InterpolatingDoubleTreeMap(); // ADD VALUES AFTER TESTING!!! @@ -32,11 +33,13 @@ public Limelight(Drivetrain drivetrain) { shooterMap.put(3.45, 0.3); shooterMap.put(3.1, 0.3); - SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); + SmartDashboard.putBoolean("Robot sees a Tag", seesTag()); // I need to improve this... + SmartDashboard.putNumber("Distance to Processor", getDistanceToProcessorMeters()); } @Override public void periodic() { - SmartDashboard.putBoolean("Robot sees Processor", LimelightHelpers.getTV(ROBOT_LL_NAME)); + SmartDashboard.putBoolean("Robot sees a Tag", seesTag()); + SmartDashboard.putNumber("Distance to Processor", getDistanceToProcessorMeters()); } // Very helpful accessors public double getTXDeg() { @@ -47,7 +50,7 @@ public double getTYDeg() { return LimelightHelpers.getTY(ROBOT_LL_NAME); } - // Distance accessors for other subsystems + // Distance accessors for field areas public double getDistanceToProcessorMeters() { if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_PROCESSOR_ID || LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_PROCESSOR_ID) { @@ -60,6 +63,127 @@ public double getDistanceToProcessorMeters() { } } + public double getDistanceToReefMeters() { + // I should really iterate this through a for loop... + // Red Reef Far Right (id 6) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_FAR_RIGHT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[0] = distance; + } + else { + distances[0] = -1; + } + //Red Reef Far (id 7) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_FAR_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[1] = distance; + } + else { + distances[1] = -1; + } + // Red Reef Far Left (id 8) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_FAR_LEFT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[2] = distance; + } + else { + distances[2] = -1; + } + // Red Reef Close Left (id 9) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_CLOSE_LEFT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[3] = distance; + } + else { + distances[3] = -1; + } + // Red Reef Close (id 10) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_CLOSE_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[4] = distance; + } + else { + distances[4] = -1; + } + // Red Reef Close Right (id 11) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_CLOSE_RIGHT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[5] = distance; + } + else { + distances[5] = -1; + } + // Blue Reef Far Left (id 17) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_FAR_LEFT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[6] = distance; + } + else { + distances[6] = -1; + } + //Blue Reef Far (id 18) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_FAR_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[7] = distance; + } + else { + distances[7] = -1; + } + // Blue Reef Far Right (id 19) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_FAR_RIGHT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[8] = distance; + } + else { + distances[8] = -1; + } + // Blue Reef Close Right (id 20) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_CLOSE_RIGHT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[9] = distance; + } + else { + distances[9] = -1; + } + // Blue Reef Close (id 21) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_CLOSE_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[10] = distance; + } + else { + distances[10] = -1; + } + // Blue Reef Close Left (id 22) + if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_CLOSE_LEFT_ID) { + Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); + distances[11] = distance; + } + else { + distances[11] = -1; + } + // The part that does most of the fun stuff! + double minDist = 99999.999; //Set unreasonably high + // Finding the closest distance (not equal to -1) + for (double distValue : distances ) { + if(distValue!=-1 && distValue < minDist) { + minDist = distValue; + } + } + return minDist; + } + public double getDistanceToAlgaeMeters() { Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); if (angleToGoal.getDegrees() >= 0) { From d8211fb77e0de6a3d512fafd0445a2b759fc470a Mon Sep 17 00:00:00 2001 From: stwiggy Date: Tue, 25 Feb 2025 17:38:39 -0800 Subject: [PATCH 33/39] hell yeah!!! --- .../java/org/carlmontrobotics/Constants.java | 9 +- .../subsystems/Limelight.java | 173 ++---------------- .../subsystems/LimelightHelpers.java | 2 - 3 files changed, 23 insertions(+), 161 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 3390011..0b6a276 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -2,8 +2,6 @@ import edu.wpi.first.math.util.Units; -import edu.wpi.first.math.util.Units; - public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -11,10 +9,9 @@ public final class Constants { public static final class Limelightc { public static final String ROBOT_LL_NAME = "limelight-shooter"; // Please change the constant - public static final int[] VALID_IDS = {3, 16}; // Processors - public static final double MOUNT_ANGLE_DEG_LL = 45; // Please change - public static final double HEIGHT_FROM_GROUND_METERS_ROBOT = 0.206502; // Seems to be OK - public static final double ALGAE_HEIGHT = 0.4191; + public static final int[] VALID_IDS = {3, 16}; // TODO: change dont want 4 5 15 16 + public static final double MOUNT_ANGLE = 45; // check constant with desgin + public static final double LENS_HEIGHT_FROM_GROUND_METERS = 0.206502; // check constant with desgin public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index a8f01f8..a397c4d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -1,45 +1,29 @@ package org.carlmontrobotics.subsystems; -import static org.carlmontrobotics.Constants.Drivetrainc.*; import static org.carlmontrobotics.Constants.Limelightc.*; -import static org.carlmontrobotics.Constants.Limelightc.Apriltag.*; - import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Limelight extends SubsystemBase { private final Drivetrain drivetrain; - private final InterpolatingDoubleTreeMap shooterMap; - - private final double[] distances = {-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; // NEEDS TO SEE: Barge, Reef, Processor, Coral Dropoff public Limelight(Drivetrain drivetrain) { this.drivetrain = drivetrain; LimelightHelpers.SetFiducialIDFiltersOverride(ROBOT_LL_NAME, VALID_IDS); - shooterMap = new InterpolatingDoubleTreeMap(); // ADD VALUES AFTER TESTING!!! - // key is distance (meters), value is angle (rads) - shooterMap.put(2.06, .01); - shooterMap.put(3.05, 0.26); - shooterMap.put(2.5, 0.23); - shooterMap.put(1.39, -0.18); - shooterMap.put(3.45, 0.3); - shooterMap.put(3.1, 0.3); - SmartDashboard.putBoolean("Robot sees a Tag", seesTag()); // I need to improve this... - SmartDashboard.putNumber("Distance to Processor", getDistanceToProcessorMeters()); + SmartDashboard.putNumber("Distance to Processor", getDistanceToApriltag(ROBOT_LL_NAME, MOUNT_ANGLE, 0, LENS_HEIGHT_FROM_GROUND_METERS)); } @Override public void periodic() { SmartDashboard.putBoolean("Robot sees a Tag", seesTag()); - SmartDashboard.putNumber("Distance to Processor", getDistanceToProcessorMeters()); + SmartDashboard.putNumber("Distance to Processor", getDistanceToApriltag(ROBOT_LL_NAME, MOUNT_ANGLE, 0, LENS_HEIGHT_FROM_GROUND_METERS)); } // Very helpful accessors public double getTXDeg() { @@ -51,148 +35,31 @@ public double getTYDeg() { } // Distance accessors for field areas - public double getDistanceToProcessorMeters() { - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_PROCESSOR_ID - || LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_PROCESSOR_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - return distance; + public double getDistanceToApriltag(String limelightName, double mountAngle, double tagHeight, double limelightHeight) { + if (!seesTag()) { + return -1; } else { - return -1; + Rotation2d angleToGoal = Rotation2d.fromDegrees(mountAngle).plus(Rotation2d.fromDegrees(getTYDeg())); + double distance = (tagHeight - limelightHeight) / angleToGoal.getTan(); + return distance; } } - public double getDistanceToReefMeters() { - // I should really iterate this through a for loop... - // Red Reef Far Right (id 6) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_FAR_RIGHT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[0] = distance; - } - else { - distances[0] = -1; - } - //Red Reef Far (id 7) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_FAR_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[1] = distance; - } - else { - distances[1] = -1; - } - // Red Reef Far Left (id 8) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_FAR_LEFT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[2] = distance; - } - else { - distances[2] = -1; - } - // Red Reef Close Left (id 9) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_CLOSE_LEFT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[3] = distance; - } - else { - distances[3] = -1; - } - // Red Reef Close (id 10) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_CLOSE_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[4] = distance; - } - else { - distances[4] = -1; - } - // Red Reef Close Right (id 11) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == RED_REEF_CLOSE_RIGHT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[5] = distance; - } - else { - distances[5] = -1; - } - // Blue Reef Far Left (id 17) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_FAR_LEFT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[6] = distance; - } - else { - distances[6] = -1; - } - //Blue Reef Far (id 18) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_FAR_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[7] = distance; - } - else { - distances[7] = -1; - } - // Blue Reef Far Right (id 19) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_FAR_RIGHT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[8] = distance; - } - else { - distances[8] = -1; - } - // Blue Reef Close Right (id 20) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_CLOSE_RIGHT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[9] = distance; - } - else { - distances[9] = -1; - } - // Blue Reef Close (id 21) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_CLOSE_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[10] = distance; - } - else { - distances[10] = -1; - } - // Blue Reef Close Left (id 22) - if (LimelightHelpers.getFiducialID(ROBOT_LL_NAME) == BLUE_REEF_CLOSE_LEFT_ID) { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - double distance = (PROCESSOR_CENTER_HEIGHT_METERS - HEIGHT_FROM_GROUND_METERS_ROBOT) / angleToGoal.getTan(); - distances[11] = distance; - } - else { - distances[11] = -1; - } - // The part that does most of the fun stuff! - double minDist = 99999.999; //Set unreasonably high - // Finding the closest distance (not equal to -1) - for (double distValue : distances ) { - if(distValue!=-1 && distValue < minDist) { - minDist = distValue; - } - } - return minDist; + public double getApriltagID(String limelightName) { + return LimelightHelpers.getFiducialID(limelightName); } - public double getDistanceToAlgaeMeters() { - Rotation2d angleToGoal = Rotation2d.fromDegrees(MOUNT_ANGLE_DEG_LL).plus(Rotation2d.fromDegrees(getTYDeg())); - if (angleToGoal.getDegrees() >= 0) { - double distance = (HEIGHT_FROM_GROUND_METERS_ROBOT - ALGAE_HEIGHT)/Math.tan(Math.abs(angleToGoal.getRadians())); - return distance; - } - else { - return -1; - } + public double getRotateAngleRadMT2(String limelightName) { + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(limelightName); // pose of the target + + double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the + // robot and target + double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset + + double targetOffsetRads = MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI); + + return targetOffsetRads; } public boolean seesTag() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java index 1b885bc..befbca8 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java +++ b/src/main/java/org/carlmontrobotics/subsystems/LimelightHelpers.java @@ -8,8 +8,6 @@ import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.networktables.TimestampedDoubleArray; -import org.carlmontrobotics.subsystems.LimelightHelpers.LimelightResults; -import org.carlmontrobotics.subsystems.LimelightHelpers.PoseEstimate; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; From 5462f16faefceafec4d22358f06b1ceff672fe7a Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Tue, 25 Feb 2025 20:25:07 -0800 Subject: [PATCH 34/39] added camera --- .vscode/settings.json | 3 ++ .../carlmontrobotics/subsystems/Camera.java | 30 +++++++++++++++++++ .../subsystems/Limelight.java | 2 +- 3 files changed, 34 insertions(+), 1 deletion(-) create mode 100644 .vscode/settings.json create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Camera.java diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Camera.java b/src/main/java/org/carlmontrobotics/subsystems/Camera.java new file mode 100644 index 0000000..ee6648d --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Camera.java @@ -0,0 +1,30 @@ +package org.carlmontrobotics.subsystems; +import edu.wpi.first.cscore.CvSink; +import edu.wpi.first.cscore.CvSource; +import edu.wpi.first.cscore.MjpegServer; +import edu.wpi.first.cscore.UsbCamera; +import edu.wpi.first.cscore.UsbCameraInfo; +import edu.wpi.first.util.PixelFormat; +public class Camera { + public Camera(){ + // Last year's code + UsbCamera usbCamera = new UsbCamera("USB Camera 0", 0); + MjpegServer mjpegServer1 = new MjpegServer("serve_USB Camera 0", 1 + ); + mjpegServer1.setSource(usbCamera); + + // Creates the CvSink and connects it to the UsbCamera + CvSink cvSink = new CvSink("opencv_USB Camera 0"); + cvSink.setSource(usbCamera); + + // Creates the CvSource and MjpegServer [2] and connects them + CvSource outputStream = new CvSource("Blur", PixelFormat.kMJPEG, 640, 480, 30); + MjpegServer mjpegServer2 = new MjpegServer("serve_Blur", 2); + mjpegServer2.setSource(outputStream); + + + //OR + + } + +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index a397c4d..d896759 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -54,7 +54,7 @@ public double getRotateAngleRadMT2(String limelightName) { Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(limelightName); // pose of the target double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the - // robot and target + // robot and target double targetZ = -targetPoseRobotSpace.getZ(); // the sideways offset double targetOffsetRads = MathUtil.inputModulus(Math.atan2(targetX, targetZ), -Math.PI, Math.PI); From 061dd23e246beb2cfa4ca14f1f039a246abf95cd Mon Sep 17 00:00:00 2001 From: stwiggy Date: Sat, 1 Mar 2025 16:48:44 -0800 Subject: [PATCH 35/39] reminder to fix constants! --- .../java/org/carlmontrobotics/Constants 2.j | 467 ----------------- .../java/org/carlmontrobotics/Constants.java | 23 +- .../org/carlmontrobotics/RobotContainer 2.j | 488 ------------------ .../commands/AlignToCoralStation.java | 90 ++++ .../commands/AlignToReef.java | 89 ++++ .../subsystems/Limelight.java | 37 +- 6 files changed, 219 insertions(+), 975 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/Constants 2.j delete mode 100644 src/main/java/org/carlmontrobotics/RobotContainer 2.j create mode 100644 src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java create mode 100644 src/main/java/org/carlmontrobotics/commands/AlignToReef.java diff --git a/src/main/java/org/carlmontrobotics/Constants 2.j b/src/main/java/org/carlmontrobotics/Constants 2.j deleted file mode 100644 index 2b72303..0000000 --- a/src/main/java/org/carlmontrobotics/Constants 2.j +++ /dev/null @@ -1,467 +0,0 @@ - -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.button.POVButton; -import edu.wpi.first.wpilibj.util.Color; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide - * numerical or boolean - * constants. This class should not be used for any other purpose. All constants - * should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

- * It is advised to statically import this class (or one of its inner classes) - * wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public static final double g = 9.81; // meters per second squared - public static final class Led { - - public static final Color8Bit DEFAULT_COLOR_BLUE = new Color8Bit(0, 0, 200); - public static final Color8Bit DETECT_NOTE_YELLOW = new Color8Bit(255, 255, 0); - public static final Color8Bit HOLDING_GREEN = new Color8Bit(0, 250, 0); - public static final Color8Bit RED_NEO_550_MOTOR = new Color8Bit(255,0,0); - public static final int ledPort = 0; - // TODO: figure out how to get port of LED, it could be 0 or - } - - public static final class Effectorc { - // PID values - - public static final int INTAKE = 0; - public static final int OUTTAKE = 1; - // 0.0001184 - - /* - * public static final double kP = 0.0001067; public static final double kI = 0; public - * static final double kD = 0; public static final double kS = 0; public static final double - * kV = 0.11124; public static final double kA = 0.039757; - */ - - public static final double[] kP = {0, 0.0001067}; - public static final double[] kI = { /* /Intake/ */0, /* /Outake/ */0 }; - public static final double[] kD = { /* /Intake/ */0, /* /Outake/ */0 }; - public static final double[] kS = { /* /Intake/ */0.22, /* /Outake/ */0.29753 * 2 }; - public static final double[] kV = {0.122, 0.11124}; - public static final double[] kA = {0, 0.039757}; - public static final int INTAKE_PORT = 9; // port - public static final int OUTTAKE_PORT = 10; // port - public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port - public static final int OUTAKE_DISTANCE_SENSOR_PORT = 10; // port - public static final double DISTANCE_BETWEEN_SENSORS_INCHES = 8.189; // inches - public static final double OFFSET_FROM_GROUND_INCHES = 21; // in - public static final double DS_DEPTH_INCHES = 9.97; // Distance sensor Depth - public static final double DETECT_DISTANCE_INCHES = 13; - - public static final double INTAKE_RPM = 6300; - public static final double INTAKE_SLOWDOWN_RPM = 4500; - public static final double MAX_SECONDS_OVERLOAD = 2.0; - public static final double PASS_RPM = 2000; - public static final double TEST_RPM = 3000; - public static final double AMP_RPM = 1000; - public static final double SUBWOOFER_RPM = 2100; - public static final double PODIUM_RPM = 4000; - public static final double SPEAKER_RPM = 2100; - // WTF FAB ISSUE - //public static final double SAFE_RPM = 6000;// WTF FAB ISSUE - - public static final double EJECT_RPM_INTAKE = -2550; - public static final double EJECT_RPM_OUTAKE = -2550; - - public static final double MANUAL_RPM_MAX = 9500; - - public static final double RPM_TOLERANCE = 200; - public static final double SPEAKER_HEIGHT_INCHES = 83; // inches - - public static final boolean INTAKE_MOTOR_INVERSION = true; - public static final boolean OUTAKE_MOTOR_INVERSION = false; - - public static final double EJECT_TIME_SECS = 5.; - public static final double EJECT_MIN_SECS = 1.25; - public static final double INTAKE_TIME_SECS = 4.; - public static final double ledDefaultColorRestoreTime = 3; - public static final Color defaultColor = new Color(0, 0, 200); - public static final Color pickupSuccessColor = new Color(0, 200, 0); - - } - - public static final class Armc { - - // Motor port - public static final int ARM_MOTOR_PORT_MASTER = - CONFIG.isSwimShady() ? 20 : 13; - public final static int ARM_MOTOR_PORT_FOLLOWER = 18; - // Config for motors - public static final boolean MOTOR_INVERTED_MASTER = false; - public static final boolean MOTOR_INVERTED_FOLLOWER = true; // verifyed by design AND physical testing - - public static final double ROTATION_TO_RAD = 2 * Math.PI; - public static final boolean ENCODER_INVERTED = true; - - // difference between CoG and arm is .328 rad - public static final double ENCODER_OFFSET_RAD = 6.132; - // 0.08 + .328 - 0.404; // - 0.6095; - - // TODO: finish understand why this is broken public static final Measure - // INTAKE_ANGLE = Degrees.to(-1); - - // USE RADIANS FOR THE ARM - public static final double INTAKE_ANGLE_RAD = Units.degreesToRadians(0); - public static final double HANG_ANGLE_RAD = Units.degreesToRadians(90); - public static final double AMP_ANGLE_RAD = 1.28; - public static final double AMP_ANGLE_RAD_NEW_MOTOR = 1.456; - public static final double SPEAKER_ANGLE_RAD = -0.2; - public static final double PODIUM_ANGLE_RAD = -0.2 + Units.degreesToRadians(7.5); - // -0.427725 - public static final double GROUND_INTAKE_POS = -0.34537; - - public static final double HANG_ANGL_RAD = GROUND_INTAKE_POS + Units.degreesToRadians(30); - - public static final double SUBWOOFER_ANGLE_RAD = -0.22;// touching the base of the speaker - public static final double SAFE_ZONE_ANGLE_RAD = Units.degreesToRadians(36);// touching the white line - //public static final double PODIUM_ANGLE_RAD = Units.degreesToRadians(40);// touching the safe pad on the stage - - // 0.4 rad for shooting at podium - - // PID, Feedforward, Trapezoid - public static final double kP = 0.45;// 5.7938; // (2 * Math.PI); - public static final double kI = 0; - public static final double kD = 0 * 1000; - public static final double kS = 1.6 / 2; // 0.1498; - public static final double kG = 0.8067; // 0.3489; - public static final double kV = 5.1201 / (2 * Math.PI); - public static final double kA = 0.43308 / (2 * Math.PI); - public static final double IZONE_RAD = 0; - // fine for now, change it later before use - ("Incorect use of setIZone()" - // Issue #22) - // public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s - public static final double MAX_FF_VEL_RAD_P_S = Math.PI * .5; - public static final double MAX_FF_ACCEL_RAD_P_S = 53.728 / 4; // rad / s^2 - // ((.89*2)/(1.477/(61.875^2))/61.875)-20.84 - - public static final double MAX_FF_VEL_RAD_P_S_BABY = 0; - public static final double MAX_FF_ACCEL_RAD_P_S_BABY = 0; - //TODO: determine these values^ - - - public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0; - public static final double CLIMB_POS = 1.701; //RADIANS - public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12); - public static final double MAX_VOLTAGE = 0.5; // (kS + kG + 1)/12; - public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE/12 *0.7; - public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE/12*0.7; - public static final double CLIMB_FINISH_POS = -0.38; - // if needed - public static final double COM_ARM_LENGTH_METERS = 0.381; - public static final double ARM_MASS_KG = 9.59302503; - - public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS;// initalized by arm constructor - // other0; - - // public static final double MARGIN_OF_ERROR = Math.PI / 18; v^2 /a - - // Boundaries - public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder - public static final double POS_TOLERANCE_RAD = - Units.degreesToRadians(5); // placeholder //Whether or not this is the actual - // account - // idk TODO: test on actual encoder without a conversion - // factor - public static final double VEL_TOLERANCE_RAD_P_SEC = (POS_TOLERANCE_RAD / 0.02); // 20ms per robot loop - public static final double UPPER_ANGLE_LIMIT_RAD = 1.63; - public static final double LOWER_ANGLE_LIMIT_RAD = -0.5; - public static final double CLIMBER_UP_ANGLE_RAD = UPPER_ANGLE_LIMIT_RAD; - public static final double CLIMBER_DOWN_ANGLE_RAD = LOWER_ANGLE_LIMIT_RAD; - - public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; - - public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = 0.5; - public static final double SMART_CURRENT_LIMIT_TIMEOUT = 0.8; - - - public static final int NEO_BUILTIN_ENCODER_CPR = 42; - } - - public static final class Drivetrainc { - - // #region Subsystem Constants - - public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) - : Units.inchesToMeters(16.75); - public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) - : Units.inchesToMeters(23.75); - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); - // The gearing reduction from the drive motor controller to the wheels - // Gearing for the Swerve Modules is 6.75 : 1 - public static final double driveGearing = 6.75; - // Turn motor shaft to "module shaft" - public static final double turnGearing = 150 / 7; - - public static final double driveModifier = 1; - public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* - * empirical - * correction - */; - public static final double mu = 1; /* 70/83.2; */ - - public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s - // Angular speed to translational speed --> v = omega * r / gearing - public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; - public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out - public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out - // seconds it takes to go from 0 to 12 volts(aka MAX) - public static final double secsPer12Volts = 0.1; - - // maxRCW is the angular velocity of the robot. - // Calculated by looking at one of the motors and treating it as a point mass - // moving around in a circle. - // Tangential speed of this point mass is maxSpeed and the radius of the circle - // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) - // Angular velocity = Tangential speed / radius - public static final double maxRCW = maxSpeed / swerveRadius; - - public static final boolean[] reversed = { false, false, false, false }; - // public static final boolean[] reversed = {true, true, true, true}; - // Determine correct turnZero constants (FL, FR, BL, BR) - public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } - : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } - : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ - - // kP, kI, and kD constants for turn motor controllers in the order of - // front-left, front-right, back-left, back-right. - // Determine correct turn PID constants - public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, - // 0.00374}; - public static final double[] turnkI = { 0, 0, 0, 0 }; - public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d - // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; - - // V = kS + kV * v + kA * a - // 12 = 0.2 + 0.00463 * v - // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 }; - public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 }; - - // kP is an average of the forward and backward kP values - // Forward: 1.72, 1.71, 1.92, 1.94 - // Backward: 1.92, 1.92, 2.11, 1.89 - // Order of modules: (FL, FR, BL, BR) - public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } - : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, - // 1.915/100}; - public static final double[] drivekI = { 0, 0, 0, 0 }; - public static final double[] drivekD = { 0, 0, 0, 0 }; - public static final boolean[] driveInversion = (CONFIG.isSwimShady() - ? new boolean[] { false, false, false, false } - : new boolean[] { true, false, true, false }); - public static final boolean[] turnInversion = { true, true, true, true }; - // kS - public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kBackwardVolts = kForwardVolts; - - public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kBackwardVels = kForwardVels; - public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kBackwardAccels = kForwardAccels; - - public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second - public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 - public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java - // The maximum acceleration the robot can achieve is equal to the coefficient of - // static friction times the gravitational acceleration - // a = mu * 9.8 m/s^2 - public static final double autoCentripetalAccel = mu * g * 2; - - public static final boolean isGyroReversed = true; - - // PID values are listed in the order kP, kI, and kD - public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } - : new double[] { 2, 0.0, 0.0 }; - public static final double[] yPIDController = xPIDController; - public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } - : new double[] {0.05, 0.0, 0.00}; - - public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, - autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, - kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, - driveInversion, reversed, driveModifier, turnInversion); - - // public static final Limelight.Transform limelightTransformForPoseEstimation = - // Transform.BOTPOSE_WPIBLUE; - - // #endregion - - // #region Ports - - public static final int driveFrontLeftPort = CONFIG.isSwimShady() ? 8 : 11; // - public static final int driveFrontRightPort = CONFIG.isSwimShady() ? 13 : 19; // - public static final int driveBackLeftPort = CONFIG.isSwimShady() ? 5 : 14; // - public static final int driveBackRightPort = CONFIG.isSwimShady() ? 11 : 17; // correct - - public static final int turnFrontLeftPort = CONFIG.isSwimShady() ? 7 : 12; // - public static final int turnFrontRightPort = CONFIG.isSwimShady() ? 14 : 20; // 20 - public static final int turnBackLeftPort = CONFIG.isSwimShady() ? 6 : 15; // - public static final int turnBackRightPort = CONFIG.isSwimShady() ? 12 : 16; // correct - - public static final int canCoderPortFL = CONFIG.isSwimShady() ? 4 : 0; - public static final int canCoderPortFR = CONFIG.isSwimShady() ? 2 : 3; - public static final int canCoderPortBL = CONFIG.isSwimShady() ? 3 : 2; - public static final int canCoderPortBR = CONFIG.isSwimShady() ? 1 : 1; - - // #endregion - - // #region Command Constants - - public static double kNormalDriveSpeed = 1; // Percent Multiplier - public static double kNormalDriveRotation = 0.5; // Percent Multiplier - public static double kSlowDriveSpeed = 0.4; // Percent Multiplier - public static double kSlowDriveRotation = 0.250; // Percent Multiplier - - //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use - public static double kBabyDriveSpeed = 0.3; - public static double kBabyDriveRotation = 0.2; - public static double kAlignMultiplier = 1D / 3D; - public static final double kAlignForward = 0.6; - - public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to - // orient the wheels to the correct angle. This - // should be very small to avoid actually moving the - // robot. - - public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, - // Meters, - // Degrees - public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, - // Meters, - // Degrees/Second - - // #endregion - // #region Subsystem Constants - - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; - public static final double turnIzone = .1; - - public static final double driveIzone = .1; - - public static final class Autoc { - public static final ReplanningConfig replanningConfig = new ReplanningConfig( /* - * put in - * Constants.Drivetrain.Auto - */ - false, // replan at start of path if robot not at start of path? - false, // replan if total error surpasses total error/spike threshold? - 1.5, // total error threshold in meters that will cause the path to be replanned - .8 // error spike threshold, in meters, that will cause the path to be replanned - ); - public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, - 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the - // angular constraints have no effect. - } - } - - public static final class Limelightc { - public static final String INTAKE_LL_NAME = "limelight-intake"; - public static final String SHOOTER_LL_NAME = "limelight-shooter"; - - public static final int[] VALID_IDS = { 4, 7 }; - - public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field - public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field - public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high - // to represent unreliable heading - public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity - - public static final double ERROR_TOLERANCE_RAD = 0.1; - public static final double HORIZONTAL_FOV_DEG = 0; // unused - public static final double RESOLUTION_WIDTH_PIX = 640; // unused - public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; - public static final double MOUNT_ANGLE_DEG_INTAKE = -29; - public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); - public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = - Units.inchesToMeters(13); - public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused - public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused - public static final double MIN_MOVEMENT_METERSPSEC = 1.5; - public static final double MIN_MOVEMENT_RADSPSEC = 0.5; - public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); - public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); - public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); - public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused - public static final class Apriltag { - public static final int RED_SPEAKER_CENTER_TAG_ID = 4; - public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; - public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); - public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); - public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); - } - } - - public static final class OI { - public static final class Driver { - public static final int port = 0; - - public static final int slowDriveButton = Button.kLeftBumper.value; - public static final int resetFieldOrientationButton = Button.kRightBumper.value; - public static final int toggleFieldOrientedButton = Button.kStart.value; - - public static final int rotateFieldRelative0Deg = Button.kY.value; - public static final int rotateFieldRelative90Deg = Button.kB.value; - public static final int rotateFieldRelative180Deg = Button.kA.value; - public static final int rotateFieldRelative270Deg = Button.kX.value; - } - - public static final class Manipulator { - public static final int port = 1; - // NEW BINDINGS(easier for manipulator) - // Xbox left joy Y axis -> raw Intake control - // Xbox right joy Y axis -> raw Outtake control - // Xbox right trigger axis -> Intake pos + intake - // Xbox left trigger axis -> amp pos , eject into amp - // Xbox left bumper button -> CLOSE Speaker pos , Fire - // Xbox right bumper button -> SAFE Speaker pos , Fire - // Xbox X button -> goto Intake pos - // Xbox Y button -> Eject rpm - public static final Axis INTAKE_BUTTON = Axis.kLeftTrigger; - public static final Axis SHOOTER_BUTTON = Axis.kRightTrigger; - public static final int EJECT_BUTTON = Button.kLeftBumper.value; - public static final int OPPOSITE_EJECT = Button.kRightBumper.value; - public static final Axis INTAKE_AX = Axis.kRightTrigger; - public static final Axis AMP_AX = Axis.kLeftTrigger; - public static final int SPEAKER_POS = Button.kX.value; - public static final int ARM_TO_AMP_BUTTON = Button.kY.value; - public static final int PASS_TO_OUTTAKE_STICK = - Button.kLeftStick.value; - public static final int PASS_TO_INTAKE_STICK = - Button.kRightStick.value; - public static final int UP_D_PAD = 0; - public static final int DOWN_D_PAD = 180; - public static final int LEFT_D_PAD = 270; - - public static final int A_BUTTON = Button.kA.value; - public static final int RAMP_OUTTAKE = Button.kB.value; - } - - - public static final double JOY_THRESH = 0.01; - public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. - - } -} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 55fdf0e..bad8168 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -229,11 +229,26 @@ public static final class Autoc { } } public static final class Limelightc { - - public static final class Apriltag { + public static final String CORAL_LL = "limelight-coral"; + public static final String REEF_LL = "limelight-reef"; - } - } + public static final int[] CORAL_VALID_IDS = {1, 2, 12, 13}; + public static final int[] REEF_VALID_IDS = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; + public static final double CORAL_MOUNT_ANGLE = 25; // pitch + public static final double REEF_MOUNT_ANGLE = 15; // pitch TODO: change + public static final double CORAL_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: change + public static final double REEF_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: change + + + public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field + public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high + + public static final class Apriltagc { + public static final double CORAL_HEIGHT_METERS = Units.inchesToMeters(47.88); // Going to re-assume that this is for Apriltag center. + public static final double REEF_HEIGHT_METERS = Units.inchesToMeters(8.75); // Also center of Reef + } + } public static final class OI { public static final class Driver { diff --git a/src/main/java/org/carlmontrobotics/RobotContainer 2.j b/src/main/java/org/carlmontrobotics/RobotContainer 2.j deleted file mode 100644 index ebddde9..0000000 --- a/src/main/java/org/carlmontrobotics/RobotContainer 2.j +++ /dev/null @@ -1,488 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics; - -// java -import java.util.ArrayList; -import java.util.List; - -// constants -import static org.carlmontrobotics.Constants.Armc.*; -import static org.carlmontrobotics.Constants.OI.Manipulator.*; -import static org.carlmontrobotics.Constants.Effectorc.*; -import static org.carlmontrobotics.Constants.Limelightc.*; - -// non static constants -import org.carlmontrobotics.Constants.OI; -import org.carlmontrobotics.Constants.OI.Driver; -import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.Constants.Armc; -import org.carlmontrobotics.Constants.Drivetrainc.Autoc; -import org.carlmontrobotics.Constants.Effectorc; -// robotcode2024 imports -import org.carlmontrobotics.commands.*; -import org.carlmontrobotics.subsystems.*; - -import com.pathplanner.lib.auto.AutoBuilder; -// pathplanner -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathPlannerPath; - -// wpilib geometry classes -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; -// controllers -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; - -// dashboards -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -// commands -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -// control bindings -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class RobotContainer { - private static boolean babyMode = false; - - // 1. using GenericHID allows us to use different kinds of controllers - // 2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(Driver.port); - public final GenericHID manipulatorController = new GenericHID(Manipulator.port); - private final IntakeShooter intakeShooter = new IntakeShooter(); - - // ignore warning, LED must be initialized - private final Led led = new Led(intakeShooter); - private final Arm arm = new Arm(); - private final Drivetrain drivetrain = new Drivetrain(); - private final Limelight limelight = new Limelight(drivetrain); - - /* These are assumed to be equal to the AUTO ames in pathplanner */ - /* These must be equal to the pathPlanner path names from the GUI! */ - // Order matters - but the first one is index 1 on the physical selector - index - // 0 is reserved for - // null command. - // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 - // is simple - // straight - private List autoCommands; - private SendableChooser autoSelector = new SendableChooser(); - - private boolean hasSetupAutos = false; - private final String[] autoNames = new String[] { - /* These are assumed to be equal to the AUTO ames in pathplanner */ - "Left-Auto Ruiner", "Center-Auto Ruiner", "Right-Auto Ruiner", - "Right Limelight 4 Piece", "Left Limelight 4 Piece", - "Center Limelight 4 Piece", - - "Left-Amp", - - "Center Limelight 1 Piece", "Left Limelight 1 Piece", - "Right Limelight 1 Piece", "Center Field Limelight", - "Center Field Limelight(No Preload)", "Center Forward", - "Right Forward", "Left Forward", - "Backup-Center", "Backup-Right", "Backup-Left", - "Preload1Center", "Preload1Right", "Preload1Left", - - }; - DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; - - public RobotContainer() { - { - // Put any configuration overrides to the dashboard and the terminal - SmartDashboard.putData("CONFIG overrides", Config.CONFIG); - System.out.println(Config.CONFIG); - - SmartDashboard.putData("BuildConstants", BuildInfo.getInstance()); - - SmartDashboard.setDefaultBoolean("babymode", babyMode); - SmartDashboard.setPersistent("babymode"); - // safe auto setup... stuff in setupAutos() is not safe to run here - will break - // robot - registerAutoCommands(); - SmartDashboard.putData(autoSelector); - SmartDashboard.setPersistent("SendableChooser[0]"); - - autoSelector.addOption("Nothing", 0); - autoSelector.addOption("Raw Forward", 1); - autoSelector.addOption("PP Simple Forward", 2);// index corresponds to index in autoCommands[] - - int i = 3; - for (String n : autoNames) { - autoSelector.addOption(n, i); - i++; - } - - ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); - autoSelectorTab.add(autoSelector).withSize(2, 1); - } - - setDefaultCommands(); - setBindingsDriver(); - // setBindingsManipulatorENDEFF(); - setBindingsManipulator(); - } - - private void setDefaultCommands() { - drivetrain.setDefaultCommand(new TeleopDrive(drivetrain, - () -> ProcessedAxisValue(driverController, Axis.kLeftY), - () -> ProcessedAxisValue(driverController, Axis.kLeftX), - () -> ProcessedAxisValue(driverController, Axis.kRightX), - () -> driverController.getRawButton(Driver.slowDriveButton))); - // TODO: Are we going to use default command for intakeshooter? - intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter, - () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), - manipulatorController, driverController)); - // TODO - // intakeShooter.setDefaultCommand(new RampMaxRPMDriving(intakeShooter)); - - arm.setDefaultCommand( - Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm) - : new TeleopArm(arm, - () -> ProcessedAxisValue(manipulatorController, - Axis.kLeftY))); - - } - - private void setBindingsDriver() { - new JoystickButton(driverController, Driver.resetFieldOrientationButton) - .onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); - // axisTrigger(driverController, Axis.kRightTrigger) - // .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), - // new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight))); - - new POVButton(driverController, 0) - .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 1))); - - axisTrigger(driverController, Axis.kLeftTrigger) - // .onTrue(new AlignToApriltag(drivetrain, limelight)); - .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false))) - .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true))); - - axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) - .whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0)); - new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue( - new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); - new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(270), - drivetrain)); - new JoystickButton(driverController, Driver.rotateFieldRelative180Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(180), - drivetrain)); - new JoystickButton(driverController, Driver.rotateFieldRelative270Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), - drivetrain)); - } - - private void setBindingsManipulator() { - new JoystickButton(manipulatorController, EJECT_BUTTON) - .onTrue(new Eject(intakeShooter)); - - // new JoystickButton(manipulatorController, A_BUTTON) - // .onTrue(new RampMaxRPMDriving(intakeShooter)); - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - // new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), - // new PassToOuttake(intakeShooter))); - - axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), - new PassToOuttake(intakeShooter)), new InstantCommand(() -> { - }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME))); - - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - // .whileTrue(new PassToOuttake(intakeShooter)); - - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - // .whileTrue(new AimArmSpeaker(arm, limelight)); - - new JoystickButton(manipulatorController, RAMP_OUTTAKE) - .whileTrue(new RampMaxRPM(intakeShooter)); - new JoystickButton(manipulatorController, OPPOSITE_EJECT) - .whileTrue(new EjectOuttakeSide(intakeShooter)); - - /* - * axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - * .onTrue(Config.CONFIG.useSmartDashboardControl - * ? new TestRPM(intakeShooter) - * : new Outtake(intakeShooter, arm) - */ - - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - // .onTrue(new PassToOuttake(intakeShooter)); - - axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) - .whileTrue(new Intake(intakeShooter)); - new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON) - .onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR)); - new JoystickButton(manipulatorController, A_BUTTON) - .onTrue(new ArmToPos(arm, GROUND_INTAKE_POS)); - new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK) - .onTrue(new PassToOuttake(intakeShooter)); - new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK) - .onTrue(new PassToIntake(intakeShooter)); - new JoystickButton(manipulatorController, SPEAKER_POS) - .onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD)); - new POVButton(manipulatorController, UP_D_PAD) - .onTrue(new ArmToPos(arm, CLIMB_POS)); - new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm)); - new POVButton(manipulatorController, LEFT_D_PAD) - .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD)); - - } - - /** - * Flips an axis' Y coordinates upside down, but only if the select axis is a - * joystick axis - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double getStickValue(GenericHID hid, Axis axis) { - return hid.getRawAxis(axis.value) - * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); - } - - /** - * Processes an input from the joystick into a value between -1 and 1, - * sinusoidally instead of - * linearly - * - * @param value The value to be processed. - * @return The processed value. - */ - private double inputProcessing(double value) { - double processedInput; - // processedInput = - // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); - processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) - * ((1 - Math.cos(value * Math.PI)) / 2), value); - return processedInput; - } - - /** - * Combines both getStickValue and inputProcessing into a single function for - * processing joystick - * outputs - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double ProcessedAxisValue(GenericHID hid, Axis axis) { - return DeadzonedAxis(inputProcessing(getStickValue(hid, axis))); - } - - /** - * Returns zero if a axis input is inside the deadzone - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double DeadzonedAxis(double axOut) { - return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut; - } - - /** - * Returns a new instance of Trigger based on the given Joystick and Axis - * objects. The Trigger is - * triggered when the absolute value of the stick value on the specified axis - * exceeds a minimum - * threshold value. - * - * @param stick The Joystick object to retrieve stick value from. - * @param axis The Axis object to retrieve value from the Joystick. - * @return A new instance of Trigger based on the given Joystick and Axis - * objects. * @throws - * NullPointerException if either stick or axis is null. - */ - private Trigger axisTrigger(GenericHID controller, Axis axis) { - return new Trigger(() -> Math - .abs(getStickValue(controller, axis)) > OI.MIN_AXIS_TRIGGER_VALUE); - } - - private void registerAutoCommands() { - //// AUTO-USABLE COMMANDS - NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); - NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); - - // NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm, - // Armc.SPEAKER_ANGLE_RAD, 0)); - NamedCommands.registerCommand("ArmToAmp", - new ArmToPos(arm, Armc.AMP_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToSubwoofer", - new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToPodium", - new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToGround", - new ArmToPos(arm, GROUND_INTAKE_POS)); - - NamedCommands.registerCommand("RampRPMAuton", - new RampRPMAuton(intakeShooter)); - - NamedCommands.registerCommand("SwitchRPMShoot", - new Outtake(intakeShooter, arm)); - - // NamedCommands.registerCommand("AutonRuinerShoot", new - // AutonRuinerShoot(intakeShooter)); - // NamedCommands.registerCommand("IntakeAutonRuiner", new - // IntakeAutonRuiner(intakeShooter)); - - NamedCommands.registerCommand("AutonRuinerShootAndIntake", - new AutonRuinerShootAndIntake(intakeShooter, arm)); - - NamedCommands.registerCommand("PassToOuttake", - new PassToOuttake(intakeShooter)); - NamedCommands.registerCommand("AimArmSpeakerMT2", - new AimArmSpeaker(arm, limelight)); - NamedCommands.registerCommand("AlignToAprilTagMegaTag2", - new AlignToApriltag(drivetrain, limelight, 0.0)); - NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ParallelDeadlineGroup( - new WaitCommand(3.0), - new SequentialCommandGroup( - // TODO: Use Align To Drivetrain - // new AlignDrivetrain(drivetrain), - new ParallelCommandGroup( - new AlignToApriltag(drivetrain, limelight, 0.0), - new AimArmSpeaker(arm, limelight), - new RampRPMAuton(intakeShooter)), - new PassToOuttake(intakeShooter), - new ArmToPos(arm, GROUND_INTAKE_POS))))); - NamedCommands.registerCommand("ShootSubwoofer", - new SequentialCommandGroup(new ParallelCommandGroup( - new ArmToPos(arm, - Armc.SUBWOOFER_ANGLE_RAD), - new RampRPMAuton(intakeShooter)), - new PassToOuttake(intakeShooter), - new ArmToPos(arm, GROUND_INTAKE_POS))); - NamedCommands.registerCommand("Limelight Intake CCW", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 1))); - NamedCommands.registerCommand("Limelight Intake CW", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, -1))); - - NamedCommands.registerCommand("Limelight Intake Straight", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 0))); - - NamedCommands.registerCommand("StopIntake", - new InstantCommand(intakeShooter::stopIntake)); - NamedCommands.registerCommand("StopOutake", - new InstantCommand(intakeShooter::stopOuttake)); - NamedCommands.registerCommand("StopBoth", - new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake), - new InstantCommand(intakeShooter::stopOuttake))); - } - - private void setupAutos() { - //// CREATING PATHS from files - if (!hasSetupAutos) { - autoCommands=new ArrayList();//clear old/nonexistent autos - - for (int i = 0; i < autoNames.length; i++) { - String name = autoNames[i]; - - autoCommands.add(new PathPlannerAuto(name)); - - /* - * // Charles' opinion: we shouldn't have it path find to the starting pose at the start of match - * new SequentialCommandGroup( - * AutoBuilder.pathfindToPose( - * PathPlannerAuto.getStaringPoseFromAutoFile(name), - * PathPlannerAuto.getPathGroupFromAutoFile(name).get(0). - * getPreviewStartingHolonomicPose(), - * Autoc.pathConstraints), - * new PathPlannerAuto(name)); - */ - } - hasSetupAutos = true; - - // NOTHING - autoCommands.add(0, new PrintCommand("Running NULL Auto!")); - // RAW FORWARD command - autoCommands.add(1, new SequentialCommandGroup( - new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new WaitCommand(0.5), - new LastResortAuto(drivetrain))); - // dumb PP forward command - autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!")); - } - // force regeneration each auto call - autoCommands.set(2, constructPPSimpleAuto());//overwrite this slot each time auto runs - } - - public Command constructPPSimpleAuto() { - /** - * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd - * (m/s): 4.30 - * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max - * AngAccel: 360 - * (guesses!) - */ - // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER - Pose2d currPos = drivetrain.getPose(); - - //FIXME running red PP file autos seems to break something, so the robot drivetrain drives in the wrong direction. - //running blue PP autos is fine though - //Note: alliance detection and path generation work correctly! - //Solution: Redeploy after auto. - Pose2d endPos = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) - ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) - : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); - - List bezierPoints = PathPlannerPath.bezierFromPoses(currPos, endPos); - - // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, rad/s^2 */ - PathPlannerPath path = new PathPlannerPath(bezierPoints, - Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); - - path.preventFlipping = false;// don't flip, we do that manually already. - - return new SequentialCommandGroup( - new InstantCommand(()->drivetrain.drive(-.0001, 0, 0)),//align drivetrain wheels. - AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); - } - - public Command getAutonomousCommand() { - setupAutos(); - - Integer autoIndex = autoSelector.getSelected(); - - if (autoIndex != null && autoIndex != 0) { - new PrintCommand("Running selected auto: " + autoSelector.toString()); - return autoCommands.get(autoIndex.intValue()); - } - return new PrintCommand("No auto :("); - } - -} diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java b/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java new file mode 100644 index 0000000..185bdc5 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java @@ -0,0 +1,90 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; +import static org.carlmontrobotics.Constants.Limelightc.CORAL_LL; + +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class AlignToCoralStation extends Command { + + public final Drivetrain drivetrain; + public final TeleopDrive teleopDrive; + private Timer time = new Timer(); + private Limelight limelight; + private double startTime; + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2]); + double RotationSpeed = 0.0; + public AlignToCoralStation(Drivetrain drivetrain, Limelight limelight, double RotationSpeed) { + this.limelight = limelight; + this.drivetrain = drivetrain; + this.RotationSpeed = RotationSpeed; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + rotationPID.enableContinuousInput(-180, 180); + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2("limelight-coral"))); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + time.reset(); + time.start(); + startTime = Timer.getFPGATimestamp(); + addRequirements(drivetrain); + } + + @Override + public void execute() { + // double kp = SmartDashboard.getNumber("apriltag align kp", + // rotationPID.getP()); + // double ki = SmartDashboard.getNumber("apriltag align ki", + // rotationPID.getI()); + // double kd = SmartDashboard.getNumber("apriltag align kd", + // rotationPID.getD()); + // if (kp != rotationPID.getP()) + // rotationPID.setP(kp); + // if (ki != rotationPID.getI()) + // rotationPID.setI(ki); + // if (kd != rotationPID.getD()) + // rotationPID.setD(kd); + // double posTolerance = SmartDashboard.getNumber( + // "apriltag align pos tolerance", + // rotationPID.getPositionTolerance()); + // double velTolerance = SmartDashboard.getNumber( + // "apriltag align vel tolerance", + // rotationPID.getVelocityTolerance()); + // if (posTolerance != rotationPID.getPositionTolerance() + // || velTolerance != rotationPID + // .getVelocityTolerance()) + // rotationPID.setTolerance(posTolerance, velTolerance); + // SmartDashboard.putNumber("apriltag align pos error (rad)", + // rotationPID.getPositionError()); + // SmartDashboard.putNumber("apriltag align vel error (rad/s)", + // rotationPID.getVelocityError()); + + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2(CORAL_LL))); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + double rotationDrive = rotationPID.calculate(drivetrain.getHeading()); + if (!limelight.seesTag(CORAL_LL)) rotationDrive = RotationSpeed; + if (rotationPID.atSetpoint()) rotationDrive = 0; + if (teleopDrive == null) drivetrain.drive(0, 0, rotationDrive); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], rotationDrive); + } + } + + @Override + public boolean isFinished() { + // return false; + // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint() || (Timer.getFPGATimestamp() - startTime) > 3; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToReef.java b/src/main/java/org/carlmontrobotics/commands/AlignToReef.java new file mode 100644 index 0000000..de5d1dd --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/AlignToReef.java @@ -0,0 +1,89 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.Drivetrainc.*; +import static org.carlmontrobotics.Constants.Limelightc.REEF_LL; + +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class AlignToReef extends Command { + + public final Drivetrain drivetrain; + public final TeleopDrive teleopDrive; + private Timer time = new Timer(); + private Limelight limelight; + private double startTime; + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], thetaPIDController[2]); + double RotationSpeed = 0.0; + public AlignToReef(Drivetrain drivetrain, Limelight limelight, double RotationSpeed) { + this.limelight = limelight; + this.drivetrain = drivetrain; + this.RotationSpeed = RotationSpeed; + this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand(); + rotationPID.enableContinuousInput(-180, 180); + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2(REEF_LL))); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]); + SendableRegistry.addChild(this, rotationPID); + time.reset(); + time.start(); + startTime = Timer.getFPGATimestamp(); + addRequirements(drivetrain); + } + @Override + public void execute() { + // double kp = SmartDashboard.getNumber("apriltag align kp", + // rotationPID.getP()); + // double ki = SmartDashboard.getNumber("apriltag align ki", + // rotationPID.getI()); + // double kd = SmartDashboard.getNumber("apriltag align kd", + // rotationPID.getD()); + // if (kp != rotationPID.getP()) + // rotationPID.setP(kp); + // if (ki != rotationPID.getI()) + // rotationPID.setI(ki); + // if (kd != rotationPID.getD()) + // rotationPID.setD(kd); + // double posTolerance = SmartDashboard.getNumber( + // "apriltag align pos tolerance", + // rotationPID.getPositionTolerance()); + // double velTolerance = SmartDashboard.getNumber( + // "apriltag align vel tolerance", + // rotationPID.getVelocityTolerance()); + // if (posTolerance != rotationPID.getPositionTolerance() + // || velTolerance != rotationPID + // .getVelocityTolerance()) + // rotationPID.setTolerance(posTolerance, velTolerance); + // SmartDashboard.putNumber("apriltag align pos error (rad)", + // rotationPID.getPositionError()); + // SmartDashboard.putNumber("apriltag align vel error (rad/s)", + // rotationPID.getVelocityError()); + + Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading()).plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2("limelight-reef"))); + rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180)); + double rotationDrive = rotationPID.calculate(drivetrain.getHeading()); + if (!limelight.seesTag(REEF_LL)) rotationDrive = RotationSpeed; + if (rotationPID.atSetpoint()) rotationDrive = 0; + if (teleopDrive == null) drivetrain.drive(0, 0, rotationDrive); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], rotationDrive); + } + } + + @Override + public boolean isFinished() { + // return false; + // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint() || (Timer.getFPGATimestamp() - startTime) > 3; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index d896759..abd5d4a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -2,6 +2,9 @@ package org.carlmontrobotics.subsystems; import static org.carlmontrobotics.Constants.Limelightc.*; + +import org.carlmontrobotics.Constants.Limelightc.Apriltagc; + import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -11,36 +14,37 @@ public class Limelight extends SubsystemBase { private final Drivetrain drivetrain; - // NEEDS TO SEE: Barge, Reef, Processor, Coral Dropoff public Limelight(Drivetrain drivetrain) { this.drivetrain = drivetrain; - LimelightHelpers.SetFiducialIDFiltersOverride(ROBOT_LL_NAME, VALID_IDS); + LimelightHelpers.SetFiducialIDFiltersOverride(CORAL_LL, CORAL_VALID_IDS); + LimelightHelpers.SetFiducialIDFiltersOverride(REEF_LL, REEF_VALID_IDS); - SmartDashboard.putBoolean("Robot sees a Tag", seesTag()); // I need to improve this... - SmartDashboard.putNumber("Distance to Processor", getDistanceToApriltag(ROBOT_LL_NAME, MOUNT_ANGLE, 0, LENS_HEIGHT_FROM_GROUND_METERS)); + SmartDashboard.putBoolean("sees coral station tag", seesTag(CORAL_LL)); + SmartDashboard.putBoolean("sees reef tag", seesTag(REEF_LL)); + SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); + SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); } + @Override public void periodic() { - SmartDashboard.putBoolean("Robot sees a Tag", seesTag()); - SmartDashboard.putNumber("Distance to Processor", getDistanceToApriltag(ROBOT_LL_NAME, MOUNT_ANGLE, 0, LENS_HEIGHT_FROM_GROUND_METERS)); - } - // Very helpful accessors - public double getTXDeg() { - return LimelightHelpers.getTX(ROBOT_LL_NAME); + SmartDashboard.putBoolean("sees coral station tag", seesTag(CORAL_LL)); + SmartDashboard.putBoolean("sees reef tag", seesTag(REEF_LL)); + SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); + SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); } - public double getTYDeg() { - return LimelightHelpers.getTY(ROBOT_LL_NAME); + public double getTYDeg(String limelightName) { + return LimelightHelpers.getTY(limelightName); } // Distance accessors for field areas public double getDistanceToApriltag(String limelightName, double mountAngle, double tagHeight, double limelightHeight) { - if (!seesTag()) { + if (!seesTag(limelightName)) { return -1; } else { - Rotation2d angleToGoal = Rotation2d.fromDegrees(mountAngle).plus(Rotation2d.fromDegrees(getTYDeg())); + Rotation2d angleToGoal = Rotation2d.fromDegrees(mountAngle).plus(Rotation2d.fromDegrees(getTYDeg(limelightName))); double distance = (tagHeight - limelightHeight) / angleToGoal.getTan(); return distance; } @@ -62,7 +66,8 @@ public double getRotateAngleRadMT2(String limelightName) { return targetOffsetRads; } - public boolean seesTag() { - return LimelightHelpers.getTV(ROBOT_LL_NAME); + + public boolean seesTag(String limelightName) { + return LimelightHelpers.getTV(limelightName); } } \ No newline at end of file From 8497db9efca8e28dbdc2a4e3a6ead2322a01f30f Mon Sep 17 00:00:00 2001 From: stwiggy Date: Fri, 7 Mar 2025 20:15:47 -0800 Subject: [PATCH 36/39] added strafing to the left/right, check math when not sleepy --- .../java/org/carlmontrobotics/Constants.java | 10 +++- .../commands/MoveToLeftBranch.java | 56 +++++++++++++++++++ .../commands/MoveToRightBranch.java | 56 +++++++++++++++++++ .../subsystems/Limelight.java | 12 +++- 4 files changed, 129 insertions(+), 5 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java create mode 100644 src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index bad8168..d47088a 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -235,9 +235,13 @@ public static final class Limelightc { public static final int[] CORAL_VALID_IDS = {1, 2, 12, 13}; public static final int[] REEF_VALID_IDS = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; public static final double CORAL_MOUNT_ANGLE = 25; // pitch - public static final double REEF_MOUNT_ANGLE = 15; // pitch TODO: change - public static final double CORAL_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: change - public static final double REEF_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: change + public static final double REEF_MOUNT_ANGLE = 15; // pitch + public static final double CORAL_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: ask brandon + public static final double REEF_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: ask brandon + // TODO: CHANGE NUMBERS ON LIMELIGHT INTERFACE + + public static final double LEFT_TO_CORAL_BRANCH = Units.inchesToMeters(-6.593); + public static final double RIGHT_TO_CORAL_BRANCH = Units.inchesToMeters(6.345); public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java b/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java new file mode 100644 index 0000000..ce96ceb --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; +import org.carlmontrobotics.subsystems.LimelightHelpers; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import static org.carlmontrobotics.Constants.Limelightc.*; + +public class MoveToLeftBranch extends Command { + private final Drivetrain dt; + private final Limelight ll; + private boolean originalFieldOrientation; + + /** Creates a new MoveToLeftBranch. */ + public MoveToLeftBranch(Drivetrain dt, Limelight ll) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.dt=dt); + this.ll = ll; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + originalFieldOrientation = dt.getFieldOriented(); + dt.setFieldOriented(false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (ll.seesTag(CORAL_LL)) { //TODO: test with getdistancetoapriltag vs getdistancetoapriltagmt2 + double strafeErr = Math.sin(Units.degreesToRadians(LimelightHelpers.getTX(CORAL_LL))) * ll.getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, REEF_LL_HEIGHT_FROM_GROUND_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS); + dt.drive(0, strafeErr, 0); + } + else { + dt.drive(0, LEFT_TO_CORAL_BRANCH,0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + dt.setFieldOriented(originalFieldOrientation); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java b/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java new file mode 100644 index 0000000..43ab2d4 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java @@ -0,0 +1,56 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.Drivetrain; +import org.carlmontrobotics.subsystems.Limelight; +import org.carlmontrobotics.subsystems.LimelightHelpers; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj2.command.Command; +import static org.carlmontrobotics.Constants.Limelightc.*; + +public class MoveToRightBranch extends Command { + private final Drivetrain dt; + private final Limelight ll; + private boolean originalFieldOrientation; + + /** Creates a new MoveToLeftBranch. */ + public MoveToRightBranch(Drivetrain dt, Limelight ll) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.dt=dt); + this.ll = ll; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + originalFieldOrientation = dt.getFieldOriented(); + dt.setFieldOriented(false); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (ll.seesTag(CORAL_LL)) { //TODO: test with getdistancetoapriltag vs getdistancetoapriltagmt2 + double strafeErr = Math.sin(Units.degreesToRadians(LimelightHelpers.getTX(CORAL_LL))) * ll.getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, REEF_LL_HEIGHT_FROM_GROUND_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS); + dt.drive(0, strafeErr, 0); + } + else { + dt.drive(0, RIGHT_TO_CORAL_BRANCH,0); + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + dt.setFieldOriented(originalFieldOrientation); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index abd5d4a..977a8bd 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -32,6 +32,7 @@ public void periodic() { SmartDashboard.putBoolean("sees reef tag", seesTag(REEF_LL)); SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); + SmartDashboard.putNumber("distance to reef megatag2", getDistanceToApriltagMT2()); } public double getTYDeg(String limelightName) { @@ -50,10 +51,17 @@ public double getDistanceToApriltag(String limelightName, double mountAngle, dou } } - public double getApriltagID(String limelightName) { - return LimelightHelpers.getFiducialID(limelightName); + public double getDistanceToApriltagMT2() { + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(CORAL_LL); + + double x = targetPoseRobotSpace.getX(); + double z = targetPoseRobotSpace.getZ(); + + return Math.hypot(x, z); } + //TODO: TEST WHICH ONE IS MORE ACCURATE + public double getRotateAngleRadMT2(String limelightName) { Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(limelightName); // pose of the target From 2381c844659c9b2392ebd05ef523cbdba154a754 Mon Sep 17 00:00:00 2001 From: stwiggy Date: Mon, 10 Mar 2025 17:30:53 -0700 Subject: [PATCH 37/39] timer --- .../org/carlmontrobotics/commands/AlignToCoralStation.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java b/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java index 185bdc5..5c775d2 100644 --- a/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java +++ b/src/main/java/org/carlmontrobotics/commands/AlignToCoralStation.java @@ -35,7 +35,6 @@ public AlignToCoralStation(Drivetrain drivetrain, Limelight limelight, double Ro SendableRegistry.addChild(this, rotationPID); time.reset(); time.start(); - startTime = Timer.getFPGATimestamp(); addRequirements(drivetrain); } @@ -85,6 +84,6 @@ public boolean isFinished() { // return false; // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); // SmartDashboard.putNumber("Error", rotationPID.getPositionError()); - return rotationPID.atSetpoint() || (Timer.getFPGATimestamp() - startTime) > 3; + return rotationPID.atSetpoint() || time.get() > 3; } } \ No newline at end of file From 5537b2ee11e1d958062fb8bc4c40fc1b901129ad Mon Sep 17 00:00:00 2001 From: stwiggy Date: Wed, 12 Mar 2025 19:39:17 -0700 Subject: [PATCH 38/39] pretest commits --- .../java/org/carlmontrobotics/Constants.java | 14 +++++------ .../org/carlmontrobotics/RobotContainer.java | 8 ++++++- .../commands/MoveToLeftBranch.java | 7 ++++-- .../commands/MoveToRightBranch.java | 7 ++++-- .../subsystems/Limelight.java | 24 ++++++++++++------- 5 files changed, 39 insertions(+), 21 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index d47088a..e85c102 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -233,15 +233,15 @@ public static final class Limelightc { public static final String REEF_LL = "limelight-reef"; public static final int[] CORAL_VALID_IDS = {1, 2, 12, 13}; - public static final int[] REEF_VALID_IDS = {6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; - public static final double CORAL_MOUNT_ANGLE = 25; // pitch - public static final double REEF_MOUNT_ANGLE = 15; // pitch - public static final double CORAL_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: ask brandon - public static final double REEF_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: ask brandon + public static final int[] REEF_VALID_IDS = {1, 6, 7, 8, 9, 10, 11, 17, 18, 19, 20, 21, 22}; // 1 is only for testing reef + // public static final double CORAL_MOUNT_ANGLE = -25; // pitch + // public static final double REEF_MOUNT_ANGLE = 15; // pitch + // public static final double CORAL_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; + // public static final double REEF_LL_HEIGHT_FROM_GROUND_METERS = 0.206502; // TODO: CHANGE NUMBERS ON LIMELIGHT INTERFACE - public static final double LEFT_TO_CORAL_BRANCH = Units.inchesToMeters(-6.593); - public static final double RIGHT_TO_CORAL_BRANCH = Units.inchesToMeters(6.345); + public static final double LEFT_CORAL_BRANCH = Units.inchesToMeters(-6.593); + public static final double RIGHT_CORAL_BRANCH = Units.inchesToMeters(6.345); public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index f1ee3ec..9e654b8 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -56,6 +56,7 @@ public class RobotContainer { public final GenericHID manipulatorController = new GenericHID(Manipulator.port); private final Drivetrain drivetrain = new Drivetrain(); + private final Limelight limelight = new Limelight(drivetrain); /* These are assumed to be equal to the AUTO ames in pathplanner */ /* These must be equal to the pathPlanner path names from the GUI! */ @@ -113,7 +114,12 @@ private void setDefaultCommands() { () -> ProcessedAxisValue(driverController, Axis.kRightX), () -> driverController.getRawButton(OI.Driver.slowDriveButton))); } - private void setBindingsDriver() {} + private void setBindingsDriver() { + new JoystickButton(driverController, 1) + .whileTrue(new MoveToLeftBranch(drivetrain, limelight)); + new JoystickButton(driverController, 2) + .whileTrue(new MoveToRightBranch(drivetrain, limelight)); + } private void setBindingsManipulator() {} /** diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java b/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java index ce96ceb..a624891 100644 --- a/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java +++ b/src/main/java/org/carlmontrobotics/commands/MoveToLeftBranch.java @@ -8,6 +8,7 @@ import org.carlmontrobotics.subsystems.Limelight; import org.carlmontrobotics.subsystems.LimelightHelpers; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import static org.carlmontrobotics.Constants.Limelightc.*; @@ -15,6 +16,7 @@ public class MoveToLeftBranch extends Command { private final Drivetrain dt; private final Limelight ll; private boolean originalFieldOrientation; + double strafeErr; /** Creates a new MoveToLeftBranch. */ public MoveToLeftBranch(Drivetrain dt, Limelight ll) { @@ -33,12 +35,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + SmartDashboard.putNumber("strafe left", strafeErr); if (ll.seesTag(CORAL_LL)) { //TODO: test with getdistancetoapriltag vs getdistancetoapriltagmt2 - double strafeErr = Math.sin(Units.degreesToRadians(LimelightHelpers.getTX(CORAL_LL))) * ll.getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, REEF_LL_HEIGHT_FROM_GROUND_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS); + strafeErr = Math.sin(Units.degreesToRadians(LimelightHelpers.getTX(REEF_LL))) * ll.getDistanceToApriltagMT2(REEF_LL); dt.drive(0, strafeErr, 0); } else { - dt.drive(0, LEFT_TO_CORAL_BRANCH,0); + dt.drive(0, LEFT_CORAL_BRANCH,0); } } diff --git a/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java b/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java index 43ab2d4..72342dc 100644 --- a/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java +++ b/src/main/java/org/carlmontrobotics/commands/MoveToRightBranch.java @@ -8,6 +8,7 @@ import org.carlmontrobotics.subsystems.Limelight; import org.carlmontrobotics.subsystems.LimelightHelpers; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import static org.carlmontrobotics.Constants.Limelightc.*; @@ -15,6 +16,7 @@ public class MoveToRightBranch extends Command { private final Drivetrain dt; private final Limelight ll; private boolean originalFieldOrientation; + double strafeErr; /** Creates a new MoveToLeftBranch. */ public MoveToRightBranch(Drivetrain dt, Limelight ll) { @@ -33,12 +35,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + SmartDashboard.putNumber("strafe left", strafeErr); if (ll.seesTag(CORAL_LL)) { //TODO: test with getdistancetoapriltag vs getdistancetoapriltagmt2 - double strafeErr = Math.sin(Units.degreesToRadians(LimelightHelpers.getTX(CORAL_LL))) * ll.getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, REEF_LL_HEIGHT_FROM_GROUND_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS); + strafeErr = Math.sin(Units.degreesToRadians(LimelightHelpers.getTX(REEF_LL))) * ll.getDistanceToApriltagMT2(REEF_LL); dt.drive(0, strafeErr, 0); } else { - dt.drive(0, RIGHT_TO_CORAL_BRANCH,0); + dt.drive(0, RIGHT_CORAL_BRANCH,0); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java index 977a8bd..65f0a61 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Limelight.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Limelight.java @@ -20,19 +20,25 @@ public Limelight(Drivetrain drivetrain) { LimelightHelpers.SetFiducialIDFiltersOverride(CORAL_LL, CORAL_VALID_IDS); LimelightHelpers.SetFiducialIDFiltersOverride(REEF_LL, REEF_VALID_IDS); - SmartDashboard.putBoolean("sees coral station tag", seesTag(CORAL_LL)); + // SmartDashboard.putBoolean("sees coral station tag", seesTag(CORAL_LL)); SmartDashboard.putBoolean("sees reef tag", seesTag(REEF_LL)); - SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); - SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); + // SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); + // SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); + SmartDashboard.putNumber("distance to reef mt2", getDistanceToApriltagMT2(REEF_LL)); + + SmartDashboard.putNumber("strafe left", 0); + SmartDashboard.putNumber("strafe right", 0); + } @Override public void periodic() { - SmartDashboard.putBoolean("sees coral station tag", seesTag(CORAL_LL)); + // SmartDashboard.putBoolean("sees coral station tag", seesTag(CORAL_LL)); SmartDashboard.putBoolean("sees reef tag", seesTag(REEF_LL)); - SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); - SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); - SmartDashboard.putNumber("distance to reef megatag2", getDistanceToApriltagMT2()); + // SmartDashboard.putNumber("distance to coral", getDistanceToApriltag(CORAL_LL, CORAL_MOUNT_ANGLE, Apriltagc.CORAL_HEIGHT_METERS, CORAL_LL_HEIGHT_FROM_GROUND_METERS)); + // SmartDashboard.putNumber("distance to reef", getDistanceToApriltag(REEF_LL, REEF_MOUNT_ANGLE, Apriltagc.REEF_HEIGHT_METERS, REEF_LL_HEIGHT_FROM_GROUND_METERS)); + SmartDashboard.putNumber("distance to reef megatag2", getDistanceToApriltagMT2(REEF_LL)); + } public double getTYDeg(String limelightName) { @@ -51,8 +57,8 @@ public double getDistanceToApriltag(String limelightName, double mountAngle, dou } } - public double getDistanceToApriltagMT2() { - Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(CORAL_LL); + public double getDistanceToApriltagMT2(String limelightName) { + Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(limelightName); double x = targetPoseRobotSpace.getX(); double z = targetPoseRobotSpace.getZ(); From 0856d9c3032840b004e212b88cad46c3b14c810a Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 25 Mar 2025 17:16:41 -0700 Subject: [PATCH 39/39] Some adhit code --- src/main/java/org/carlmontrobotics/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 9e654b8..dbfebb7 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -119,6 +119,8 @@ private void setBindingsDriver() { .whileTrue(new MoveToLeftBranch(drivetrain, limelight)); new JoystickButton(driverController, 2) .whileTrue(new MoveToRightBranch(drivetrain, limelight)); + new JoystickButton(driverController, 3) + .whileTrue(new AlignToCoralStation(drivetrain, limelight, 2*Math.PI)); } private void setBindingsManipulator() {}