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/52] 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/52] 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/52] //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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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/52] 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 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 25/52] 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 26/52] 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 ed14b0bc303590b236970bf41ddae9cd4c52caa9 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 11 Feb 2025 16:54:08 -0800 Subject: [PATCH 27/52] Switched around turn and drive ports --- .../java/org/carlmontrobotics/Constants.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b3029e5..d256b57 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -159,15 +159,15 @@ public static final class Drivetrainc { // #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; + public static final int driveFrontLeftPort = 11; + public static final int driveFrontRightPort = 12; + public static final int driveBackLeftPort = 13; + public static final int driveBackRightPort = 14; + + public static final int turnFrontLeftPort = 1; + public static final int turnFrontRightPort = 2; + public static final int turnBackLeftPort = 3; + public static final int turnBackRightPort = 4; //to be configured public static final int canCoderPortFL = 0; public static final int canCoderPortFR = 3; From adb181bb1af98988798aa87c737784657da38d14 Mon Sep 17 00:00:00 2001 From: TuskAct2 <112046931+TuskAct2@users.noreply.github.com> Date: Tue, 11 Feb 2025 18:45:04 -0800 Subject: [PATCH 28/52] fixed ports --- .../subsystems/Drivetrain.java | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 9d5d354..af732fc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -146,7 +146,10 @@ public class Drivetrain extends SubsystemBase { double kI = 0; double kD = 0; public Drivetrain() { - + SmartDashboard.putNumber("Goal Velocity", 0); + SmartDashboard.putNumber("kP", 0); + SmartDashboard.putNumber("kI", 0); + SmartDashboard.putNumber("kD", 0); // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", @@ -154,7 +157,7 @@ public Drivetrain() { // SmartDashboard.putNumber("pose estimator std dev x", STD_DEV_X_METERS); // SmartDashboard.putNumber("pose estimator std dev y", STD_DEV_Y_METERS); - + SmartDashboard.putNumber("GoalPos", 0); // Calibrate Gyro { @@ -203,23 +206,23 @@ public Drivetrain() { moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = new SparkMax(0, MotorType.kBrushless), - turnMotors[0] = new SparkMax(1, MotorType.kBrushless), + driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), + turnMotors[0] = new SparkMax(turnFrontLeftPort, 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), + driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), + turnMotors[1] = new SparkMax(turnFrontRightPort, 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), + driveMotors[2] = new SparkMax(driveBackLeftPort, MotorType.kBrushless), + turnMotors[2] = new SparkMax(turnBackLeftPort, 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), + driveMotors[3] = new SparkMax(driveBackRightPort, MotorType.kBrushless), + turnMotors[3] = new SparkMax(turnBackRightPort, MotorType.kBrushless), turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; turnPidControllers[0] = turnMotors[0].getClosedLoopController(); @@ -334,13 +337,18 @@ public void simulationPeriodic() { @Override public void periodic() { - SmartDashboard.getNumber("Velocity FL: ", turnEncoders[0].getVelocity().getValueAsDouble()); - double velocity = SmartDashboard.getNumber("Goal Velocity", 0); + SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); + double velocity = SmartDashboard.getNumber("GoalPos", 0); kP = SmartDashboard.getNumber("kP", 0); kI = SmartDashboard.getNumber("kI", 0); kD = SmartDashboard.getNumber("kD", 0); - turnPidControllers[0].setReference(velocity, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + + SparkMaxConfig config = new SparkMaxConfig(); + config.closedLoop.pidf(kP,kI,kD,10); + + turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + turnPidControllers[0].setReference(velocity, ControlType.kPosition, ClosedLoopSlot.kSlot0); // for (CANcoder coder : turnEncoders) { // SignalLogger.writeDouble("Regular position " + coder.toString(), @@ -400,10 +408,10 @@ public void periodic() { // 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()); + 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 From cf2534d8fe943a9e079c5c16f61f9c51dfc814f6 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 14 Feb 2025 21:39:26 -0800 Subject: [PATCH 29/52] lots of pid testing and stuffs that turn an angle just not the goal angle. (The angles it turns to instead are pretty consistant which means there may be an offset somewhere that can fix it) --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- .../carlmontrobotics/subsystems/Drivetrain.java | 15 +++++++++------ 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index d256b57..7adbf61 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -169,10 +169,10 @@ public static final class Drivetrainc { public static final int turnBackLeftPort = 3; public static final int turnBackRightPort = 4; //to be configured - public static final int canCoderPortFL = 0; + public static final int canCoderPortFL = 1; //0 public static final int canCoderPortFR = 3; public static final int canCoderPortBL = 2; - public static final int canCoderPortBR = 1; + public static final int canCoderPortBR = 0; //1 // #endregion diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index af732fc..053cf04 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -345,11 +345,14 @@ public void periodic() { kD = SmartDashboard.getNumber("kD", 0); SparkMaxConfig config = new SparkMaxConfig(); - config.closedLoop.pidf(kP,kI,kD,10); - - turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - turnPidControllers[0].setReference(velocity, ControlType.kPosition, ClosedLoopSlot.kSlot0); - + config.closedLoop.pid(0.05 + ,0.0001,0.6); + config.encoder.positionConversionFactor(360*Constants.Drivetrainc.turnGearing); + turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + turnPidControllers[0].setReference(360*5000/239 + , ControlType.kPosition, ClosedLoopSlot.kSlot0); + // 167 -> -200 + // 138 -> 360 // for (CANcoder coder : turnEncoders) { // SignalLogger.writeDouble("Regular position " + coder.toString(), // coder.getPosition().getValue()); @@ -366,7 +369,7 @@ public void periodic() { // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - module.periodic(); + //module.periodic(); // module.move(0, goal); } From acc52a1870ab04843df0db983ce8c5977da538a2 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 14 Feb 2025 22:03:02 -0800 Subject: [PATCH 30/52] changed multiplying turngearing to dividing (this is the correct operation) --- 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 053cf04..4adcc7c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -347,7 +347,7 @@ public void periodic() { SparkMaxConfig config = new SparkMaxConfig(); config.closedLoop.pid(0.05 ,0.0001,0.6); - config.encoder.positionConversionFactor(360*Constants.Drivetrainc.turnGearing); + config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); turnPidControllers[0].setReference(360*5000/239 , ControlType.kPosition, ClosedLoopSlot.kSlot0); From 42110be387c35e20c9c326314f77ea9c08cf7a00 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Sat, 15 Feb 2025 15:20:34 -0800 Subject: [PATCH 31/52] changed turn ff constants in constants.java to 0 in case we end up testing with module.move --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 3 +++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7adbf61..9b43094 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -105,8 +105,8 @@ public static final class Drivetrainc { // 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 }; + public static final double[] turnkV = { 0, 0, 0, 0 }; + public static final double[] turnkA = { 0, 0, 0, 0 }; // kP is an average of the forward and backward kP values // Forward: 1.72, 1.71, 1.92, 1.94 diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 4adcc7c..d40ad36 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -351,6 +351,9 @@ public void periodic() { turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); turnPidControllers[0].setReference(360*5000/239 , ControlType.kPosition, ClosedLoopSlot.kSlot0); + moduleFL.move(0, 90); + + // 167 -> -200 // 138 -> 360 // for (CANcoder coder : turnEncoders) { From 3b9be0159d85e9bf647c98cef84389e2a9a9988c Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 15 Feb 2025 16:15:25 -0800 Subject: [PATCH 32/52] pid no workee I call RYAAAAAAN --- .../carlmontrobotics/subsystems/Drivetrain.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 4adcc7c..0c3e18d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -45,6 +45,7 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.VecBuilder; @@ -150,6 +151,7 @@ public Drivetrain() { SmartDashboard.putNumber("kP", 0); SmartDashboard.putNumber("kI", 0); SmartDashboard.putNumber("kD", 0); + // SmartDashboard.putNumber("Pose Estimator t x (m)", lastSetX); // SmartDashboard.putNumber("Pose Estimator set y (m)", lastSetY); // SmartDashboard.putNumber("Pose Estimator set rotation (deg)", @@ -339,17 +341,17 @@ public void simulationPeriodic() { public void periodic() { SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); double velocity = SmartDashboard.getNumber("GoalPos", 0); - + PIDController pid = new PIDController(kP, kI, kD); kP = SmartDashboard.getNumber("kP", 0); kI = SmartDashboard.getNumber("kI", 0); kD = SmartDashboard.getNumber("kD", 0); - + pid.setIZone(50); SparkMaxConfig config = new SparkMaxConfig(); - config.closedLoop.pid(0.05 - ,0.0001,0.6); + config.closedLoop.pid(51 + ,0.0001,0); config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - turnPidControllers[0].setReference(360*5000/239 + turnPidControllers[0].setReference(0 , ControlType.kPosition, ClosedLoopSlot.kSlot0); // 167 -> -200 // 138 -> 360 @@ -520,8 +522,8 @@ public void drive(SwerveModuleState[] moduleStates) { // this // :D // ); - /* - AutoBuilder.configureHolonomic( + + /* 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 @@ -552,6 +554,7 @@ public void drive(SwerveModuleState[] moduleStates) { this ); */ + // } public void autoCancelDtCommand() { From 106765983b4f3231a53dff1883c8fe163f4ecf9a Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 15 Feb 2025 17:56:21 -0800 Subject: [PATCH 33/52] pid has been restored back to normal operations --- .../carlmontrobotics/subsystems/Drivetrain.java | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 47905e5..a20d8ef 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -43,6 +43,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import edu.wpi.first.math.controller.PIDController; @@ -211,7 +212,7 @@ public Drivetrain() { driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), turnMotors[0] = new SparkMax(turnFrontLeftPort, MotorType.kBrushless), turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); - + SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), turnMotors[1] = new SparkMax(turnFrontRightPort, MotorType.kBrushless), @@ -340,20 +341,21 @@ public void simulationPeriodic() { @Override public void periodic() { SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); + SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); double velocity = SmartDashboard.getNumber("GoalPos", 0); PIDController pid = new PIDController(kP, kI, kD); kP = SmartDashboard.getNumber("kP", 0); kI = SmartDashboard.getNumber("kI", 0); kD = SmartDashboard.getNumber("kD", 0); - pid.setIZone(50); + pid.setIZone(0.1); SparkMaxConfig config = new SparkMaxConfig(); - config.closedLoop.pid(51 - ,0.0001,0); - config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); + //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); + config.closedLoop.pid(1 + ,0,0.4); + //config.encoder.positionConversionFactor(Constants.Drivetrainc.turnGearing); turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - turnPidControllers[0].setReference(0 + turnPidControllers[0].setReference(180*(150/7) , ControlType.kPosition, ClosedLoopSlot.kSlot0); - moduleFL.move(0, 90); // 167 -> -200 From d1feaf686b38c70aa060fce8cabc91ec6c5a83b3 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 16 Feb 2025 11:49:10 -0800 Subject: [PATCH 34/52] pushing so we can test on Kenneth's laptop --- src/main/java/org/carlmontrobotics/Constants.java | 6 +++--- .../org/carlmontrobotics/subsystems/Drivetrain.java | 13 ++++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 9b43094..23344fd 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -57,7 +57,7 @@ public static final class Drivetrainc { // 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 + // Gearing for the Swerve Modules is 6.75 : 1e public static final double driveGearing = 6.75; // Turn motor shaft to "module shaft" public static final double turnGearing = 150 / 7; @@ -95,10 +95,10 @@ 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 = { 0, 0, 0, 0 }; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = { 0.2, 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, 0, 0}; // todo: use d + public static final double[] turnkD = { 0.2/* 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, 0, 0, 0 }; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index a20d8ef..0401400 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -347,14 +347,17 @@ public void periodic() { kP = SmartDashboard.getNumber("kP", 0); kI = SmartDashboard.getNumber("kI", 0); kD = SmartDashboard.getNumber("kD", 0); - pid.setIZone(0.1); + pid.setIZone(20); SparkMaxConfig config = new SparkMaxConfig(); //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); - config.closedLoop.pid(1 - ,0,0.4); - //config.encoder.positionConversionFactor(Constants.Drivetrainc.turnGearing); + config.closedLoop.pid(0.2 + ,0,0.2); + config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - turnPidControllers[0].setReference(180*(150/7) + //moduleFL.move(0.0000001, 180); + //moduleFL.move(0.01, 180); + turnPidControllers[0].setReference(320 + , ControlType.kPosition, ClosedLoopSlot.kSlot0); From c2d0a0d6a5472589fec37ad06781f4cd33e56864 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Sun, 16 Feb 2025 12:06:01 -0800 Subject: [PATCH 35/52] trying module.move --- .vscode/settings.json | 3 ++ networktables.json | 1 + simgui-ds.json | 92 +++++++++++++++++++++++++++++++++++++++++++ simgui.json | 12 ++++++ 4 files changed, 108 insertions(+) create mode 100644 .vscode/settings.json create mode 100644 networktables.json create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..0e14d8e --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "disabled" +} \ No newline at end of file diff --git a/networktables.json b/networktables.json new file mode 100644 index 0000000..fe51488 --- /dev/null +++ b/networktables.json @@ -0,0 +1 @@ +[] diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..73cc713 --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..8be9db9 --- /dev/null +++ b/simgui.json @@ -0,0 +1,12 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/SmartDashboard/Field": "Field2d", + "/SmartDashboard/SendableChooser[1]": "String Chooser" + } + }, + "NetworkTables Info": { + "visible": true + } +} From 7d790d307708e97afef5450308a09ab73ba39c39 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Sun, 16 Feb 2025 16:23:39 -0800 Subject: [PATCH 36/52] turned pid using module.move and found turn zeros --- .../java/org/carlmontrobotics/Constants.java | 30 ++++++----- .../org/carlmontrobotics/RobotContainer.java | 1 + .../subsystems/Drivetrain.java | 52 +++++++++++-------- 3 files changed, 48 insertions(+), 35 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 23344fd..77002a9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -60,7 +60,7 @@ public static final class Drivetrainc { // Gearing for the Swerve Modules is 6.75 : 1e public static final double driveGearing = 6.75; // Turn motor shaft to "module shaft" - public static final double turnGearing = 150 / 7; + public static final double turnGearing = 150.0 / 7; public static final double driveModifier = 1; public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* @@ -90,23 +90,23 @@ public static final class Drivetrainc { // 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 */ + : new double[] { 65.8301, -160.9277, 170.5078, 91.1426 });/* 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 = { 0.2, 0, 0, 0 }; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = { 51.078, 25, 35.946, 30.986 }; // {0.00374, 0.00374, 0.00374, // 0.00374}; - public static final double[] turnkI = { 0, 0, 0, 0 }; - public static final double[] turnkD = { 0.2/* dont edit */, 0, 0, 0}; // todo: use d + public static final double[] turnkI = { 0, 0.1, 0, 0 }; + public static final double[] turnkD = { 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0, 0, 0, 0 }; + 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 = { 0, 0, 0, 0 }; - public static final double[] turnkA = { 0, 0, 0, 0 }; + 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 @@ -122,12 +122,16 @@ public static final class Drivetrainc { : 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[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; + public static final double[] kForwardVolts = { 0, 0, 0, 0 }; public static final double[] kBackwardVolts = kForwardVolts; - public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; + public static final double[] kForwardVels = { 0, 0, 0, 0 };//volts per m/s 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[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; + public static final double[] kForwardAccels = { 0, 0, 0, 0 };// volts per m/s^2 public static final double[] kBackwardAccels = kForwardAccels; public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second @@ -170,8 +174,8 @@ public static final class Drivetrainc { public static final int turnBackRightPort = 4; //to be configured public static final int canCoderPortFL = 1; //0 - public static final int canCoderPortFR = 3; - public static final int canCoderPortBL = 2; + public static final int canCoderPortFR = 2; + public static final int canCoderPortBL = 3; public static final int canCoderPortBR = 0; //1 // #endregion diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index f1ee3ec..7520587 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -75,6 +75,7 @@ public RobotContainer() { { // Put any configuration overrides to the dashboard and the terminal SmartDashboard.putData("CONFIG overrides", Config.CONFIG); + SmartDashboard.putData(drivetrain); System.out.println(Config.CONFIG); SmartDashboard.putData("BuildConstants", BuildInfo.getInstance()); diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 0401400..2ad51d1 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -238,7 +238,7 @@ public Drivetrain() { }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } - + SmartDashboard.putData("Module FL",moduleFL); SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig.openLoopRampRate(secsPer12Volts); driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); @@ -252,12 +252,13 @@ public Drivetrain() { } SparkMaxConfig turnConfig = new SparkMaxConfig(); turnConfig.encoder.positionConversionFactor(360/turnGearing); - turnConfig.encoder.positionConversionFactor(360/turnGearing/60); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); turnConfig.encoder.uvwAverageDepth(2); turnConfig.encoder.uvwMeasurementPeriod(16); + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); for (SparkMax turnMotor : turnMotors) { - turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } for (CANcoder coder : turnEncoders) { @@ -266,7 +267,7 @@ public Drivetrain() { coder.getVelocity().setUpdateFrequency(500); } - turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + SmartDashboard.putData("Field", field); // for(SparkMax driveMotor : driveMotors) @@ -340,25 +341,32 @@ public void simulationPeriodic() { @Override public void periodic() { - SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); - SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); - double velocity = SmartDashboard.getNumber("GoalPos", 0); - PIDController pid = new PIDController(kP, kI, kD); - kP = SmartDashboard.getNumber("kP", 0); - kI = SmartDashboard.getNumber("kI", 0); - kD = SmartDashboard.getNumber("kD", 0); - pid.setIZone(20); - SparkMaxConfig config = new SparkMaxConfig(); + // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); + // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + // double goal = SmartDashboard.getNumber("GoalPos", 0); + // PIDController pid = new PIDController(kP, kI, kD); + // kP = SmartDashboard.getNumber("kP", 0); + // kI = SmartDashboard.getNumber("kI", 0); + // kD = SmartDashboard.getNumber("kD", 0); + //pid.setIZone(20); + //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); + // SparkMaxConfig config = new SparkMaxConfig(); + //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); - config.closedLoop.pid(0.2 - ,0,0.2); - config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); - turnMotors[0].configure(config, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); - //moduleFL.move(0.0000001, 180); + // System.out.println(kP); + // config.closedLoop.pid(kP + // ,kI,kD); + // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); + // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + // //moduleFL.move(0.0000001, 180); //moduleFL.move(0.01, 180); - turnPidControllers[0].setReference(320 + moduleFR.move(0.00001, 180); + moduleBR.move(0.00001, 180); + moduleFL.move(0.00001, 180); + moduleBL.move(0.00001, 180); + // turnPidControllers[0].setReference(goal - , ControlType.kPosition, ClosedLoopSlot.kSlot0); + // , ControlType.kPosition, ClosedLoopSlot.kSlot0); // 167 -> -200 @@ -379,7 +387,7 @@ public void periodic() { // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - //module.periodic(); + module.periodic(); // module.move(0, goal); } @@ -433,7 +441,7 @@ public void initSendable(SendableBuilder builder) { for (SwerveModule module : modules) SendableRegistry.addChild(this, module); - + builder.addBooleanProperty("Magnetic Field Disturbance", gyro::isMagneticDisturbance, null); builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); From 2780b5cb840029349deecf024dcbedfcf3d2d7b9 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Sun, 16 Feb 2025 16:31:24 -0800 Subject: [PATCH 37/52] got real zeros this time from testing --- src/main/java/org/carlmontrobotics/Constants.java | 2 +- .../java/org/carlmontrobotics/subsystems/Drivetrain.java | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 77002a9..42ea485 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -90,7 +90,7 @@ public static final class Drivetrainc { // 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[] { 65.8301, -160.9277, 170.5078, 91.1426 });/* real values here */ + : new double[] { 20.8301, -105.9277, 260.5078, 91.1426 });/* real values here */ // kP, kI, and kD constants for turn motor controllers in the order of // front-left, front-right, back-left, back-right. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 2ad51d1..351335e 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -360,10 +360,10 @@ public void periodic() { // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); // //moduleFL.move(0.0000001, 180); //moduleFL.move(0.01, 180); - moduleFR.move(0.00001, 180); - moduleBR.move(0.00001, 180); - moduleFL.move(0.00001, 180); - moduleBL.move(0.00001, 180); + moduleFR.move(0.00001, 0); + moduleBR.move(0.00001, 0); + moduleFL.move(0.00001, 0); + moduleBL.move(0.00001, 0); // turnPidControllers[0].setReference(goal // , ControlType.kPosition, ClosedLoopSlot.kSlot0); From 4933849de9541662ec4d2d69db074304d2517bfb Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Sun, 16 Feb 2025 16:41:14 -0800 Subject: [PATCH 38/52] updated kS values and drive inversion --- src/main/java/org/carlmontrobotics/Constants.java | 6 +++--- .../java/org/carlmontrobotics/subsystems/Drivetrain.java | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 42ea485..263eb38 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -113,17 +113,17 @@ public static final class Drivetrainc { // 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, + : new double[] { 0, 0, 0, 0 }; // {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 }); + : new boolean[] { false, true, false, true }); 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[] kForwardVolts = { 0, 0, 0, 0 }; + public static final double[] kForwardVolts = { 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 351335e..a5b6bc3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -360,10 +360,10 @@ public void periodic() { // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); // //moduleFL.move(0.0000001, 180); //moduleFL.move(0.01, 180); - moduleFR.move(0.00001, 0); - moduleBR.move(0.00001, 0); - moduleFL.move(0.00001, 0); - moduleBL.move(0.00001, 0); + moduleFR.move(0.1, 0); + moduleBR.move(0.1, 0); + moduleFL.move(0.1, 0); + moduleBL.move(0.1, 0); // turnPidControllers[0].setReference(goal // , ControlType.kPosition, ClosedLoopSlot.kSlot0); From 19fdba6aa8f040c996521d2c9f4fce7d3a0dd9c2 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Tue, 18 Feb 2025 15:02:03 -0800 Subject: [PATCH 39/52] build.gradle fixed with propper capitlization on DeepBlueRobotics line 88 --- .DataLogTool/datalogtool.json | 1 + build.gradle | 3 +-- src/main/java/org/carlmontrobotics/Config.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) create mode 100644 .DataLogTool/datalogtool.json diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.DataLogTool/datalogtool.json @@ -0,0 +1 @@ +{} diff --git a/build.gradle b/build.gradle index 8d59a2e..db1ac0f 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,6 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2025.2.1" id "com.peterabeles.gversion" version "1.10" - } java { @@ -86,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.deepbluerobotics:lib199:2025-SNAPSHOT" + implementation "com.github.DeepBlueRobotics:lib199:2025-SNAPSHOT" } test { diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index 9a68135..699f34a 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -23,7 +23,7 @@ public abstract class Config implements Sendable { // Add additional config settings by declaring a protected field, and... protected boolean exampleFlagEnabled = false; protected boolean swimShady = false; - protected boolean setupSysId = false; + protected boolean setupSysId = true; protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of // outtake through SmartDashboard // Note: disables joystick control of arm and From fafa90b913852aaac9cf2cdf9a3288208fba8877 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 18 Feb 2025 21:42:01 -0800 Subject: [PATCH 40/52] doin some sys id tests and also corrected turn zeros --- .DataLogTool/datalogtool.json | 6 +++- .SysId/sysid.json | 1 + build.gradle | 2 +- .../java/org/carlmontrobotics/Constants.java | 2 +- .../subsystems/Drivetrain.java | 35 ++++++++++--------- 5 files changed, 27 insertions(+), 19 deletions(-) create mode 100644 .SysId/sysid.json diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json index 0967ef4..4c9c653 100644 --- a/.DataLogTool/datalogtool.json +++ b/.DataLogTool/datalogtool.json @@ -1 +1,5 @@ -{} +{ + "download": { + "serverTeam": "199" + } +} diff --git a/.SysId/sysid.json b/.SysId/sysid.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.SysId/sysid.json @@ -0,0 +1 @@ +{} diff --git a/build.gradle b/build.gradle index db1ac0f..f3b925f 100644 --- a/build.gradle +++ b/build.gradle @@ -85,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.DeepBlueRobotics:lib199:2025-SNAPSHOT" + implementation "com.github.DeepBlueRobotics:lib199:dbfca96f4f6795f3856e0adcdac4eafe20708dc6" } test { diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 263eb38..64f4bdc 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -90,7 +90,7 @@ public static final class Drivetrainc { // 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[] { 20.8301, -105.9277, 260.5078, 91.1426 });/* real values here */ + : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* real values here */ // kP, kI, and kD constants for turn motor controllers in the order of // front-left, front-right, back-left, back-right. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index a5b6bc3..7d1b87c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -209,24 +209,24 @@ public Drivetrain() { moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), - turnMotors[0] = new SparkMax(turnFrontLeftPort, MotorType.kBrushless), - turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), + turnMotors[0] = new SparkMax(turnFrontLeftPort, MotorType.kBrushless), + turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), - turnMotors[1] = new SparkMax(turnFrontRightPort, MotorType.kBrushless), - turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); + driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), + turnMotors[1] = new SparkMax(turnFrontRightPort, 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(driveBackLeftPort, MotorType.kBrushless), - turnMotors[2] = new SparkMax(turnBackLeftPort, MotorType.kBrushless), - turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); + driveMotors[2] = new SparkMax(driveBackLeftPort, MotorType.kBrushless), + turnMotors[2] = new SparkMax(turnBackLeftPort, 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(driveBackRightPort, MotorType.kBrushless), - turnMotors[3] = new SparkMax(turnBackRightPort, MotorType.kBrushless), - turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); + driveMotors[3] = new SparkMax(driveBackRightPort, MotorType.kBrushless), + turnMotors[3] = new SparkMax(turnBackRightPort, MotorType.kBrushless), + turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; turnPidControllers[0] = turnMotors[0].getClosedLoopController(); turnPidControllers[1] = turnMotors[1].getClosedLoopController(); @@ -239,6 +239,9 @@ public Drivetrain() { gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } SmartDashboard.putData("Module FL",moduleFL); + SmartDashboard.putData("Module FR",moduleFR); + SmartDashboard.putData("Module BL",moduleBL); + SmartDashboard.putData("Module BR",moduleBR); SparkMaxConfig driveConfig = new SparkMaxConfig(); driveConfig.openLoopRampRate(secsPer12Volts); driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); @@ -360,10 +363,10 @@ public void periodic() { // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); // //moduleFL.move(0.0000001, 180); //moduleFL.move(0.01, 180); - moduleFR.move(0.1, 0); - moduleBR.move(0.1, 0); - moduleFL.move(0.1, 0); - moduleBL.move(0.1, 0); + // moduleFR.move(0.000000001, 0); + // moduleBR.move(0.0000001, 0); + // moduleFL.move(0.000001, 0); + // moduleBL.move(0.000001, 0); // turnPidControllers[0].setReference(goal // , ControlType.kPosition, ClosedLoopSlot.kSlot0); From bcee079ef0297320851eae252708533a36477445 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 18 Feb 2025 23:38:53 -0800 Subject: [PATCH 41/52] fixed the optimization thingy where the module finds the shortest path to turn to the position --- .DataLogTool/datalogtool.json | 1 + src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 4 +--- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json index 4c9c653..fc05ab5 100644 --- a/.DataLogTool/datalogtool.json +++ b/.DataLogTool/datalogtool.json @@ -1,5 +1,6 @@ { "download": { + "localDir": "C:\\SysIdLogs", "serverTeam": "199" } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 7d1b87c..0c27e0b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -492,10 +492,8 @@ 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())); + moduleStates[i].optimize(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 85593a689145e112c5e2cbb43981a2c6582732e9 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 18 Feb 2025 23:44:44 -0800 Subject: [PATCH 42/52] Changed build.grade implementation (line 88) to back 2025-SNAPSHOT since alex fixed his commit in lib 199 --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index f3b925f..db1ac0f 100644 --- a/build.gradle +++ b/build.gradle @@ -85,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.DeepBlueRobotics:lib199:dbfca96f4f6795f3856e0adcdac4eafe20708dc6" + implementation "com.github.DeepBlueRobotics:lib199:2025-SNAPSHOT" } test { From 2e995cee6929413c8e1d842196be24dc38c5a34d Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 21 Feb 2025 16:33:42 -0800 Subject: [PATCH 43/52] All turn pid + ff values gotten, smartdashboard buttons changed to fix shuffleboard problems --- .../java/org/carlmontrobotics/Config.java | 2 +- .../java/org/carlmontrobotics/Constants.java | 28 +++++++++---------- .../subsystems/Drivetrain.java | 19 +++++++++---- 3 files changed, 29 insertions(+), 20 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index 699f34a..9a68135 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -23,7 +23,7 @@ public abstract class Config implements Sendable { // Add additional config settings by declaring a protected field, and... protected boolean exampleFlagEnabled = false; protected boolean swimShady = false; - protected boolean setupSysId = true; + 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 diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 64f4bdc..69ab514 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -95,25 +95,25 @@ 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, 25, 35.946, 30.986 }; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = {18, 18, 27, 30};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; - public static final double[] turnkI = { 0, 0.1, 0, 0 }; - public static final double[] turnkD = { 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; + public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // 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.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 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 }; + public static final double[] turnkV = {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 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[] { 0, 0, 0, 0 }; // {1.82/100, 1.815/100, 2.015/100, + : new double[] {1.2406, 0.29066, 1.9366, 2.1562}; // {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 }; @@ -123,15 +123,15 @@ public static final class Drivetrainc { 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[] kForwardVolts = { 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kForwardVolts = {0.33073, 0.32889, 0.12425, 0.10611}; //{ 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = { 0, 0, 0, 0 };//volts per m/s + public static final double[] kForwardVels = {2.5857, 2.3916, 3.0435, 3.0442}; //{ 0, 0, 0, 0 };//volts per m/s 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[] kForwardAccels = { 0, 0, 0, 0 };// volts per m/s^2 + public static final double[] kForwardAccels = {0.22104, 0.36626, 0.34054, 0.42251}; //{ 0, 0, 0, 0 };// volts per m/s^2 public static final double[] kBackwardAccels = kForwardAccels; public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second @@ -245,10 +245,10 @@ public static final class Driver { 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 int y = Button.kY.value; + public static final int b = Button.kB.value; + public static final int a = Button.kA.value; + public static final int x = Button.kX.value; } public static final class Manipulator { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 0c27e0b..dc8daed 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -390,7 +390,7 @@ public void periodic() { // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - module.periodic(); + module.turnPeriodic(); // module.move(0, goal); } @@ -926,6 +926,11 @@ private void sysIdSetup() { sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); + + sysIdTab + .add(sysIdChooser) + .withSize(2, 1); + sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); @@ -937,13 +942,16 @@ private void sysIdSetup() { 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("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + //sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); sysIdTab.add(this); for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors @@ -977,7 +985,8 @@ private void sysIdSetup() { // } private SysIdTest selector() { - SysIdTest test = sysIdChooser.getSelected(); + //SysIdTest test = sysIdChooser.getSelected(); + SysIdTest test = SysIdTest.BR_ROT; System.out.println("Test Selected: " + test); return test; } From 4feaabe96a7e968b010d4c4626565fcaf10d19db Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sat, 22 Feb 2025 17:50:47 -0800 Subject: [PATCH 44/52] Updated drive pid, turn pid fixed, shakes a lot --- .OutlineViewer/outlineviewer.json | 1 + src/main/java/org/carlmontrobotics/Constants.java | 10 ++++++---- .../org/carlmontrobotics/subsystems/Drivetrain.java | 5 +++-- 3 files changed, 10 insertions(+), 6 deletions(-) create mode 100644 .OutlineViewer/outlineviewer.json diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..0967ef4 --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1 @@ +{} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 69ab514..f614646 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -113,7 +113,7 @@ public static final class Drivetrainc { // 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.2406, 0.29066, 1.9366, 2.1562}; // {1.82/100, 1.815/100, 2.015/100, + : new double[] {2.2319, 2.2462, 2.4136, 3.6862}; // {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 }; @@ -123,15 +123,17 @@ public static final class Drivetrainc { 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[] kForwardVolts = {0.33073, 0.32889, 0.12425, 0.10611}; //{ 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kForwardVolts = {0, 0, 0, 0};//{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; + //kV // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = {2.5857, 2.3916, 3.0435, 3.0442}; //{ 0, 0, 0, 0 };//volts per m/s + public static final double[] kForwardVels = {2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s public static final double[] kBackwardVels = kForwardVels; + //kA // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kForwardAccels = {0.22104, 0.36626, 0.34054, 0.42251}; //{ 0, 0, 0, 0 };// volts per m/s^2 + public static final double[] kForwardAccels = {0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 public static final double[] kBackwardAccels = kForwardAccels; public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index dc8daed..e231e22 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -390,7 +390,8 @@ public void periodic() { // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - module.turnPeriodic(); + //module.turnPeriodic(); + module.drivePeriodic(); // module.move(0, goal); } @@ -986,7 +987,7 @@ private void sysIdSetup() { private SysIdTest selector() { //SysIdTest test = sysIdChooser.getSelected(); - SysIdTest test = SysIdTest.BR_ROT; + SysIdTest test = SysIdTest.BACK_DRIVE; System.out.println("Test Selected: " + test); return test; } From 56a79c12da5131047418611d40bf6d7102ca0fcd Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 23 Feb 2025 13:41:31 -0800 Subject: [PATCH 45/52] fixed drive kP and inverted one of the motors to fix a bug. drivetrain code now is fully updated and works --- src/main/java/org/carlmontrobotics/Constants.java | 12 ++++++------ .../org/carlmontrobotics/subsystems/Drivetrain.java | 3 ++- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index f614646..728273b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -85,7 +85,7 @@ public static final class Drivetrainc { // 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, 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 } @@ -113,9 +113,9 @@ public static final class Drivetrainc { // 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[] {2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {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[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; public static final double[] drivekD = { 0, 0, 0, 0 }; public static final boolean[] driveInversion = (CONFIG.isSwimShady() ? new boolean[] { false, false, false, false } @@ -123,17 +123,17 @@ public static final class Drivetrainc { 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[] kForwardVolts = {0, 0, 0, 0};//{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; //kV // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = {2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s + public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s public static final double[] kBackwardVels = kForwardVels; //kA // public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kForwardAccels = {0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 + public static final double[] kForwardAccels = { 0, 0, 0, 0 };//{0.31958, 0.33557, 0.70264, 0.46644}; //{ 0, 0, 0, 0 };// volts per m/s^2 public static final double[] kBackwardAccels = kForwardAccels; public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index e231e22..f6c83ba 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -391,7 +391,8 @@ public void periodic() { // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { //module.turnPeriodic(); - module.drivePeriodic(); + // module.turnPeriodic(); + module.periodic(); // module.move(0, goal); } From 03e8229923f37862b76a1a0ca2e26225ec40c237 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 23 Feb 2025 15:19:35 -0800 Subject: [PATCH 46/52] adjusting kP and reverted motor inversions to fix drivetrain again --- .../deploy/pathplanner/autos/deploy/pathplanner/navgrid.json | 1 + src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json diff --git a/src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/deploy/pathplanner/navgrid.json @@ -0,0 +1 @@ +{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]} \ 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 728273b..cee383b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -85,7 +85,7 @@ public static final class Drivetrainc { // Angular velocity = Tangential speed / radius public static final double maxRCW = maxSpeed / swerveRadius; - public static final boolean[] reversed = { true, false, false, false }; + 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 } @@ -95,7 +95,7 @@ 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 = {18, 18, 27, 30};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d From 80c7c1b3ba1a6e7e77e9c6776febe484fd533a58 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 4 Mar 2025 18:32:15 -0800 Subject: [PATCH 47/52] changed some of constants to have hammer head if statements --- .../java/org/carlmontrobotics/Config.java | 6 ++-- .../java/org/carlmontrobotics/Constants.java | 31 ++++++++++++------- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Config.java b/src/main/java/org/carlmontrobotics/Config.java index 9a68135..5e85067 100644 --- a/src/main/java/org/carlmontrobotics/Config.java +++ b/src/main/java/org/carlmontrobotics/Config.java @@ -22,7 +22,7 @@ public abstract class Config implements Sendable { // Add additional config settings by declaring a protected field, and... protected boolean exampleFlagEnabled = false; - protected boolean swimShady = false; + protected boolean hammerHead = false; protected boolean setupSysId = false; protected boolean useSmartDashboardControl = false; // whether to control arm position + rpm of // outtake through SmartDashboard @@ -37,8 +37,8 @@ public boolean isExampleFlagEnabled() { return exampleFlagEnabled; } - public boolean isSwimShady() { - return swimShady; + public boolean isHammerHead() { + return hammerHead; } public boolean isSysIdTesting() { diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index cee383b..3faa7f7 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -89,35 +89,42 @@ public static final class Drivetrainc { // 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 } + : (CONFIG.isHammerHead() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } : new double[] { 17.2266, -96.8555, -95.8008, 85.166 });/* 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 = {12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = CONFIG.isHammerHead() ? new double[] { 51.078, 60.885, 60.946, 60.986/2 } : + new double[]{12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; - public static final double[] turnkD = {1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + public static final double[] turnkD = CONFIG.isHammerHead() ? new double[] { 0/* dont edit */, 0.5, 0.42, 1 } : + new double[]{1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = {0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; + public static final double[] turnkS = CONFIG.isHammerHead() ? new double[]{ 0.13027, 0.17026, 0.2, 0.23262 }: + new double[]{0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 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.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; - public static final double[] turnkA = {0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; + public static final double[] turnkV = CONFIG.isHammerHead()? new double[] { 2.6532, 2.7597, 2.7445, 2.7698 }: + new double[] {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = CONFIG.isHammerHead()? new double[] { 0.17924, 0.17924, 0.17924, 0.17924 }: + new double[]{0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 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 } + public static final double[] drivekP = CONFIG.isHammerHead() ? new double[] { 1.75*1.275, 1.75*1.275, 1.75*1.275, 1.75*1.275 } : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, // 1.915/100}; - public static final double[] drivekI = { 0.1, 0.1, 0.1, 0.1 }; - public static final double[] drivekD = { 0, 0, 0, 0 }; - public static final boolean[] driveInversion = (CONFIG.isSwimShady() + public static final double[] drivekI = CONFIG.isHammerHead()? new double[] {0,0,0,0} : + new double[] { 0.1, 0.1, 0.1, 0.1 }; + public static final double[] drivekD = CONFIG.isHammerHead()? new double[] {0.005,0.005,0.005,0.005}: + new double[] { 0, 0, 0, 0 }; + public static final boolean[] driveInversion = (CONFIG.isHammerHead() ? new boolean[] { false, false, false, false } : new boolean[] { false, true, false, true }); public static final boolean[] turnInversion = { true, true, true, true }; @@ -147,10 +154,10 @@ public static final class Drivetrainc { 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 } + public static final double[] xPIDController = CONFIG.isHammerHead() ? 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 } + public static final double[] thetaPIDController = CONFIG.isHammerHead() ? 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, From 26e65f21a2444768003a71a2ac79d536783a6ea9 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 4 Mar 2025 19:58:02 -0800 Subject: [PATCH 48/52] more hammerhead constants added --- build.gradle | 2 +- .../java/org/carlmontrobotics/Constants.java | 34 ++++++++++--------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/build.gradle b/build.gradle index db1ac0f..fccb2e1 100644 --- a/build.gradle +++ b/build.gradle @@ -85,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.DeepBlueRobotics:lib199:2025-SNAPSHOT" + implementation "com.github.DeepBlueRobotics:lib199:32da18340af20581b2ef5e17f9d4dfa07c106dbb" } test { diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 3faa7f7..51d9b1f 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -125,17 +125,19 @@ public static final class Drivetrainc { public static final double[] drivekD = CONFIG.isHammerHead()? new double[] {0.005,0.005,0.005,0.005}: new double[] { 0, 0, 0, 0 }; public static final boolean[] driveInversion = (CONFIG.isHammerHead() - ? new boolean[] { false, false, false, false } + ? new boolean[] { true, false, true, false } : new boolean[] { false, true, false, true }); 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[] kForwardVolts = {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; + public static final double[] kForwardVolts = CONFIG.isHammerHead() ? new double[] { 0.26744, 0.31897, 0.27967, 0.2461 }: + new double[] {0, 0, 0, 0}; //{0.59395, 0.52681, 0.11097, 0.17914}; //{ 0.2, 0.2, 0.2, 0.2 }; public static final double[] kBackwardVolts = kForwardVolts; //kV // public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kForwardVels = { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s + public static final double[] kForwardVels = CONFIG.isHammerHead() ? new double[] { 2.81, 2.9098, 2.8378, 2.7391 }: + new double[] { 0, 0, 0, 0 };//{2.4114, 2.7465, 2.7546, 2.7412}; //{ 0, 0, 0, 0 };//volts per m/s public static final double[] kBackwardVels = kForwardVels; //kA @@ -172,20 +174,20 @@ public static final class Drivetrainc { // #region Ports //I think all of these are right - public static final int driveFrontLeftPort = 11; - public static final int driveFrontRightPort = 12; - public static final int driveBackLeftPort = 13; - public static final int driveBackRightPort = 14; - - public static final int turnFrontLeftPort = 1; - public static final int turnFrontRightPort = 2; - public static final int turnBackLeftPort = 3; - public static final int turnBackRightPort = 4; + public static final int driveFrontLeftPort = CONFIG.isHammerHead() ? 1 : 11; + public static final int driveFrontRightPort = CONFIG.isHammerHead() ? 2 : 12; + public static final int driveBackLeftPort = CONFIG.isHammerHead() ? 3 : 13; + public static final int driveBackRightPort = CONFIG.isHammerHead() ? 4 : 14; + + public static final int turnFrontLeftPort = CONFIG.isHammerHead() ? 11 : 1; + public static final int turnFrontRightPort = CONFIG.isHammerHead() ? 12 : 2; + public static final int turnBackLeftPort = CONFIG.isHammerHead() ? 17 : 3; + public static final int turnBackRightPort = CONFIG.isHammerHead() ? 14 : 4; //to be configured - public static final int canCoderPortFL = 1; //0 - public static final int canCoderPortFR = 2; - public static final int canCoderPortBL = 3; - public static final int canCoderPortBR = 0; //1 + public static final int canCoderPortFL = CONFIG.isHammerHead() ? 0 : 1; + public static final int canCoderPortFR = CONFIG.isHammerHead() ? 3 : 2; + public static final int canCoderPortBL = CONFIG.isHammerHead() ? 2 : 3; + public static final int canCoderPortBR = CONFIG.isHammerHead() ? 1 : 0; // #endregion From a1a48f1744e6411a4936b1fd57e5330df4aab6ca Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 9 Mar 2025 12:11:59 -0700 Subject: [PATCH 49/52] updated turnPID and drivePID but they need a lot more fixing. pushing so dean can check --- build.gradle | 4 +- .../deploy/pathplanner/autos/Circle Path.auto | 19 ++++ .../deploy/pathplanner/paths/Circle Path.path | 102 ++++++++++++++++++ .../java/org/carlmontrobotics/Constants.java | 38 +++---- .../subsystems/Drivetrain.java | 20 ++-- 5 files changed, 152 insertions(+), 31 deletions(-) create mode 100644 src/main/deploy/pathplanner/autos/Circle Path.auto create mode 100644 src/main/deploy/pathplanner/paths/Circle Path.path diff --git a/build.gradle b/build.gradle index fccb2e1..6112390 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2025.2.1" + id "edu.wpi.first.GradleRIO" version "2025.3.1" id "com.peterabeles.gversion" version "1.10" } @@ -85,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.DeepBlueRobotics:lib199:32da18340af20581b2ef5e17f9d4dfa07c106dbb" + implementation "com.github.deepbluerobotics:lib199:2025-SNAPSHOT" } test { diff --git a/src/main/deploy/pathplanner/autos/Circle Path.auto b/src/main/deploy/pathplanner/autos/Circle Path.auto new file mode 100644 index 0000000..418f5db --- /dev/null +++ b/src/main/deploy/pathplanner/autos/Circle Path.auto @@ -0,0 +1,19 @@ +{ + "version": "2025.0", + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Circle Path" + } + } + ] + } + }, + "resetOdom": true, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Circle Path.path b/src/main/deploy/pathplanner/paths/Circle Path.path new file mode 100644 index 0000000..44c7eee --- /dev/null +++ b/src/main/deploy/pathplanner/paths/Circle Path.path @@ -0,0 +1,102 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.1937499999999996, + "y": 6.418181818181818 + }, + "prevControl": null, + "nextControl": { + "x": 4.068409090909091, + "y": 7.564914772727272 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.990085227272727, + "y": 6.268607954545454 + }, + "prevControl": { + "x": 6.232244318181818, + "y": 6.9965340909090905 + }, + "nextControl": { + "x": 8.571967417451962, + "y": 4.749168482399608 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 6.541363636363636, + "y": 1.0833806818181801 + }, + "prevControl": { + "x": 8.03211647727273, + "y": 1.768304332386363 + }, + "nextControl": { + "x": 5.050610795454545, + "y": 0.39845703124999776 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.951590909090909, + "y": 0.5449147727272714 + }, + "prevControl": { + "x": 3.7592897727272727, + "y": 0.2756818181818164 + }, + "nextControl": { + "x": 1.2005926062430576, + "y": 1.1285808736765564 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.1937499999999996, + "y": 6.418181818181818 + }, + "prevControl": { + "x": 0.6082670454545456, + "y": 6.029289772727273 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "pointTowardsZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0, + "nominalVoltage": 12.0, + "unlimited": false + }, + "goalEndState": { + "velocity": 0, + "rotation": 0.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0, + "rotation": 0.0 + }, + "useDefaultConstraints": true +} \ 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 51d9b1f..2ada3f5 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -95,30 +95,30 @@ 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 = CONFIG.isHammerHead() ? new double[] { 51.078, 60.885, 60.946, 60.986/2 } : - new double[]{12, 12, 23, 23};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + public static final double[] turnkP = CONFIG.isHammerHead() ? new double[] {4.0087, 60.885, 60.946, 60.986/2 } : + new double[]{1.9085, /*0.21577*/0.1, /*0.12356*/0.05, 0.36431};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; public static final double[] turnkD = CONFIG.isHammerHead() ? new double[] { 0/* dont edit */, 0.5, 0.42, 1 } : - new double[]{1, 1.55, 0, 2};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d + new double[]{0, 0, 0, 0};//{ 0.2/* dont edit */, 0.3, 0.5, 0.4}; // todo: use d // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = CONFIG.isHammerHead() ? new double[]{ 0.13027, 0.17026, 0.2, 0.23262 }: - new double[]{0.050634, 0.033065, 0.018117, 0.021337};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 0.13027, 0.17026, 0.2, 0.23262 }; + public static final double[] turnkS = CONFIG.isHammerHead() ? new double[]{ 0.12507, 0.17026, 0.2, 0.23262 }: + new double[]{0.21969, 0.11487, 0.18525, 0.24865};//sysid for fr that didnt't work{0.041796, 0.09111, 0.64804, 1.0873}//{ 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 = CONFIG.isHammerHead()? new double[] { 2.6532, 2.7597, 2.7445, 2.7698 }: - new double[] {2.6153, 2.5924, 2.6495, 2.6705};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; - public static final double[] turnkA = CONFIG.isHammerHead()? new double[] { 0.17924, 0.17924, 0.17924, 0.17924 }: - new double[]{0.18525, 0.13879, 0.23625, 0.25589};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 0.17924, 0.17924, 0.17924, 0.17924 }; + public static final double[] turnkV = CONFIG.isHammerHead()? new double[] { 2.6172, 2.7597, 2.7445, 2.7698 }: + new double[] {2.7073, 2.6208, 2.7026, 2.7639};//sysid for fr that didnt't work{2.6403, 2.6603, 2.6168, 2.5002} //{2.6532, 2.7597, 2.7445, 2.7698}; + public static final double[] turnkA = CONFIG.isHammerHead()? new double[] { 1.2097, 0.17924, 0.17924, 0.17924 }: + new double[]{0.18069, 0.06593, 0.17439, 0.2571};//sysid for fr that didnt't work{0.33266, 0.25535, 0.17924, 0.17924} //{ 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.isHammerHead() ? new double[] { 1.75*1.275, 1.75*1.275, 1.75*1.275, 1.75*1.275 } - : new double[] {1.75, 1.75, 1.75, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + : new double[] {1.99+.6, 1.99+.5, 1.99+ .1 , 1.99}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, // 1.915/100}; public static final double[] drivekI = CONFIG.isHammerHead()? new double[] {0,0,0,0} : new double[] { 0.1, 0.1, 0.1, 0.1 }; @@ -174,15 +174,15 @@ public static final class Drivetrainc { // #region Ports //I think all of these are right - public static final int driveFrontLeftPort = CONFIG.isHammerHead() ? 1 : 11; - public static final int driveFrontRightPort = CONFIG.isHammerHead() ? 2 : 12; - public static final int driveBackLeftPort = CONFIG.isHammerHead() ? 3 : 13; - public static final int driveBackRightPort = CONFIG.isHammerHead() ? 4 : 14; - - public static final int turnFrontLeftPort = CONFIG.isHammerHead() ? 11 : 1; - public static final int turnFrontRightPort = CONFIG.isHammerHead() ? 12 : 2; - public static final int turnBackLeftPort = CONFIG.isHammerHead() ? 17 : 3; - public static final int turnBackRightPort = CONFIG.isHammerHead() ? 14 : 4; + public static final int driveFrontLeftPort = CONFIG.isHammerHead() ? 1 : 1; + public static final int driveFrontRightPort = CONFIG.isHammerHead() ? 2 : 2; + public static final int driveBackLeftPort = CONFIG.isHammerHead() ? 3 : 3; + public static final int driveBackRightPort = CONFIG.isHammerHead() ? 4 : 4; + + public static final int turnFrontLeftPort = CONFIG.isHammerHead() ? 11 : 11; + public static final int turnFrontRightPort = CONFIG.isHammerHead() ? 12 : 12; + public static final int turnBackLeftPort = CONFIG.isHammerHead() ? 17 : 13; + public static final int turnBackRightPort = CONFIG.isHammerHead() ? 14 : 14; //to be configured public static final int canCoderPortFL = CONFIG.isHammerHead() ? 0 : 1; public static final int canCoderPortFR = CONFIG.isHammerHead() ? 3 : 2; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index f6c83ba..3ddf094 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -390,9 +390,9 @@ public void periodic() { // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - //module.turnPeriodic(); + module.turnPeriodic(); // module.turnPeriodic(); - module.periodic(); + //module.periodic(); // module.move(0, goal); } @@ -947,14 +947,14 @@ private void sysIdSetup() { sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? .withSize(2, 1); - //sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); - //sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); - sysIdTab.add("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); - sysIdTab.add("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + // sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + // sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + SmartDashboard.putData("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); + SmartDashboard.putData("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); - sysIdTab.add("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); - sysIdTab.add("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); - sysIdTab.add(this); + SmartDashboard.putData("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); + SmartDashboard.putData("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse));//.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] = Volt.mutable(0); @@ -988,7 +988,7 @@ private void sysIdSetup() { private SysIdTest selector() { //SysIdTest test = sysIdChooser.getSelected(); - SysIdTest test = SysIdTest.BACK_DRIVE; + SysIdTest test = SysIdTest.FRONT_DRIVE; System.out.println("Test Selected: " + test); return test; } From 2b4e91ac8344a43cfff0f6e6cba20b530b6da356 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 9 Mar 2025 12:58:55 -0700 Subject: [PATCH 50/52] Add lib 199 for motorsssssssssssssss --- build.gradle | 2 +- .../carlmontrobotics/subsystems/Drivetrain.java | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/build.gradle b/build.gradle index 6112390..a3e569c 100644 --- a/build.gradle +++ b/build.gradle @@ -85,7 +85,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' - implementation "com.github.deepbluerobotics:lib199:2025-SNAPSHOT" + implementation "com.github.deepbluerobotics:lib199:32da18340af20581b2ef5e17f9d4dfa07c106dbb" } test { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 3ddf094..4603f74 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -209,23 +209,23 @@ public Drivetrain() { moduleFL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = new SparkMax(driveFrontLeftPort, MotorType.kBrushless), - turnMotors[0] = new SparkMax(turnFrontLeftPort, MotorType.kBrushless), + driveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), + turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = new SparkMax(driveFrontRightPort, MotorType.kBrushless), - turnMotors[1] = new SparkMax(turnFrontRightPort, MotorType.kBrushless), + driveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), + turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, - driveMotors[2] = new SparkMax(driveBackLeftPort, MotorType.kBrushless), - turnMotors[2] = new SparkMax(turnBackLeftPort, MotorType.kBrushless), + driveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), + turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, - driveMotors[3] = new SparkMax(driveBackRightPort, MotorType.kBrushless), - turnMotors[3] = new SparkMax(turnBackRightPort, MotorType.kBrushless), + driveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), + turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; turnPidControllers[0] = turnMotors[0].getClosedLoopController(); From c23ca6d1a8ec2de944fde724c552ae3c887488bc Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 9 Mar 2025 15:44:06 -0700 Subject: [PATCH 51/52] PID values that actually work for both turn and drive --- src/main/java/org/carlmontrobotics/Constants.java | 2 +- src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 2ada3f5..e543ba3 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -96,7 +96,7 @@ public static final class Drivetrainc { // front-left, front-right, back-left, back-right. // Determine correct turn PID constants public static final double[] turnkP = CONFIG.isHammerHead() ? new double[] {4.0087, 60.885, 60.946, 60.986/2 } : - new double[]{1.9085, /*0.21577*/0.1, /*0.12356*/0.05, 0.36431};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, + new double[]{50,50,50,50};//{1.9085, /*0.21577*/0.1, /*0.12356*/0.05, 0.36431};//sysid for fr that didnt't work{0.099412, 0.13414, 3.6809, 3.6809} //{49, 23,33, 28};//{51.078, 25, 35.946, 30.986}; // {0.00374, 0.00374, 0.00374, // 0.00374}; public static final double[] turnkI = {0, 0, 0, 0};//{ 0, 0.1, 0, 0 }; public static final double[] turnkD = CONFIG.isHammerHead() ? new double[] { 0/* dont edit */, 0.5, 0.42, 1 } : diff --git a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java index 4603f74..d81a982 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java @@ -390,9 +390,9 @@ public void periodic() { // moduleBR.periodic(); // double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - module.turnPeriodic(); // module.turnPeriodic(); - //module.periodic(); + // module.turnPeriodic(); + module.periodic(); // module.move(0, goal); } From 63f655b40d2a0dd660c5690ff494c529a8a6c210 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sun, 9 Mar 2025 18:10:08 -0700 Subject: [PATCH 52/52] Maybe Fix Joystick Drift? --- src/main/java/org/carlmontrobotics/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index e543ba3..7840adc 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -266,7 +266,7 @@ public static final class Manipulator { public static final int port = 1; } - public static final double JOY_THRESH = 0.01; + public static final double JOY_THRESH = 0.02; public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. }