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 001/153] 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 002/153] 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 ebf6128b9905db4f249510afb83d79b7a11bc922 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 17 Jan 2025 17:29:44 -0800 Subject: [PATCH 003/153] Pathplannerstuff --- src/main/deploy/pathplanner/navgrid.json | 1 + .../pathplanner/paths/CoralConfigs.path | 135 ++++++++++++++++++ .../pathplanner/paths/StartingConfigs.path | 115 +++++++++++++++ src/main/deploy/pathplanner/settings.json | 32 +++++ 4 files changed, 283 insertions(+) create mode 100644 src/main/deploy/pathplanner/navgrid.json create mode 100644 src/main/deploy/pathplanner/paths/CoralConfigs.path create mode 100644 src/main/deploy/pathplanner/paths/StartingConfigs.path create mode 100644 src/main/deploy/pathplanner/settings.json diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json new file mode 100644 index 0000000..23e0db9 --- /dev/null +++ b/src/main/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/deploy/pathplanner/paths/CoralConfigs.path b/src/main/deploy/pathplanner/paths/CoralConfigs.path new file mode 100644 index 0000000..cd412d7 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/CoralConfigs.path @@ -0,0 +1,135 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 2.2448049779275094, + "y": 7.447722322258365 + }, + "prevControl": null, + "nextControl": { + "x": 3.244804977927509, + "y": 7.447722322258365 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 5.004174292696725, + "y": 5.244474697559479 + }, + "prevControl": { + "x": 4.7064615000910095, + "y": 5.244474697559479 + }, + "nextControl": { + "x": 5.30188708530244, + "y": 5.244474697559479 + }, + "isLocked": false, + "linkedName": "C11" + }, + { + "anchor": { + "x": 5.289208179311857, + "y": 5.080085654005255 + }, + "prevControl": { + "x": 5.040367278685427, + "y": 5.056039735943611 + }, + "nextControl": { + "x": 5.538049079938287, + "y": 5.1041315720668985 + }, + "isLocked": false, + "linkedName": "C12" + }, + { + "anchor": { + "x": 5.805, + "y": 4.189142416305975 + }, + "prevControl": { + "x": 5.5324143394454595, + "y": 5.159338758385921 + }, + "nextControl": { + "x": 6.07758566055454, + "y": 3.218946074226031 + }, + "isLocked": false, + "linkedName": "C21" + }, + { + "anchor": { + "x": 5.803912909888542, + "y": 3.8582569473798314 + }, + "prevControl": { + "x": 5.667794856321749, + "y": 4.295016773740704 + }, + "nextControl": { + "x": 5.940030963455334, + "y": 3.4214971210189535 + }, + "isLocked": false, + "linkedName": "C22" + }, + { + "anchor": { + "x": 6.364839756618353, + "y": 0.5451487714944108 + }, + "prevControl": { + "x": 6.799189344211289, + "y": 1.457427320518574 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": -119.99999999999999 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 4, + "rotationDegrees": 180.0 + } + ], + "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": -117.26958303209868 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/StartingConfigs.path b/src/main/deploy/pathplanner/paths/StartingConfigs.path new file mode 100644 index 0000000..ce1c8d0 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/StartingConfigs.path @@ -0,0 +1,115 @@ +{ + "version": "2025.0", + "waypoints": [ + { + "anchor": { + "x": 8.0, + "y": 7.4 + }, + "prevControl": null, + "nextControl": { + "x": 8.062264438838344, + "y": 7.644490337970196 + }, + "isLocked": false, + "linkedName": "S1F" + }, + { + "anchor": { + "x": 8.0, + "y": 6.5 + }, + "prevControl": { + "x": 7.971636103762701, + "y": 7.116609069648897 + }, + "nextControl": { + "x": 8.020395624409266, + "y": 6.056614969724504 + }, + "isLocked": false, + "linkedName": "S2F" + }, + { + "anchor": { + "x": 8.0, + "y": 5.5 + }, + "prevControl": { + "x": 7.987043611353682, + "y": 5.749664038245891 + }, + "nextControl": { + "x": 8.012956388646318, + "y": 5.250335961754109 + }, + "isLocked": false, + "linkedName": "S3F" + }, + { + "anchor": { + "x": 8.0, + "y": 4.5 + }, + "prevControl": { + "x": 8.122297824367529, + "y": 4.846496610775691 + }, + "nextControl": { + "x": 7.894024420976902, + "y": 4.199747897017828 + }, + "isLocked": false, + "linkedName": "S4F" + }, + { + "anchor": { + "x": 6.3982675999074194, + "y": 2.8808078241188046 + }, + "prevControl": { + "x": 6.5963632343599095, + "y": 2.728301694370307 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 1, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 2, + "rotationDegrees": 180.0 + }, + { + "waypointRelativePos": 3, + "rotationDegrees": 180.0 + } + ], + "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": 180.0 + }, + "reversed": false, + "folder": null, + "idealStartingState": { + "velocity": 0.0, + "rotation": 180.0 + }, + "useDefaultConstraints": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json new file mode 100644 index 0000000..2ca3c4c --- /dev/null +++ b/src/main/deploy/pathplanner/settings.json @@ -0,0 +1,32 @@ +{ + "robotWidth": 0.9, + "robotLength": 0.9, + "holonomicMode": true, + "pathFolders": [], + "autoFolders": [], + "defaultMaxVel": 3.0, + "defaultMaxAccel": 3.0, + "defaultMaxAngVel": 540.0, + "defaultMaxAngAccel": 720.0, + "defaultNominalVoltage": 12.0, + "robotMass": 74.088, + "robotMOI": 6.883, + "robotTrackwidth": 0.546, + "driveWheelRadius": 0.048, + "driveGearing": 5.143, + "maxDriveSpeed": 5.45, + "driveMotorType": "NEO", + "driveCurrentLimit": 60.0, + "wheelCOF": 1.2, + "flModuleX": 0.273, + "flModuleY": 0.273, + "frModuleX": 0.273, + "frModuleY": -0.273, + "blModuleX": -0.273, + "blModuleY": 0.273, + "brModuleX": -0.273, + "brModuleY": -0.273, + "bumperOffsetX": 0.0, + "bumperOffsetY": 0.0, + "robotFeatures": [] +} \ No newline at end of file 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 004/153] //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 005/153] 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 006/153] 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 e745c86bb06ab0ff3213f787d116633ae7d48cc3 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:42:33 -0800 Subject: [PATCH 007/153] Create AlgaeEffectors subsystem --- src/main/java/AlgaeEffector.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/main/java/AlgaeEffector.java diff --git a/src/main/java/AlgaeEffector.java b/src/main/java/AlgaeEffector.java new file mode 100644 index 0000000..e69de29 From 7cf195dd1f775d10efc70799702086be1b20ac6c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:43:54 -0800 Subject: [PATCH 008/153] More algae stuff --- src/main/java/AlgaeEffector.java | 59 +++++++++++++++++++ .../java/org/carlmontrobotics/Constants.java | 4 ++ 2 files changed, 63 insertions(+) diff --git a/src/main/java/AlgaeEffector.java b/src/main/java/AlgaeEffector.java index e69de29..c6fc6e3 100644 --- a/src/main/java/AlgaeEffector.java +++ b/src/main/java/AlgaeEffector.java @@ -0,0 +1,59 @@ +package org.carlmontrobotics.subsystems; + +import static edu.wpi.first.units.MutableMeasure.mutable; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volts; +import static org.carlmontrobotics.Constants.Armc.*; +import static org.carlmontrobotics.Constants.Armc.NEO_BUILTIN_ENCODER_CPR; // (42) explicit import + +import org.carlmontrobotics.commands.TeleopArm; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.sim.MockedEncoder; + +import static org.carlmontrobotics.RobotContainer.*; + +import org.carlmontrobotics.RobotContainer; + +import com.revrobotics.CANSparkBase; +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.CANSparkMax; +import com.revrobotics.SparkAbsoluteEncoder; +import com.revrobotics.SparkPIDController; + +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.Angle; +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.RobotBase; +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.simulation.SimDeviceSim; +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.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + +public class AlgaeEffector extends SubsystemBase { + private CANSparkMax upperMotor = new CANSparkMax(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); + private CANSparkMax lowerMotor = newCANSparkMax(Constatns.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); + public RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { + upperMotor.set(upperMotorSpeed); + lowerMotor.set(lowerMotorSpeed); + } +} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..58ef954 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -12,4 +12,8 @@ public static final class Manipulator { public static final int port = 1; } } + public static final class AlgaeEffectorc { + public static int upperMotorID = 1; + public static int lowerMotorID = 2; + } } From d12fdb3d39e5056cd0b2682d1c47dd446d954ab6 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:44:56 -0800 Subject: [PATCH 009/153] Moved --- src/main/java/{ => org/carlmontrobotics}/AlgaeEffector.java | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename src/main/java/{ => org/carlmontrobotics}/AlgaeEffector.java (100%) diff --git a/src/main/java/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java similarity index 100% rename from src/main/java/AlgaeEffector.java rename to src/main/java/org/carlmontrobotics/AlgaeEffector.java From ddb9ff8924361dbe222b87120fe82581bb3a4b8b Mon Sep 17 00:00:00 2001 From: KeremOlgun <144630037+KeremOlgun@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:52:12 -0800 Subject: [PATCH 010/153] Made command --- .../java/org/carlmontrobotics/RunAlgae.java | 39 +++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/RunAlgae.java diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/RunAlgae.java new file mode 100644 index 0000000..6005987 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/RunAlgae.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.subsystems.IntakeShooter; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class RunAlgae extends Command { + + public RunAlgae(AlgaeEffector algaeEffector, boolean inverted) { + addRequirements(this.algaeEffector = algaeEffector); + this.inverted = inverted; + } + + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (inverted) { + algaeEffector.RunAlgaeMotors(0.5, -0.5) + } + else { + algaeEffector.RunAlgaeMotors(0.5, 0.5) + } + } + + // 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 From 5b6d40ab74c89ddceb1a375b3e52e6183b3c8e50 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:52:36 -0800 Subject: [PATCH 011/153] update algae effector --- src/main/java/org/carlmontrobotics/AlgaeEffector.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java index c6fc6e3..4055fb2 100644 --- a/src/main/java/org/carlmontrobotics/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/AlgaeEffector.java @@ -20,16 +20,18 @@ import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; +import com.revrobotics.SparkLowLevel; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.units.Angle; +import edu.wpi.first.unitss.Angle; import edu.wpi.first.units.Measure; import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.Velocity; @@ -50,9 +52,9 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class AlgaeEffector extends SubsystemBase { - private CANSparkMax upperMotor = new CANSparkMax(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); - private CANSparkMax lowerMotor = newCANSparkMax(Constatns.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); - public RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { + private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType kBrushless); + private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType kBrushless); + public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { upperMotor.set(upperMotorSpeed); lowerMotor.set(lowerMotorSpeed); } From d9bcafd8bc6a26c32adf548ddb5e65e18f4fe261 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:56:45 -0800 Subject: [PATCH 012/153] l --- src/main/java/org/carlmontrobotics/AlgaeEffector.java | 1 + src/main/java/org/carlmontrobotics/RunAlgae.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java index 4055fb2..3d5f260 100644 --- a/src/main/java/org/carlmontrobotics/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/AlgaeEffector.java @@ -55,6 +55,7 @@ public class AlgaeEffector extends SubsystemBase { private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType kBrushless); private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType kBrushless); public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { + upperMotor.set(upperMotorSpeed); lowerMotor.set(lowerMotorSpeed); } diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/RunAlgae.java index 6005987..8189ea1 100644 --- a/src/main/java/org/carlmontrobotics/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/RunAlgae.java @@ -9,6 +9,7 @@ public class RunAlgae extends Command { public RunAlgae(AlgaeEffector algaeEffector, boolean inverted) { addRequirements(this.algaeEffector = algaeEffector); this.inverted = inverted; + } @Override From 5bf7b098f5c7727b159b42b97b4a7b24f66f697d Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:57:05 -0800 Subject: [PATCH 013/153] semicolons --- src/main/java/org/carlmontrobotics/RunAlgae.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/RunAlgae.java index 8189ea1..ef817e7 100644 --- a/src/main/java/org/carlmontrobotics/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/RunAlgae.java @@ -21,10 +21,10 @@ public void initialize() { @Override public void execute() { if (inverted) { - algaeEffector.RunAlgaeMotors(0.5, -0.5) + algaeEffector.RunAlgaeMotors(0.5, -0.5); } else { - algaeEffector.RunAlgaeMotors(0.5, 0.5) + algaeEffector.RunAlgaeMotors(0.5, 0.5); } } From 295dda69efd76eb9c28867bd3e53bdd1f8e53100 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 13:57:15 -0800 Subject: [PATCH 014/153] Stuff --- .../java/org/carlmontrobotics/AlgaeEffector.java | 12 ++---------- src/main/java/org/carlmontrobotics/Constants.java | 10 ++++++++-- .../java/org/carlmontrobotics/RobotContainer.java | 10 ++++------ 3 files changed, 14 insertions(+), 18 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/AlgaeEffector.java index 3d5f260..c0c9f39 100644 --- a/src/main/java/org/carlmontrobotics/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/AlgaeEffector.java @@ -1,17 +1,9 @@ package org.carlmontrobotics.subsystems; -import static edu.wpi.first.units.MutableMeasure.mutable; -import static edu.wpi.first.units.Units.Radians; -import static edu.wpi.first.units.Units.RadiansPerSecond; -import static edu.wpi.first.units.Units.Seconds; -import static edu.wpi.first.units.Units.Volts; -import static org.carlmontrobotics.Constants.Armc.*; -import static org.carlmontrobotics.Constants.Armc.NEO_BUILTIN_ENCODER_CPR; // (42) explicit import - -import org.carlmontrobotics.commands.TeleopArm; + import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; -import org.carlmontrobotics.lib199.sim.MockedEncoder; + import static org.carlmontrobotics.RobotContainer.*; diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 58ef954..6fea057 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -7,13 +7,19 @@ public final class Constants { public static final class OI { public static final class Driver { public static final int port = 0; + public static final int A = 1; + public static final int B = 2; + public static final int X = 3; + public static final int Y = 4; + public static final int leftBumper = 5; + public static final int rightBumper = 6; } public static final class Manipulator { public static final int port = 1; } } public static final class AlgaeEffectorc { - public static int upperMotorID = 1; - public static int lowerMotorID = 2; + public static final int upperMotorID = 1; + public static final int lowerMotorID = 2; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 836869c..47cb7d9 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -30,13 +30,9 @@ public class RobotContainer { //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); - + private final AlgaeEffector algaeEffector = new AlgaeEffector(); public RobotContainer() { - - setDefaultCommands(); setBindingsDriver(); - setBindingsManipulator(); } private void setDefaultCommands() { @@ -48,7 +44,9 @@ private void setDefaultCommands() { // () -> driverController.getRawButton(OI.Driver.slowDriveButton) // )); } - private void setBindingsDriver() {} + private void setBindingsDriver() { + + } private void setBindingsManipulator() {} public Command getAutonomousCommand() { From 489785e3d11510de3c358c748e1c006db91f1178 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 13:58:05 -0800 Subject: [PATCH 015/153] move stuff --- .../java/org/carlmontrobotics/{ => commands}/RunAlgae.java | 5 +++-- .../org/carlmontrobotics/{ => subsystems}/AlgaeEffector.java | 0 2 files changed, 3 insertions(+), 2 deletions(-) rename src/main/java/org/carlmontrobotics/{ => commands}/RunAlgae.java (91%) rename src/main/java/org/carlmontrobotics/{ => subsystems}/AlgaeEffector.java (100%) diff --git a/src/main/java/org/carlmontrobotics/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java similarity index 91% rename from src/main/java/org/carlmontrobotics/RunAlgae.java rename to src/main/java/org/carlmontrobotics/commands/RunAlgae.java index ef817e7..d1e0eb5 100644 --- a/src/main/java/org/carlmontrobotics/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -1,5 +1,5 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.subsystems.IntakeShooter; +import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; @@ -37,4 +37,5 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false} \ No newline at end of file + return false; + } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java similarity index 100% rename from src/main/java/org/carlmontrobotics/AlgaeEffector.java rename to src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java From 02c34a8006f89eacac44a78c065f582bde98a6a9 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:00:48 -0800 Subject: [PATCH 016/153] push push! --- src/main/java/org/carlmontrobotics/commands/RunAlgae.java | 3 ++- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index d1e0eb5..be0740f 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -38,4 +38,5 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { return false; - } \ No newline at end of file + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index c0c9f39..b17ecb5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -7,12 +7,14 @@ import static org.carlmontrobotics.RobotContainer.*; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; import com.revrobotics.CANSparkBase; import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.CANSparkMax; import com.revrobotics.SparkAbsoluteEncoder; import com.revrobotics.SparkPIDController; @@ -44,8 +46,8 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class AlgaeEffector extends SubsystemBase { - private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType kBrushless); - private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType kBrushless); + private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); + private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { upperMotor.set(upperMotorSpeed); From f47ef0600ae7dbb712a66e4ed8774ad5a3f2e96a Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:01:53 -0800 Subject: [PATCH 017/153] import yay! --- src/main/java/org/carlmontrobotics/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 47cb7d9..d65e2e8 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -8,7 +8,7 @@ // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; - +import org.carlmontrobotics.subsystems.AlgaeEffector; //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; From 05d75ff3fba4887fa2c95d366b9100dbcf7fd8f5 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:03:58 -0800 Subject: [PATCH 018/153] CookingStuff --- .../java/org/carlmontrobotics/RobotContainer.java | 3 +++ .../org/carlmontrobotics/commands/RunAlgae.java | 13 +++++++++---- 2 files changed, 12 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index d65e2e8..12752cc 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -45,6 +45,9 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { + new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, false, false)); + new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, true, false)); + new Trigger(() -> driverController.getRawButton(OI.A)).onTrue(new RunAlgae(algaeEffector, false, true)); } private void setBindingsManipulator() {} diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index be0740f..6305770 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -6,7 +6,7 @@ public class RunAlgae extends Command { - public RunAlgae(AlgaeEffector algaeEffector, boolean inverted) { + public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); this.inverted = inverted; @@ -20,11 +20,16 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (inverted) { - algaeEffector.RunAlgaeMotors(0.5, -0.5); + if (stop) { + algaeEffector.RunAlgaeMotors(0,0); } else { - algaeEffector.RunAlgaeMotors(0.5, 0.5); + if (inverted) { + algaeEffector.RunAlgaeMotors(0.5, -0.5); + } + else { + algaeEffector.RunAlgaeMotors(0.5, 0.5); + } } } From 195fb80ca3cf5504586bf3ad57738ceb59a9acff Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:04:37 -0800 Subject: [PATCH 019/153] l --- src/main/java/org/carlmontrobotics/commands/RunAlgae.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index 6305770..ad32788 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -5,6 +5,8 @@ import edu.wpi.first.wpilibj2.command.Command; public class RunAlgae extends Command { + AlgaeEffector algaeEffector; + Boolean inverted; public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); From 27e3eaa6c57a42e110acb23afd8baf7e4261e4fa Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:07:02 -0800 Subject: [PATCH 020/153] o --- src/main/java/org/carlmontrobotics/Constants.java | 1 + src/main/java/org/carlmontrobotics/commands/RunAlgae.java | 1 + 2 files changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6fea057..527d683 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -17,6 +17,7 @@ public static final class Driver { public static final class Manipulator { public static final int port = 1; } + public static final int X = 0; } public static final class AlgaeEffectorc { public static final int upperMotorID = 1; diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index ad32788..6dc2732 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -8,6 +8,7 @@ public class RunAlgae extends Command { AlgaeEffector algaeEffector; Boolean inverted; + public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); this.inverted = inverted; From 29b307ef608bdb23cf6dc6074c2f7c2e0b81599c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:10:05 -0800 Subject: [PATCH 021/153] RobotContainer --- src/main/java/org/carlmontrobotics/RobotContainer.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 12752cc..30b34e7 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -45,8 +45,9 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { - new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, false, false)); - new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, true, false)); + new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); + new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); + new Trigger(() -> driverController.getRawButton(OI.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); new Trigger(() -> driverController.getRawButton(OI.A)).onTrue(new RunAlgae(algaeEffector, false, true)); } From aa0356ecb82c06a644bd030e0954a82d6a1e9f46 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Mon, 20 Jan 2025 14:11:14 -0800 Subject: [PATCH 022/153] Update RunAlgae.java --- .../carlmontrobotics/commands/RunAlgae.java | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java index 6dc2732..e8d3c0c 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/RunAlgae.java @@ -6,12 +6,15 @@ public class RunAlgae extends Command { AlgaeEffector algaeEffector; - Boolean inverted; + boolean stop; + int direction; - public RunAlgae(AlgaeEffector algaeEffector, boolean inverted, boolean stop) { + + public RunAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { addRequirements(this.algaeEffector = algaeEffector); - this.inverted = inverted; + this.direction = direction; + this.stop = stop; } @@ -27,11 +30,14 @@ public void execute() { algaeEffector.RunAlgaeMotors(0,0); } else { - if (inverted) { - algaeEffector.RunAlgaeMotors(0.5, -0.5); + if (direction == 1) { + algaeEffector.RunAlgaeMotors(0.5, 0.5); + } + else if (direction == 2) { + algaeEffector.RunAlgaeMotors(-0.5, -0.5); } - else { - algaeEffector.RunAlgaeMotors(0.5, 0.5); + else if (direction == 3) { + algaeEffector.RunAlgaeMotors(-0.5, 0.5); } } } From a365b59f5ed5e8c74ca78ddc99a041aa827ce172 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:21:17 -0800 Subject: [PATCH 023/153] k --- build.gradle | 2 +- .../subsystems/AlgaeEffector.java | 15 ++++----------- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/build.gradle b/build.gradle index 8d59a2e..35c0c0f 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamNumber() + team = project.frc.getTeamOrDefault(199) debug = project.frc.getDebugOrDefault(false) artifacts { diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index b17ecb5..9f202c1 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -10,26 +10,19 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; -import com.revrobotics.CANSparkBase; -import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkBase.SoftLimitDirection; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.CANSparkMax; -import com.revrobotics.SparkAbsoluteEncoder; -import com.revrobotics.SparkPIDController; -import com.revrobotics.SparkLowLevel; + import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; -import edu.wpi.first.unitss.Angle; + 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.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; @@ -43,7 +36,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; + public class AlgaeEffector extends SubsystemBase { private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); From c44fc1ecefafe1d56f5f3f4d407d315072690f20 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 20 Jan 2025 14:34:17 -0800 Subject: [PATCH 024/153] yay it builds hooray!! --- src/main/java/org/carlmontrobotics/RobotContainer.java | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 30b34e7..c5e9f54 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -8,6 +8,8 @@ // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; + +import org.carlmontrobotics.commands.RunAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; //controllers import edu.wpi.first.wpilibj.GenericHID; @@ -45,10 +47,10 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { - new Trigger(() -> driverController.getRawButton(OI.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); - new Trigger(() -> driverController.getRawButton(OI.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); - new Trigger(() -> driverController.getRawButton(OI.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); - new Trigger(() -> driverController.getRawButton(OI.A)).onTrue(new RunAlgae(algaeEffector, false, true)); + new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong + new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); + new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); + new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); } private void setBindingsManipulator() {} 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 025/153] 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 4d0dba2da111ccfc5e2418ec59a522049b6dd070 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Fri, 24 Jan 2025 12:49:24 -0800 Subject: [PATCH 026/153] did some of the coral effector --- .../java/org/carlmontrobotics/Constants.java | 11 +++ .../carlmontrobotics/commands/RunCoral.java | 44 ++++++++++++ .../subsystems/CoralEffector.java | 67 +++++++++++++++++++ 3 files changed, 122 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/commands/RunCoral.java create mode 100644 src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 527d683..6da292f 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -23,4 +23,15 @@ public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; } + public static final class CoralEffectorc { + public static final int effectorMotorID = 3; + public static final int effectorDistanceSensorID = 4; + public static final double kP = 0.0; + public static final double kI = 0.0; + public static final double kD = 0.0; + public static final double kS = 0.0; + + public static final double feedforward[] = {0.0}; + + } } diff --git a/src/main/java/org/carlmontrobotics/commands/RunCoral.java b/src/main/java/org/carlmontrobotics/commands/RunCoral.java new file mode 100644 index 0000000..78ff52a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/RunCoral.java @@ -0,0 +1,44 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class RunCoral extends Command { + // intake until sees game peice or 4sec has passed + private final CoralEffector Coral; + private double initSpeed = 2100; + + public RunCoral(CoralEffector Coral) { + addRequirements(this.Coral = Coral); + } + + @Override + public void initialize() { + Coral.setRPMEffector(initSpeed); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + increaseSpeed = SmartDashboard.getNumber("Increase speed", 0); + slowSpeed = SmartDashboard.getNumber("Slow intake speed", 0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Coral.stopEffector(); + } + + @Override + public boolean isFinished() { + //distance sensor doesn't detect coral + return Coral.outtakeDetectsNote(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java new file mode 100644 index 0000000..f9e76ca --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -0,0 +1,67 @@ +package org.carlmontrobotics.subsystems; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.playingwithfusion.TimeOfFlight; +import com.playingwithfusion.TimeOfFlight.RangingMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkFlex; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.datalog.DataLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkFlex; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + + + +public class CoralEffector extends SubsystemBase { + private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); + private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); + private final SparkPIDController pidControllerEffector = effectorMotor.getPIDController(); + private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); + private double goalOutakeRPM = effectorEncoder.getVelocity(); + + private Timer effectorTOFTimer = new Timer(); + private TimeOfFlight effectorDistanceSensor = new TimeOfFlight(Constants.CoralEffectorc.effectorDistanceSensorID); + + private double lastValidDistanceIntake = Double.POSITIVE_INFINITY; + private double lastValidDistanceOuttake = Double.POSITIVE_INFINITY; + + public CoralEffector() { + pidControllerEffector.setP(Constants.CoralEffectorc.kP); + pidControllerEffector.setI(Constants.CoralEffectorc.kI); + pidControllerEffector.setD(Constants.CoralEffectorc.kD); + //Will add distance sensor later + + } + + public void motorSetEffector(double speed) { + effectorMotor.set(speed); + } + + public void shootIntake() { + motorSetEffector(0.5); + } + + public void setRPMEffector(double rpm) { + pidControllerEffector.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, + effectorFeedforward.calculate(rpm / 60.0)); + } + + public void stopEffector() { + setRPMEffector(0); + } + + public void intakeShootEffector() { + setRPMEffector(2100); + } +} From c36bef4187e0899ad4967fef98675e15da315a60 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Fri, 24 Jan 2025 19:26:44 -0800 Subject: [PATCH 027/153] Updated commands and subsytems --- .../java/org/carlmontrobotics/Constants.java | 12 ++- .../org/carlmontrobotics/RobotContainer.java | 30 +++++++- .../{RunCoral.java => IntakeCoral.java} | 20 ++--- .../commands/OuttakeCoral.java | 42 +++++++++++ .../subsystems/CoralEffector.java | 74 ++++++++++++++----- 5 files changed, 143 insertions(+), 35 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{RunCoral.java => IntakeCoral.java} (64%) create mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6da292f..27bdb81 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,7 @@ package org.carlmontrobotics; +import edu.wpi.first.wpilibj.XboxController.Axis; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -16,6 +18,7 @@ public static final class Driver { } public static final class Manipulator { public static final int port = 1; + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; } public static final int X = 0; } @@ -26,12 +29,13 @@ public static final class AlgaeEffectorc { public static final class CoralEffectorc { public static final int effectorMotorID = 3; public static final int effectorDistanceSensorID = 4; + public static final int ManipulatorControllerPort = 5; + public static final Axis OuttakeTrigger = Axis.kRightTrigger; + public static final Axis IntakeTrigger = Axis.kLeftTrigger; public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; - public static final double kS = 0.0; - - public static final double feedforward[] = {0.0}; - + //TODO: Change after testing + public static final int DETECT_DISTANCE_INCHES = 3; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index c5e9f54..025793a 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -9,8 +9,15 @@ // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI; +import org.carlmontrobotics.Constants.OI.Manipulator.*; +import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.RunAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.subsystems.CoralEffector; +import org.carlmontrobotics.commands.IntakeCoral; +import org.carlmontrobotics.commands.OuttakeCoral; + + //controllers import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -18,11 +25,13 @@ //commands import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; //control bindings +import static org.carlmontrobotics.Constants.CoralEffectorc.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -32,9 +41,12 @@ public class RobotContainer { //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.Driver.port); private final AlgaeEffector algaeEffector = new AlgaeEffector(); + private final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { setBindingsDriver(); + setBindingsManipulator(); } private void setDefaultCommands() { @@ -53,7 +65,23 @@ private void setBindingsDriver() { new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); } - private void setBindingsManipulator() {} + + + private void setBindingsManipulator() { + axisTrigger(manipulatorController, OuttakeTrigger) + .whileTrue(new IntakeCoral(coralEffector)); + + axisTrigger(manipulatorController, IntakeTrigger) + .whileTrue(new OuttakeCoral(coralEffector)); + } + + + + private Trigger axisTrigger(GenericHID controller, Axis axis) { + return new Trigger(() -> Math + .abs(getStickValue(controller, axis)) > Constants.OI.Manipulator.MIN_AXIS_TRIGGER_VALUE); + } + public Command getAutonomousCommand() { return Commands.print("No autonomous command configured"); diff --git a/src/main/java/org/carlmontrobotics/commands/RunCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java similarity index 64% rename from src/main/java/org/carlmontrobotics/commands/RunCoral.java rename to src/main/java/org/carlmontrobotics/commands/IntakeCoral.java index 78ff52a..c5a37fc 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java @@ -8,27 +8,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -public class RunCoral extends Command { - // intake until sees game peice or 4sec has passed +public class IntakeCoral extends Command { private final CoralEffector Coral; - private double initSpeed = 2100; + int increaseSpeed; - public RunCoral(CoralEffector Coral) { + private double Speed = 1000; + + public IntakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { - Coral.setRPMEffector(initSpeed); + Coral.setRPM(Speed); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - increaseSpeed = SmartDashboard.getNumber("Increase speed", 0); - slowSpeed = SmartDashboard.getNumber("Slow intake speed", 0); - } + public void execute() {} // Called once the command ends or is interrupted. @Override @@ -39,6 +37,8 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { //distance sensor doesn't detect coral - return Coral.outtakeDetectsNote(); + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return Coral.coralDetects() == true; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java new file mode 100644 index 0000000..d1c037f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java @@ -0,0 +1,42 @@ +package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + +import org.carlmontrobotics.subsystems.CoralEffector; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class OuttakeCoral extends Command { + private final CoralEffector Coral; + int increaseSpeed; private double Speed = 2100; + public OuttakeCoral(CoralEffector Coral) { + addRequirements(this.Coral = Coral); + } + + @Override + public void initialize() { + Coral.setRPM(Speed); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Coral.stopEffector(); + } + + @Override + public boolean isFinished() { + //distance sensor doesn't detect coral + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return Coral.coralDetects() == false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index f9e76ca..f170aa7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -8,60 +8,94 @@ import com.playingwithfusion.TimeOfFlight.RangingMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; import edu.wpi.first.util.datalog.DataLogEntry; import edu.wpi.first.util.datalog.StringLogEntry; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.SparkFlex; import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + public class CoralEffector extends SubsystemBase { private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); - private final SparkPIDController pidControllerEffector = effectorMotor.getPIDController(); - private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); + private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); + // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); private double goalOutakeRPM = effectorEncoder.getVelocity(); - + DigitalInput limitSwitch = new DigitalInput(0); //TODO: change channel after wired private Timer effectorTOFTimer = new Timer(); private TimeOfFlight effectorDistanceSensor = new TimeOfFlight(Constants.CoralEffectorc.effectorDistanceSensorID); - - private double lastValidDistanceIntake = Double.POSITIVE_INFINITY; - private double lastValidDistanceOuttake = Double.POSITIVE_INFINITY; + + private double lastValidDistance = Double.POSITIVE_INFINITY; public CoralEffector() { - pidControllerEffector.setP(Constants.CoralEffectorc.kP); - pidControllerEffector.setI(Constants.CoralEffectorc.kI); - pidControllerEffector.setD(Constants.CoralEffectorc.kD); + SparkFlexConfig c = new SparkFlexConfig(); + c.closedLoop.pid( + Constants.CoralEffectorc.kP, + Constants.CoralEffectorc.kI, + Constants.CoralEffectorc.kD + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + effectorMotor.configure(c, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); //Will add distance sensor later } - public void motorSetEffector(double speed) { + public void setSpeed(double speed) { effectorMotor.set(speed); } - - public void shootIntake() { - motorSetEffector(0.5); - } - public void setRPMEffector(double rpm) { - pidControllerEffector.setReference(rpm, CANSparkBase.ControlType.kVelocity, 0, - effectorFeedforward.calculate(rpm / 60.0)); + public void setRPM(double rpm) { + pidControllerEffector.setReference(rpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); } public void stopEffector() { - setRPMEffector(0); + setRPM(0); + } + + public void setRPM() { + setRPM(2100); } - public void intakeShootEffector() { - setRPMEffector(2100); + + public boolean coralDetects() { + return lastValidDistance < DETECT_DISTANCE_INCHES; + } + + public boolean limitDetects() { + return limitSwitch.get(); + } + + public void updateValues() { + if (effectorDistanceSensor.isRangeValid()) { + if (lastValidDistance != 5.75) { + effectorTOFTimer.reset(); + } else + effectorTOFTimer.start(); + lastValidDistance = Units.metersToInches(effectorDistanceSensor.getRange()); + } + } + + + @Override + public void periodic() { + updateValues(); + limitDetects(); } } From c583b7492c692c745fe3e4bc464081822a09f0fa Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 25 Jan 2025 10:11:47 -0800 Subject: [PATCH 028/153] Comment --- .../java/org/carlmontrobotics/subsystems/CoralEffector.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index f170aa7..9c4e936 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -34,7 +34,7 @@ public class CoralEffector extends SubsystemBase { - private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); + private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); 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 029/153] 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 030/153] 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 eb02ca9374afdd02de593748113e4ac8bbd378d7 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 29 Jan 2025 09:32:35 -0800 Subject: [PATCH 031/153] Made Elevator Subsystem Code does not work for some reason with the new 2025 rev migration :( --- build.gradle | 2 +- .../java/org/carlmontrobotics/Constants.java | 23 +++++++ .../carlmontrobotics/subsystems/Elevator.java | 66 +++++++++++++++++++ 3 files changed, 90 insertions(+), 1 deletion(-) create mode 100644 src/main/java/org/carlmontrobotics/subsystems/Elevator.java diff --git a/build.gradle b/build.gradle index 8d59a2e..35c0c0f 100644 --- a/build.gradle +++ b/build.gradle @@ -21,7 +21,7 @@ deploy { // or from command line. If not found an exception will be thrown. // You can use getTeamOrDefault(team) instead of getTeamNumber if you // want to store a team number in this file. - team = project.frc.getTeamNumber() + team = project.frc.getTeamOrDefault(199) debug = project.frc.getDebugOrDefault(false) artifacts { diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 6d6986d..614add7 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,7 @@ package org.carlmontrobotics; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; @@ -12,4 +14,25 @@ public static final class Manipulator { public static final int port = 1; } } + public static final class Elevatorc { + //ports + public static final int masterPort = 19; + public static final int followerPort = 20; + + //Config + public static final IdleMode masterIdleMode = IdleMode.kBrake; + public static final IdleMode followerIdleMode = IdleMode.kBrake; + public static final boolean masterInverted = true; + public static final boolean followerInverted = true; + public static final double masterPositionConversionFactor = 1000; + public static final double followerPositionConversionFactor = 1000; + public static final double masterVelocityConversionFactor = 1000; + public static final double followerVelocityConversionFactor = 1000; + + //PID + public static final double kP = 1; + public static final double kI = 0; + public static final double kD = 0; + + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java new file mode 100644 index 0000000..4918e42 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -0,0 +1,66 @@ +// 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.subsystems; + +import org.carlmontrobotics.Constants; + +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.playingwithfusion.TimeOfFlight; +import com.playingwithfusion.TimeOfFlight.RangingMode; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; + +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.util.datalog.DataLogEntry; +import edu.wpi.first.util.datalog.StringLogEntry; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; + +public class Elevator extends SubsystemBase { + /** Creates a new Elevator. */ + private SparkMax masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); + private SparkMaxConfig masterConfig = new SparkMaxConfig(); + private SparkMax followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); + private SparkMaxConfig followerConfig = new SparkMaxConfig(); + + public Elevator() {} + //Not working :( + // followerMotor.follow(masterMotor, followerInverted); + // masterConfig + // .inverted(masterInverted) + // .IdleMode(masterIdleMode); + // masterConfig.encoder + // .positionConversionFactor(masterPositionConversionFactor) + // .velocityConversionFactor(masterVelocityConversionFactor); + // masterConfig.closedLoop + // .pid(Constants.Elevatorc.kP,Constants.Elevatorc.kI,Constants.Elevatorc.kD) + // .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + // masterMotor.configure(masterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} From fe81dcc6ef4f67e29138b095a9ed114ac107eff8 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 29 Jan 2025 21:41:42 -0800 Subject: [PATCH 032/153] Config, pid and useful methods --- .../carlmontrobotics/subsystems/Elevator.java | 76 +++++++++++++++---- 1 file changed, 62 insertions(+), 14 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 4918e42..2f2efc4 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -17,6 +17,7 @@ import com.playingwithfusion.TimeOfFlight; import com.playingwithfusion.TimeOfFlight.RangingMode; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ControlType; @@ -39,28 +40,75 @@ public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ + //Master private SparkMax masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); private SparkMaxConfig masterConfig = new SparkMaxConfig(); + private RelativeEncoder masterEncoder = masterMotor.getEncoder(); + //Follower private SparkMax followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); private SparkMaxConfig followerConfig = new SparkMaxConfig(); + private RelativeEncoder followerEncoder = followerMotor.getEncoder(); + + //Absolute Encoder + private AbsoluteEncoder primaryEncoder = masterMotor.getAbsoluteEncoder(); + + //Vars + private double heightGoal; + //PID + private final SparkClosedLoopController pidElevatorController = masterMotor.getClosedLoopController(); + + public Elevator() { + //Master Config + masterConfig + .inverted(Constants.Elevatorc.masterInverted) + .idleMode(Constants.Elevatorc.masterIdleMode); + masterConfig.encoder + .positionConversionFactor(Constants.Elevatorc.masterPositionConversionFactor) + .velocityConversionFactor(Constants.Elevatorc.masterVelocityConversionFactor); + masterConfig.closedLoop + .pid(Constants.Elevatorc.kP, Constants.Elevatorc.kI,Constants.Elevatorc.kD) + .feedbackSensor(FeedbackSensor.kPrimaryEncoder); + masterMotor.configure(masterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + //I don't know if this is needed + //Follower Config + followerConfig + .inverted(Constants.Elevatorc.followerInverted) + .idleMode(Constants.Elevatorc.followerIdleMode); + followerConfig.encoder + .positionConversionFactor(Constants.Elevatorc.followerPositionConversionFactor) + .velocityConversionFactor(Constants.Elevatorc.followerVelocityConversionFactor); + followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + //follow thingy + //followerMotor.follow(masterMotor); + } + + + public void setGoal(double goal) { + heightGoal = goal; + } - public Elevator() {} - //Not working :( - // followerMotor.follow(masterMotor, followerInverted); - // masterConfig - // .inverted(masterInverted) - // .IdleMode(masterIdleMode); - // masterConfig.encoder - // .positionConversionFactor(masterPositionConversionFactor) - // .velocityConversionFactor(masterVelocityConversionFactor); - // masterConfig.closedLoop - // .pid(Constants.Elevatorc.kP,Constants.Elevatorc.kI,Constants.Elevatorc.kD) - // .feedbackSensor(FeedbackSensor.kPrimaryEncoder); - // masterMotor.configure(masterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + public double getGoal() { + return heightGoal; + } + public void getToGoal() { + pidElevatorController.setReference(heightGoal, ControlType.kPosition); + } + + public void setSpeed(double speed){ + masterMotor.set(speed); + } + + public void stopElevator(){ + masterMotor.set(0); + } + + public double getEncoderValue() { + return masterEncoder.getPosition(); + } @Override public void periodic() { - // This method will be called once per scheduler run + getToGoal(); } } From cc615f14091236662ce174613b7e99f735a9ffc5 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 29 Jan 2025 22:11:00 -0800 Subject: [PATCH 033/153] Enums added --- .../java/org/carlmontrobotics/Constants.java | 25 +++++++++++++++++++ .../carlmontrobotics/subsystems/Elevator.java | 2 +- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 614add7..121230b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -33,6 +33,31 @@ public static final class Elevatorc { public static final double kP = 1; public static final double kI = 0; public static final double kD = 0; + //Positions + public static final double downPos = 0; + public static final double l1 = 0; + public static final double l2 = 0; + public static final double l3 = 0; + public static final double l4 =0; + public static final double net = 0; + public static final double processor = 0; + //ScoreENUM + public enum ElevatorPos { + DOWN(downPos), + L1(l1), + L2(l2), + L3(l3), + L4(l4), + NET(net), + PROCESSOR(processor); + + public final double positionInches; + ElevatorPos(double positionInches) { + this.positionInches = positionInches; + } + } + //Tolerance + public static final double elevatorTolerance = 0.4; } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 2f2efc4..2eb06cc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -25,6 +25,7 @@ import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.util.Units; @@ -82,7 +83,6 @@ public Elevator() { //followerMotor.follow(masterMotor); } - public void setGoal(double goal) { heightGoal = goal; } From 19d8cc502dccc08d5238ba8b9b7ab0135d6cea41 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 30 Jan 2025 07:29:18 -0800 Subject: [PATCH 034/153] More enum positions --- src/main/java/org/carlmontrobotics/Constants.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 121230b..47f0ae9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -38,9 +38,11 @@ public static final class Elevatorc { public static final double l1 = 0; public static final double l2 = 0; public static final double l3 = 0; - public static final double l4 =0; + public static final double l4 = 0; public static final double net = 0; public static final double processor = 0; + public static final double bottomAlgaeRemoval = 0; + public static final double uppperAlgaeRemoval = 0; //ScoreENUM public enum ElevatorPos { DOWN(downPos), @@ -49,7 +51,9 @@ public enum ElevatorPos { L3(l3), L4(l4), NET(net), - PROCESSOR(processor); + PROCESSOR(processor), + BOTTOMALGAE(bottomAlgaeRemoval), + UPPERALGAE(uppperAlgaeRemoval); public final double positionInches; ElevatorPos(double positionInches) { From 2dd92f77819e5d6cbea2fb1667cdcb7eb27797e2 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 30 Jan 2025 19:34:38 -0800 Subject: [PATCH 035/153] Fixed follow --- .../carlmontrobotics/subsystems/Elevator.java | 39 ++++++------------- 1 file changed, 11 insertions(+), 28 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 2eb06cc..8567f82 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -6,38 +6,18 @@ import org.carlmontrobotics.Constants; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkMaxConfig; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; - -import com.playingwithfusion.TimeOfFlight; -import com.playingwithfusion.TimeOfFlight.RangingMode; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkClosedLoopController; -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.datalog.DataLogEntry; -import edu.wpi.first.util.datalog.StringLogEntry; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ @@ -59,6 +39,10 @@ public class Elevator extends SubsystemBase { private final SparkClosedLoopController pidElevatorController = masterMotor.getClosedLoopController(); public Elevator() { + configureMotors(); + } + + private void configureMotors () { //Master Config masterConfig .inverted(Constants.Elevatorc.masterInverted) @@ -78,11 +62,10 @@ public Elevator() { followerConfig.encoder .positionConversionFactor(Constants.Elevatorc.followerPositionConversionFactor) .velocityConversionFactor(Constants.Elevatorc.followerVelocityConversionFactor); + followerConfig.follow(Constants.Elevatorc.masterPort, Constants.Elevatorc.followerInverted); followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - //follow thingy - //followerMotor.follow(masterMotor); } - + public void setGoal(double goal) { heightGoal = goal; } From a243e20d5cd5db17cb340ad7bc634d2cbc9bb1b5 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 30 Jan 2025 19:48:41 -0800 Subject: [PATCH 036/153] Commands created --- .../commands/ElevatorBottomAlgaeRemoval.java | 26 +++++++++++++++++++ .../commands/ElevatorDown.java | 26 +++++++++++++++++++ .../carlmontrobotics/commands/ElevatorL1.java | 26 +++++++++++++++++++ .../carlmontrobotics/commands/ElevatorL2.java | 26 +++++++++++++++++++ .../carlmontrobotics/commands/ElevatorL3.java | 26 +++++++++++++++++++ .../carlmontrobotics/commands/ElevatorL4.java | 26 +++++++++++++++++++ .../commands/ElevatorNet.java | 26 +++++++++++++++++++ .../commands/ElevatorProcessor.java | 26 +++++++++++++++++++ .../commands/ElevatorUpperAlgaeRemoval.java | 7 +++++ 9 files changed, 215 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorDown.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL1.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL2.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL3.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL4.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorNet.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java create mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java b/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java new file mode 100644 index 0000000..d1d8f3a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorBottomAlgaeRemoval extends Command { + public ElevatorBottomAlgaeRemoval() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java b/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java new file mode 100644 index 0000000..a06175f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorDown extends Command { + public ElevatorDown() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java new file mode 100644 index 0000000..29b1835 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorL1 extends Command { + public ElevatorL1() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java new file mode 100644 index 0000000..f1fa3a3 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorL2 extends Command { + public ElevatorL2() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java new file mode 100644 index 0000000..457120b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorL3 extends Command { + public ElevatorL3() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java new file mode 100644 index 0000000..a976fb2 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorL4 extends Command { + public ElevatorL4() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java b/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java new file mode 100644 index 0000000..d0a1b54 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorNet extends Command { + public ElevatorNet() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java b/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java new file mode 100644 index 0000000..62f8d4b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java @@ -0,0 +1,26 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorProcessor extends Command { + public ElevatorProcessor() { + + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java b/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java new file mode 100644 index 0000000..cbe3fde --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java @@ -0,0 +1,7 @@ +package org.carlmontrobotics.commands; + +import edu.wpi.first.wpilibj2.command.Command; + +public class ElevatorUpperAlgaeRemoval extends Command { + +} From 8e72de94b76ded9822adc78b256b43190f32caf6 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 30 Jan 2025 21:26:32 -0800 Subject: [PATCH 037/153] Added commands to elevator --- .../java/org/carlmontrobotics/Constants.java | 4 ++++ .../commands/ElevatorBottomAlgaeRemoval.java | 11 ++++++--- .../commands/ElevatorDown.java | 11 ++++++--- .../carlmontrobotics/commands/ElevatorL1.java | 11 ++++++--- .../carlmontrobotics/commands/ElevatorL2.java | 11 ++++++--- .../carlmontrobotics/commands/ElevatorL3.java | 11 ++++++--- .../carlmontrobotics/commands/ElevatorL4.java | 11 ++++++--- .../commands/ElevatorNet.java | 11 ++++++--- .../commands/ElevatorProcessor.java | 10 +++++--- .../commands/ElevatorUpperAlgaeRemoval.java | 24 +++++++++++++++++++ .../carlmontrobotics/subsystems/Elevator.java | 5 ++++ 11 files changed, 96 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 47f0ae9..d25a5c9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -59,6 +59,10 @@ public enum ElevatorPos { ElevatorPos(double positionInches) { this.positionInches = positionInches; } + + public double getPosition() { + return positionInches; + } } //Tolerance public static final double elevatorTolerance = 0.4; diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java b/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java index d1d8f3a..bc8cebf 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorBottomAlgaeRemoval extends Command { - public ElevatorBottomAlgaeRemoval() { - + Elevator elevator; + + public ElevatorBottomAlgaeRemoval(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.BOTTOMALGAE); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java b/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java index a06175f..c5d6ca9 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorDown extends Command { - public ElevatorDown() { - + Elevator elevator; + + public ElevatorDown(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.DOWN); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java index 29b1835..a8a6073 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorL1 extends Command { - public ElevatorL1() { - + Elevator elevator; + + public ElevatorL1(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.L1); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java index f1fa3a3..890ea44 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorL2 extends Command { - public ElevatorL2() { - + Elevator elevator; + + public ElevatorL2(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.L2); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java index 457120b..899d8c4 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorL3 extends Command { - public ElevatorL3() { - + Elevator elevator; + + public ElevatorL3(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.L3); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java index a976fb2..5c3fcdb 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorL4 extends Command { - public ElevatorL4() { - + Elevator elevator; + + public ElevatorL4(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.L4); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java b/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java index d0a1b54..5a5dd42 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java @@ -1,10 +1,15 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorNet extends Command { - public ElevatorNet() { - + Elevator elevator; + + public ElevatorNet(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +18,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.NET); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java b/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java index 62f8d4b..c776fb2 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java @@ -1,10 +1,14 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; public class ElevatorProcessor extends Command { - public ElevatorProcessor() { - + private final Elevator elevator; + public ElevatorProcessor(Elevator elevator) { + addRequirements(this.elevator = elevator); } @Override public void initialize() { @@ -13,7 +17,7 @@ public void initialize() { @Override public void execute() { - + elevator.setGoal(ElevatorPos.PROCESSOR); } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java b/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java index cbe3fde..be55a57 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java @@ -1,7 +1,31 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import org.carlmontrobotics.subsystems.Elevator; + import edu.wpi.first.wpilibj2.command.Command; public class ElevatorUpperAlgaeRemoval extends Command { + Elevator elevator; + public ElevatorUpperAlgaeRemoval(Elevator elevator) { + addRequirements(this.elevator = elevator); + } + @Override + public void initialize() { + + } + + @Override + public void execute() { + elevator.setGoal(ElevatorPos.UPPERALGAE); + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 8567f82..ac7079b 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -5,6 +5,7 @@ package org.carlmontrobotics.subsystems; import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; @@ -69,6 +70,10 @@ private void configureMotors () { public void setGoal(double goal) { heightGoal = goal; } + + public void setGoal(ElevatorPos goal) { + heightGoal = goal.getPosition(); + } public double getGoal() { return heightGoal; 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 038/153] 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 039/153] 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 040/153] 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 041/153] 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 042/153] 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 043/153] 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 044/153] 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 045/153] 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 046/153] 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 047/153] 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 048/153] 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 049/153] 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 fd16a132c19e90fcb6eb4584a4aeb47e76d60438 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Fri, 31 Jan 2025 19:23:58 -0800 Subject: [PATCH 050/153] added limit switch annd fixed alex's issues --- .../java/org/carlmontrobotics/Constants.java | 21 +++++++++++-------- .../org/carlmontrobotics/RobotContainer.java | 21 ++++++++++--------- .../commands/IntakeCoral.java | 9 ++++---- .../commands/OuttakeCoral.java | 6 +++--- .../subsystems/CoralEffector.java | 21 +++++++------------ 5 files changed, 38 insertions(+), 40 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 27bdb81..7e3ab54 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -7,20 +7,24 @@ public final class Constants { // public static final double MAX_SPEED_MPS = 2; // } public static final class OI { + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + public static final class Driver { - public static final int port = 0; - public static final int A = 1; + public static final int driverPort = 0; + /*public static final int A = 1; public static final int B = 2; public static final int X = 3; - public static final int Y = 4; + public static final int Y = 4;*/ + //Not neccesary public static final int leftBumper = 5; public static final int rightBumper = 6; } public static final class Manipulator { - public static final int port = 1; - public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + public static final int manipulatorPort = 5; + //public static final int X = 0; + public static final Axis OuttakeTrigger = Axis.kRightTrigger; + public static final Axis IntakeTrigger = Axis.kLeftTrigger; } - public static final int X = 0; } public static final class AlgaeEffectorc { public static final int upperMotorID = 1; @@ -29,12 +33,11 @@ public static final class AlgaeEffectorc { public static final class CoralEffectorc { public static final int effectorMotorID = 3; public static final int effectorDistanceSensorID = 4; - public static final int ManipulatorControllerPort = 5; - public static final Axis OuttakeTrigger = Axis.kRightTrigger; - public static final Axis IntakeTrigger = Axis.kLeftTrigger; public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; + public static double intakeRPM = 1000; + public static double outtakeRPM = 2100; //TODO: Change after testing public static final int DETECT_DISTANCE_INCHES = 3; } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 025793a..a411569 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -7,8 +7,9 @@ //199 files // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; -import static org.carlmontrobotics.Constants.OI; +import static org.carlmontrobotics.Constants.OI.*; +import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.RunAlgae; @@ -40,8 +41,8 @@ public class RobotContainer { //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.Driver.port); + public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); + public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); private final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { @@ -59,19 +60,19 @@ private void setDefaultCommands() { // )); } private void setBindingsDriver() { - new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong - new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); - new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); - new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong + // new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); } private void setBindingsManipulator() { - axisTrigger(manipulatorController, OuttakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) .whileTrue(new IntakeCoral(coralEffector)); - axisTrigger(manipulatorController, IntakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) .whileTrue(new OuttakeCoral(coralEffector)); } @@ -79,7 +80,7 @@ private void setBindingsManipulator() { private Trigger axisTrigger(GenericHID controller, Axis axis) { return new Trigger(() -> Math - .abs(getStickValue(controller, axis)) > Constants.OI.Manipulator.MIN_AXIS_TRIGGER_VALUE); + .abs(getStickValue(controller, axis)) > Constants.OI.MIN_AXIS_TRIGGER_VALUE); } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java index c5a37fc..f0a8ba8 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java @@ -2,6 +2,7 @@ import static org.carlmontrobotics.Constants.CoralEffectorc.*; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; @@ -10,9 +11,6 @@ public class IntakeCoral extends Command { private final CoralEffector Coral; - int increaseSpeed; - - private double Speed = 1000; public IntakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); @@ -20,7 +18,7 @@ public IntakeCoral(CoralEffector Coral) { @Override public void initialize() { - Coral.setRPM(Speed); + Coral.setRPM(Constants.CoralEffectorc.intakeRPM); } @@ -32,6 +30,7 @@ public void execute() {} @Override public void end(boolean interrupted) { Coral.stopEffector(); + //TODO: Test different times } @Override @@ -39,6 +38,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Coral.coralDetects() == true; + return !Coral.coralDetects() && !Coral.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java index d1c037f..c23ce61 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java @@ -2,6 +2,7 @@ import static org.carlmontrobotics.Constants.CoralEffectorc.*; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.DigitalInput; @@ -11,14 +12,13 @@ public class OuttakeCoral extends Command { private final CoralEffector Coral; - int increaseSpeed; private double Speed = 2100; public OuttakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { - Coral.setRPM(Speed); + Coral.setRPM(Constants.CoralEffectorc.outtakeRPM); } @@ -37,6 +37,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Coral.coralDetects() == false; + return !Coral.coralDetects() && !Coral.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index f170aa7..ae02644 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -30,9 +30,6 @@ import edu.wpi.first.math.controller.SimpleMotorFeedforward; import static org.carlmontrobotics.Constants.CoralEffectorc.*; - - - public class CoralEffector extends SubsystemBase { private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); @@ -45,6 +42,14 @@ public class CoralEffector extends SubsystemBase { private double lastValidDistance = Double.POSITIVE_INFINITY; + public boolean coralDetects() { + return lastValidDistance < DETECT_DISTANCE_INCHES; + } + + public boolean limitDetects() { + return limitSwitch.get(); + } + public CoralEffector() { SparkFlexConfig c = new SparkFlexConfig(); c.closedLoop.pid( @@ -73,15 +78,6 @@ public void setRPM() { setRPM(2100); } - - public boolean coralDetects() { - return lastValidDistance < DETECT_DISTANCE_INCHES; - } - - public boolean limitDetects() { - return limitSwitch.get(); - } - public void updateValues() { if (effectorDistanceSensor.isRangeValid()) { if (lastValidDistance != 5.75) { @@ -92,7 +88,6 @@ public void updateValues() { } } - @Override public void periodic() { updateValues(); 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 051/153] 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 052/153] 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 f736296094e9abbd43a321ce34f4426b8582b343 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 31 Jan 2025 23:29:20 -0800 Subject: [PATCH 053/153] ClimbServo added --- src/main/java/org/carlmontrobotics/Constants.java | 1 + .../org/carlmontrobotics/subsystems/Elevator.java | 13 ++++++++++++- 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index d25a5c9..95dca99 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -18,6 +18,7 @@ public static final class Elevatorc { //ports public static final int masterPort = 19; public static final int followerPort = 20; + public static final int climbServoPort = 1; //Config public static final IdleMode masterIdleMode = IdleMode.kBrake; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index ac7079b..a2e0b49 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -18,6 +18,7 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Elevator extends SubsystemBase { @@ -31,6 +32,9 @@ public class Elevator extends SubsystemBase { private SparkMaxConfig followerConfig = new SparkMaxConfig(); private RelativeEncoder followerEncoder = followerMotor.getEncoder(); + //Servo + private Servo climbServo = new Servo(Constants.Elevatorc.climbServoPort); + //Absolute Encoder private AbsoluteEncoder primaryEncoder = masterMotor.getAbsoluteEncoder(); @@ -66,7 +70,7 @@ private void configureMotors () { followerConfig.follow(Constants.Elevatorc.masterPort, Constants.Elevatorc.followerInverted); followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } - + public void setGoal(double goal) { heightGoal = goal; } @@ -95,6 +99,13 @@ public double getEncoderValue() { return masterEncoder.getPosition(); } + public void lockClimb() { + climbServo.set(1); + } + + public void unlockClimb(){ + climbServo.set(0); + } @Override public void periodic() { getToGoal(); From c698f8d06fb6415068f4d19c727bf2b3c9bf0e6b Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 1 Feb 2025 14:15:52 -0800 Subject: [PATCH 054/153] Get height and update encoders added --- src/main/java/org/carlmontrobotics/Constants.java | 1 + .../java/org/carlmontrobotics/subsystems/Elevator.java | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 95dca99..a621ad9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -29,6 +29,7 @@ public static final class Elevatorc { public static final double followerPositionConversionFactor = 1000; public static final double masterVelocityConversionFactor = 1000; public static final double followerVelocityConversionFactor = 1000; + public static final double masterGearRatio = 1000; //PID public static final double kP = 1; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index a2e0b49..07b7b30 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -82,6 +82,14 @@ public void setGoal(ElevatorPos goal) { public double getGoal() { return heightGoal; } + + public double getCurrentHeight() { + return masterEncoder.getPosition() * Constants.Elevatorc.masterPositionConversionFactor; + } + + public void updateEncoders() { + masterEncoder.setPosition(primaryEncoder.getPosition()*Constants.Elevatorc.masterGearRatio); + } public void getToGoal() { pidElevatorController.setReference(heightGoal, ControlType.kPosition); From d657af08018c6a7c41c9d8c29c1604f0d6b9ee30 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 1 Feb 2025 14:43:26 -0800 Subject: [PATCH 055/153] Sparkflex to Sparkmax update --- .../java/org/carlmontrobotics/subsystems/CoralEffector.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 8ed97e2..75d71bf 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -8,6 +8,7 @@ import com.playingwithfusion.TimeOfFlight.RangingMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -31,7 +32,7 @@ import static org.carlmontrobotics.Constants.CoralEffectorc.*; public class CoralEffector extends SubsystemBase { - private SparkFlex effectorMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkMax effectorMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); 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 056/153] 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 057/153] 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 d69aa38ff35bd8be1bfa401507a58a0f408e8892 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Sun, 2 Feb 2025 15:36:50 -0800 Subject: [PATCH 058/153] finished algae subsystem --- .../java/org/carlmontrobotics/Constants.java | 13 +++ .../org/carlmontrobotics/RobotContainer.java | 2 +- .../{RunAlgae.java => IntakeAlgae.java} | 23 +--- .../commands/OuttakeAlgae.java | 39 +++++++ .../subsystems/AlgaeEffector.java | 101 ++++++++++++++++-- 5 files changed, 150 insertions(+), 28 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{RunAlgae.java => IntakeAlgae.java} (59%) create mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7e3ab54..c708cbd 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -6,6 +6,19 @@ public final class Constants { // public static final class Drivetrain { // public static final double MAX_SPEED_MPS = 2; // } + + public static final int top = 1; + public static final int bottom = 1; + public static final int pincher = 1; + + public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kD = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + + public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final class OI { public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index a411569..62a8e0f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -12,7 +12,7 @@ import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.commands.RunAlgae; +import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; import org.carlmontrobotics.commands.IntakeCoral; diff --git a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java similarity index 59% rename from src/main/java/org/carlmontrobotics/commands/RunAlgae.java rename to src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index e8d3c0c..64ac272 100644 --- a/src/main/java/org/carlmontrobotics/commands/RunAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -4,18 +4,15 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -public class RunAlgae extends Command { +public class IntakeAlgae extends Command { AlgaeEffector algaeEffector; boolean stop; int direction; - - - public RunAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { + public IntakeAlgae(AlgaeEffector algaeEffector) { addRequirements(this.algaeEffector = algaeEffector); this.direction = direction; - this.stop = stop; - + this.stop = stop; } @Override @@ -26,20 +23,6 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (stop) { - algaeEffector.RunAlgaeMotors(0,0); - } - else { - if (direction == 1) { - algaeEffector.RunAlgaeMotors(0.5, 0.5); - } - else if (direction == 2) { - algaeEffector.RunAlgaeMotors(-0.5, -0.5); - } - else if (direction == 3) { - algaeEffector.RunAlgaeMotors(-0.5, 0.5); - } - } } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java new file mode 100644 index 0000000..27788b2 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -0,0 +1,39 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class OuttakeAlgae extends Command { + AlgaeEffector algaeEffector; + boolean stop; + int direction; + + public OuttakeAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { + addRequirements(this.algaeEffector = algaeEffector); + this.direction = direction; + this.stop = stop; + } + + @Override + public void initialize() { + + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // 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/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 9f202c1..a9d9718 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -10,13 +10,22 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; - +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.SparkMax; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; @@ -24,6 +33,7 @@ import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; @@ -39,11 +49,88 @@ public class AlgaeEffector extends SubsystemBase { - private SparkFlex upperMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); - private SparkFlex lowerMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); - public void RunAlgaeMotors(double upperMotorSpeed, double lowerMotorSpeed) { - - upperMotor.set(upperMotorSpeed); - lowerMotor.set(lowerMotorSpeed); + private SparkFlex topMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkMax bottomMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkFlex pincherMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + + private final RelativeEncoder topEncoder = topMotor.getEncoder(); + private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); + private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); + + private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); + + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[top], kV[top], kA[top]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[bottom], kV[bottom], kA[bottom]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[pincher], kV[pincher], kA[pincher]); + //TODO: add feedforward + + DigitalInput limitSwitch = new DigitalInput(1); + + public boolean limitDetects() { + return limitSwitch.get(); + } + //-------------------------------------------------------------------------------------------- + public AlgaeEffector() { + SparkFlexConfig a = new SparkFlexConfig(); + SparkFlexConfig b = new SparkFlexConfig(); + SparkFlexConfig c = new SparkFlexConfig(); + + a.closedLoop.pid( + Constants.kP[top], + Constants.kI[top], + Constants.kD[top] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + b.closedLoop.pid( + Constants.kP[bottom], + Constants.kI[bottom], + Constants.kD[bottom] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + a.closedLoop.pid( + Constants.AlgaeEffectorc.kP[pincher], + Constants.AlgaeEffectorc.kI[pincher], + Constants.AlgaeEffectorc.kD[pincher] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + //---------------------------------------------------------------------------------------- + + public void setTopRPM(double toprpm) { + pidControllerTop.setReference(toprpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + topFeedforward.calculate(toprpm); + } + + public void setBottomRPM(double bottomrpm) { + pidControllerBottom.setReference(bottomrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + bottomFeedforward.calculate(bottomrpm); + } + + public void setPincherRPM(double pincherrpm) { + pidControllerPincher.setReference(pincherrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + pincherFeedforward.calculate(pincherrpm); + } + + public void runRPM() { + //TODO: Change RPM according to design + setTopRPM(1000); + setBottomRPM(2100); + setPincherRPM(2100); + } + + public void stopMotors() { + setTopRPM(0); + setBottomRPM(0); + setPincherRPM(0); + } + + public void setSpeed(double speed) { + topMotor.set(speed); + bottomMotor.set(speed); + pincherMotor.set(speed); } } From 1d0127a7c88b9aaf33e57732be5628936ecdbaa7 Mon Sep 17 00:00:00 2001 From: Rand0mAsianKid <144396869+Rand0mAsianKid@users.noreply.github.com> Date: Sun, 2 Feb 2025 15:41:53 -0800 Subject: [PATCH 059/153] finished up some last things --- .../subsystems/AlgaeEffector.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index a9d9718..e0239cc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -78,23 +78,23 @@ public AlgaeEffector() { SparkFlexConfig c = new SparkFlexConfig(); a.closedLoop.pid( - Constants.kP[top], - Constants.kI[top], - Constants.kD[top] + Constants.kP[Constants.top], + Constants.kI[Constants.top], + Constants.kD[Constants.top] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); b.closedLoop.pid( - Constants.kP[bottom], - Constants.kI[bottom], - Constants.kD[bottom] + Constants.kP[Constants.bottom], + Constants.kI[Constants.bottom], + Constants.kD[Constants.bottom] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); a.closedLoop.pid( - Constants.AlgaeEffectorc.kP[pincher], - Constants.AlgaeEffectorc.kI[pincher], - Constants.AlgaeEffectorc.kD[pincher] + Constants.kP[Constants.pincher], + Constants.kI[Constants.pincher], + Constants.kD[Constants.pincher] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } From 73b9766df706dd76368fdc16cfdd7bd4dfc90879 Mon Sep 17 00:00:00 2001 From: Matthew Date: Sun, 2 Feb 2025 20:55:51 -0800 Subject: [PATCH 060/153] finished algae commands without bindings --- .../java/org/carlmontrobotics/Constants.java | 9 +++++ .../commands/IntakeAlgae.java | 35 +++++++++++-------- .../commands/OuttakeAlgae.java | 19 +++++----- .../subsystems/AlgaeEffector.java | 6 ++-- 4 files changed, 42 insertions(+), 27 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index c708cbd..810137b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -42,6 +42,15 @@ public static final class Manipulator { public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; + + public static double intakeTopRPM = 1000; + public static double intakeBottomRPM = 1000; + public static double intakePincherRPM = 1000; + + public static double outtakeTopRPM = 2100; + public static double outtakeBottomRPM = 2100; + public static double outtakePincherRPM = 2100; + } public static final class CoralEffectorc { public static final int effectorMotorID = 3; diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 64ac272..9b44931 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -1,39 +1,46 @@ package org.carlmontrobotics.commands; + +import static org.carlmontrobotics.Constants.CoralEffectorc.*; + +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class IntakeAlgae extends Command { - AlgaeEffector algaeEffector; - boolean stop; - int direction; - - public IntakeAlgae(AlgaeEffector algaeEffector) { - addRequirements(this.algaeEffector = algaeEffector); - this.direction = direction; - this.stop = stop; + private final AlgaeEffector Algae; + + public IntakeAlgae(AlgaeEffector Algae) { + addRequirements(this.Algae = Algae); } @Override public void initialize() { - + Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); } + // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() { - } + public void execute() {} // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + Algae.stopMotors(); + //TODO: Test different times } - // Returns true when the command should end. @Override public boolean isFinished() { - return false; + //distance sensor doesn't detect coral + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return Algae.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 27788b2..fcee808 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -1,23 +1,22 @@ package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; public class OuttakeAlgae extends Command { - AlgaeEffector algaeEffector; - boolean stop; - int direction; + AlgaeEffector Algae; - public OuttakeAlgae(AlgaeEffector algaeEffector, int direction, boolean stop) { - addRequirements(this.algaeEffector = algaeEffector); - this.direction = direction; - this.stop = stop; + public OuttakeAlgae(AlgaeEffector algaeEffector) { + addRequirements(this.Algae = algaeEffector); } @Override public void initialize() { - + Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); } // Called every time the scheduler runs while the command is scheduled. @@ -28,12 +27,12 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - + Algae.stopMotors(); } // Returns true when the command should end. @Override public boolean isFinished() { - return false; + return Algae.limitDetects(); } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index e0239cc..273714f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -61,9 +61,9 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[top], kV[top], kA[top]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[bottom], kV[bottom], kA[bottom]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[pincher], kV[pincher], kA[pincher]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.top], Constants.kV[Constants.top], Constants.kA[Constants.top]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.bottom], Constants.kV[Constants.bottom], Constants.kA[Constants.bottom]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.pincher], Constants.kV[Constants.pincher], Constants.kA[Constants.pincher]); //TODO: add feedforward DigitalInput limitSwitch = new DigitalInput(1); From 294ee6fd12950f8d1b5f5baf9d35d8c04fb61878 Mon Sep 17 00:00:00 2001 From: Matthew Date: Sun, 2 Feb 2025 21:08:39 -0800 Subject: [PATCH 061/153] BIndings(for now) --- src/main/java/org/carlmontrobotics/Constants.java | 3 +++ src/main/java/org/carlmontrobotics/RobotContainer.java | 9 ++++++++- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 810137b..7ace265 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,6 +1,7 @@ package org.carlmontrobotics; import edu.wpi.first.wpilibj.XboxController.Axis; +import edu.wpi.first.wpilibj.XboxController.Button; public final class Constants { // public static final class Drivetrain { @@ -37,6 +38,8 @@ public static final class Manipulator { //public static final int X = 0; public static final Axis OuttakeTrigger = Axis.kRightTrigger; public static final Axis IntakeTrigger = Axis.kLeftTrigger; + public static final int OuttakeBumper = Button.kRightBumper.value; + public static final int IntakeBumper = Button.kLeftBumper.value; } } public static final class AlgaeEffectorc { diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 62a8e0f..26d4977 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -15,12 +15,14 @@ import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.subsystems.CoralEffector; +import org.carlmontrobotics.commands.IntakeAlgae; import org.carlmontrobotics.commands.IntakeCoral; import org.carlmontrobotics.commands.OuttakeCoral; //controllers import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj.XboxController.Axis; //commands @@ -74,8 +76,13 @@ private void setBindingsManipulator() { axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) .whileTrue(new OuttakeCoral(coralEffector)); + + new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) + .whileTrue(new IntakeAlgae(algaeEffector)); + + new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) + .whileTrue(new OuttakeAlgae(algaeEffector)); } - private Trigger axisTrigger(GenericHID controller, Axis axis) { 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 062/153] 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 063/153] 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 47a5fed6d1eb002e74c3c930387d2ac0f4d8e693 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 7 Feb 2025 19:03:44 -0800 Subject: [PATCH 064/153] badcode -> goodcode --- .../subsystems/AlgaeEffector.java | 18 +++++++++--------- .../subsystems/CoralEffector.java | 6 +++--- 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 273714f..769733f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -73,30 +73,30 @@ public boolean limitDetects() { } //-------------------------------------------------------------------------------------------- public AlgaeEffector() { - SparkFlexConfig a = new SparkFlexConfig(); - SparkFlexConfig b = new SparkFlexConfig(); - SparkFlexConfig c = new SparkFlexConfig(); + SparkFlexConfig topConfig = new SparkFlexConfig(); + SparkFlexConfig bottomConfig = new SparkFlexConfig(); + SparkFlexConfig pincherConfig = new SparkFlexConfig(); - a.closedLoop.pid( + topConfig.closedLoop.pid( Constants.kP[Constants.top], Constants.kI[Constants.top], Constants.kD[Constants.top] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + topMotor.configure(topConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - b.closedLoop.pid( + bottomConfig.closedLoop.pid( Constants.kP[Constants.bottom], Constants.kI[Constants.bottom], Constants.kD[Constants.bottom] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + bottomMotor.configure(bottomConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - a.closedLoop.pid( + pincherConfig.closedLoop.pid( Constants.kP[Constants.pincher], Constants.kI[Constants.pincher], Constants.kD[Constants.pincher] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + pincherMotor.configure(pincherConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } //---------------------------------------------------------------------------------------- diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java index 75d71bf..ba500c9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java @@ -52,13 +52,13 @@ public boolean limitDetects() { } public CoralEffector() { - SparkFlexConfig c = new SparkFlexConfig(); - c.closedLoop.pid( + SparkFlexConfig effectorConfig = new SparkFlexConfig(); + effectorConfig.closedLoop.pid( Constants.CoralEffectorc.kP, Constants.CoralEffectorc.kI, Constants.CoralEffectorc.kD ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - effectorMotor.configure(c, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + effectorMotor.configure(effectorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); //Will add distance sensor later } From dde5ab25132b5ed342988e303b69d7adb572bf5e Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 7 Feb 2025 19:46:41 -0800 Subject: [PATCH 065/153] Aiken fixed a bunch of prs --- .vscode/settings.json | 3 + .wpilib/wpilib_preferences.json | 6 ++ networktables.json | 1 + simgui-ds.json | 98 +++++++++++++++++++ simgui.json | 19 ++++ .../java/org/carlmontrobotics/Constants.java | 16 +-- .../org/carlmontrobotics/RobotContainer.java | 4 +- .../commands/IntakeCoral.java | 9 +- .../commands/OuttakeAlgae.java | 5 +- .../commands/OuttakeCoral.java | 5 +- .../subsystems/AlgaeEffector.java | 50 +++++----- 11 files changed, 175 insertions(+), 41 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 .wpilib/wpilib_preferences.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..c5f3f6b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "java.configuration.updateBuildConfiguration": "interactive" +} \ No newline at end of file diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json new file mode 100644 index 0000000..f0ee648 --- /dev/null +++ b/.wpilib/wpilib_preferences.json @@ -0,0 +1,6 @@ +{ + "currentLanguage": "none", + "enableCppIntellisense": false, + "projectYear": "none", + "teamNumber": 199 +} \ No newline at end of file diff --git a/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..c4b7efd --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,98 @@ +{ + "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 + } + ], + "robotJoysticks": [ + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..4630a5c --- /dev/null +++ b/simgui.json @@ -0,0 +1,19 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK Flex [1] RELATIVE ENCODER": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo" + } + }, + "NetworkTables Info": { + "visible": true + } +} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7ace265..b05fb75 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -8,9 +8,7 @@ public final class Constants { // public static final double MAX_SPEED_MPS = 2; // } - public static final int top = 1; - public static final int bottom = 1; - public static final int pincher = 1; + public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; @@ -34,7 +32,7 @@ public static final class Driver { public static final int rightBumper = 6; } public static final class Manipulator { - public static final int manipulatorPort = 5; + public static final int manipulatorPort = 2; //public static final int X = 0; public static final Axis OuttakeTrigger = Axis.kRightTrigger; public static final Axis IntakeTrigger = Axis.kLeftTrigger; @@ -45,6 +43,10 @@ public static final class Manipulator { public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; + public static final int pinchMotorID = 3; + public static final int TopkS = 1; + public static final int BottomkS = 1; + public static final int PincherkS = 1; public static double intakeTopRPM = 1000; public static double intakeBottomRPM = 1000; @@ -56,12 +58,12 @@ public static final class AlgaeEffectorc { } public static final class CoralEffectorc { - public static final int effectorMotorID = 3; - public static final int effectorDistanceSensorID = 4; + public static final int effectorMotorID = 4; + public static final int effectorDistanceSensorID = 5; public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; - public static double intakeRPM = 1000; + public static double intakeRPM = -1000; public static double outtakeRPM = 2100; //TODO: Change after testing public static final int DETECT_DISTANCE_INCHES = 3; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 26d4977..8583810 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -71,10 +71,10 @@ private void setBindingsDriver() { private void setBindingsManipulator() { - axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) .whileTrue(new IntakeCoral(coralEffector)); - axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) + axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) .whileTrue(new OuttakeCoral(coralEffector)); new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java index f0a8ba8..980387f 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java @@ -4,21 +4,22 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.CoralEffector; - import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class IntakeCoral extends Command { private final CoralEffector Coral; - + private final Timer timer = new Timer(); public IntakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { - Coral.setRPM(Constants.CoralEffectorc.intakeRPM); + timer.reset(); + timer.start(); + Coral.setRPM(Constants.CoralEffectorc.intakeRPM); } @@ -38,6 +39,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return !Coral.coralDetects() && !Coral.limitDetects(); + return (Coral.coralDetects() && Coral.limitDetects()) || timer.get() > 4; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index fcee808..f510d56 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -7,7 +7,7 @@ public class OuttakeAlgae extends Command { AlgaeEffector Algae; - + private final Timer timer = new Timer(); public OuttakeAlgae(AlgaeEffector algaeEffector) { addRequirements(this.Algae = algaeEffector); } @@ -17,6 +17,7 @@ public void initialize() { Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); + timer.start(); } // Called every time the scheduler runs while the command is scheduled. @@ -33,6 +34,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Algae.limitDetects(); + return Algae.limitDetects() || timer.get() > 1; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java index c23ce61..40436ed 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java @@ -12,12 +12,15 @@ public class OuttakeCoral extends Command { private final CoralEffector Coral; + private final Timer timer = new Timer(); public OuttakeCoral(CoralEffector Coral) { addRequirements(this.Coral = Coral); } @Override public void initialize() { + timer.reset(); + timer.start(); Coral.setRPM(Constants.CoralEffectorc.outtakeRPM); } @@ -37,6 +40,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return !Coral.coralDetects() && !Coral.limitDetects(); + return (!Coral.coralDetects() && !Coral.limitDetects()) || timer.get() > 4; } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 273714f..e0d97b3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -49,9 +49,9 @@ public class AlgaeEffector extends SubsystemBase { - private SparkFlex topMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkMax bottomMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkFlex pincherMotor = new SparkFlex(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkFlex topMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkMax bottomMotor = new SparkMax(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? private final RelativeEncoder topEncoder = topMotor.getEncoder(); private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); @@ -61,9 +61,9 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.top], Constants.kV[Constants.top], Constants.kA[Constants.top]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.bottom], Constants.kV[Constants.bottom], Constants.kA[Constants.bottom]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.pincher], Constants.kV[Constants.pincher], Constants.kA[Constants.pincher]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopkS], Constants.kV[Constants.AlgaeEffectorc.TopkS], Constants.kA[Constants.AlgaeEffectorc.TopkS]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomkS], Constants.kV[Constants.AlgaeEffectorc.BottomkS], Constants.kA[Constants.AlgaeEffectorc.BottomkS]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.PincherkS], Constants.kV[Constants.AlgaeEffectorc.PincherkS], Constants.kA[Constants.AlgaeEffectorc.PincherkS]); //TODO: add feedforward DigitalInput limitSwitch = new DigitalInput(1); @@ -73,30 +73,30 @@ public boolean limitDetects() { } //-------------------------------------------------------------------------------------------- public AlgaeEffector() { - SparkFlexConfig a = new SparkFlexConfig(); - SparkFlexConfig b = new SparkFlexConfig(); - SparkFlexConfig c = new SparkFlexConfig(); - - a.closedLoop.pid( - Constants.kP[Constants.top], - Constants.kI[Constants.top], - Constants.kD[Constants.top] + SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); + SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); + + + pincherMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.TopkS], + Constants.kI[Constants.AlgaeEffectorc.TopkS], + Constants.kD[Constants.AlgaeEffectorc.TopkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + topMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - b.closedLoop.pid( - Constants.kP[Constants.bottom], - Constants.kI[Constants.bottom], - Constants.kD[Constants.bottom] + bottomMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.BottomkS], + Constants.kI[Constants.AlgaeEffectorc.BottomkS], + Constants.kD[Constants.AlgaeEffectorc.BottomkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - bottomMotor.configure(b, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - a.closedLoop.pid( - Constants.kP[Constants.pincher], - Constants.kI[Constants.pincher], - Constants.kD[Constants.pincher] + pincherMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.PincherkS], + Constants.kI[Constants.AlgaeEffectorc.PincherkS], + Constants.kD[Constants.AlgaeEffectorc.PincherkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - pincherMotor.configure(a, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); } //---------------------------------------------------------------------------------------- From e2ae207ef5c0eeef2a0077c887cc0867f5d8dfd3 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Fri, 7 Feb 2025 20:16:30 -0800 Subject: [PATCH 066/153] =?UTF-8?q?Fixed=20some=20sim=20stuff=20found=20su?= =?UTF-8?q?m=20out=20Matthew=20is=20a=20sour=20trout=F0=9F=90=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/org/carlmontrobotics/RobotContainer.java | 2 +- .../java/org/carlmontrobotics/commands/IntakeAlgae.java | 6 ++++-- .../java/org/carlmontrobotics/commands/OuttakeAlgae.java | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 8583810..e39e162 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -81,7 +81,7 @@ private void setBindingsManipulator() { .whileTrue(new IntakeAlgae(algaeEffector)); new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) - .whileTrue(new OuttakeAlgae(algaeEffector)); + .whileFalse(new OuttakeAlgae(algaeEffector)); } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 9b44931..ee38226 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -12,13 +12,15 @@ public class IntakeAlgae extends Command { private final AlgaeEffector Algae; - + private final Timer timer = new Timer(); public IntakeAlgae(AlgaeEffector Algae) { addRequirements(this.Algae = Algae); } @Override public void initialize() { + timer.reset(); + timer.start(); Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); @@ -41,6 +43,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Algae.limitDetects(); + return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index f510d56..10d885e 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -34,6 +34,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Algae.limitDetects() || timer.get() > 1; + return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file From ca3859c6899bcc9bab5d252e9df1ecf481eb0609 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 8 Feb 2025 11:14:25 -0800 Subject: [PATCH 067/153] Corrected configs and added methods to check if shooting motors are up to speed --- .../subsystems/AlgaeEffector.java | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index e0d97b3..75d4970 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -20,6 +20,7 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; import edu.wpi.first.hal.SimDouble; @@ -74,15 +75,16 @@ public boolean limitDetects() { //-------------------------------------------------------------------------------------------- public AlgaeEffector() { SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); - SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); + SparkMaxConfig bottomMotorConfig = new SparkMaxConfig(); + SparkFlexConfig topMotorConfig = new SparkFlexConfig(); - pincherMotorConfig.closedLoop.pid( + topMotorConfig.closedLoop.pid( Constants.kP[Constants.AlgaeEffectorc.TopkS], Constants.kI[Constants.AlgaeEffectorc.TopkS], Constants.kD[Constants.AlgaeEffectorc.TopkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - topMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); bottomMotorConfig.closedLoop.pid( Constants.kP[Constants.AlgaeEffectorc.BottomkS], @@ -128,6 +130,14 @@ public void stopMotors() { setPincherRPM(0); } + public boolean checkIfAtTopRPM(double rpm) { + return topEncoder.getVelocity() == rpm; + } + + public boolean checkIfAtBottomRPM() { + return bottomEncoder.getVelocity() == rpm; + } + public void setSpeed(double speed) { topMotor.set(speed); bottomMotor.set(speed); From ca9dabc12bf2255eafb048734e47535c3e867d4b Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 8 Feb 2025 11:19:11 -0800 Subject: [PATCH 068/153] forgor to add --- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 75d4970..89ae98a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -134,7 +134,7 @@ public boolean checkIfAtTopRPM(double rpm) { return topEncoder.getVelocity() == rpm; } - public boolean checkIfAtBottomRPM() { + public boolean checkIfAtBottomRPM(double rpm) { return bottomEncoder.getVelocity() == rpm; } From 908d91e85723e003ea39a80daddaa13e3644623f Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 9 Feb 2025 11:40:50 -0800 Subject: [PATCH 069/153] project year --- .wpilib/wpilib_preferences.json | 2 +- src/main/java/org/carlmontrobotics/Constants.java | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/.wpilib/wpilib_preferences.json b/.wpilib/wpilib_preferences.json index f0ee648..96c51f6 100644 --- a/.wpilib/wpilib_preferences.json +++ b/.wpilib/wpilib_preferences.json @@ -1,6 +1,6 @@ { "currentLanguage": "none", "enableCppIntellisense": false, - "projectYear": "none", + "projectYear": "2025", "teamNumber": 199 } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b05fb75..854a5fc 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -23,6 +23,8 @@ public static final class OI { public static final class Driver { public static final int driverPort = 0; + public static final int effectorMotorID = 4; + public static final int effectorDistanceSensorID = 5; /*public static final int A = 1; public static final int B = 2; public static final int X = 3; @@ -58,8 +60,7 @@ public static final class AlgaeEffectorc { } public static final class CoralEffectorc { - public static final int effectorMotorID = 4; - public static final int effectorDistanceSensorID = 5; + public static final double kP = 0.0; public static final double kI = 0.0; public static final double kD = 0.0; 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 070/153] 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 071/153] 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 97afeb5e88361dc7d97d200fc7cde99c4938768f Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Tue, 11 Feb 2025 20:15:28 -0800 Subject: [PATCH 072/153] Add constants --- src/main/java/org/carlmontrobotics/Constants.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 854a5fc..cd69add 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -68,5 +68,7 @@ public static final class CoralEffectorc { public static double outtakeRPM = 2100; //TODO: Change after testing public static final int DETECT_DISTANCE_INCHES = 3; + public static final int effectorMotorID = 0; + public static final int effectorDistanceSensorID = 0; } } 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 073/153] 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 074/153] 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 075/153] 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 076/153] 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 077/153] 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 078/153] 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 079/153] 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 080/153] 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 081/153] 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 082/153] 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 083/153] 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 084/153] 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 085/153] 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 086/153] 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 9c428802bbba514955d702ef37e0b8fa5e00324e Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 20 Feb 2025 10:12:36 -0800 Subject: [PATCH 087/153] Updated Changes based off new design --- .../java/org/carlmontrobotics/Constants.java | 6 ++- .../carlmontrobotics/subsystems/Elevator.java | 54 +++++++++++-------- 2 files changed, 35 insertions(+), 25 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index a621ad9..2501464 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -18,9 +18,10 @@ public static final class Elevatorc { //ports public static final int masterPort = 19; public static final int followerPort = 20; - public static final int climbServoPort = 1; + public static final int elevatorLimitSwitchPort = 1; //Config + //TODO figure these parts out public static final IdleMode masterIdleMode = IdleMode.kBrake; public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = true; @@ -29,7 +30,8 @@ public static final class Elevatorc { public static final double followerPositionConversionFactor = 1000; public static final double masterVelocityConversionFactor = 1000; public static final double followerVelocityConversionFactor = 1000; - public static final double masterGearRatio = 1000; + public static final double maxElevatorHeightInches = 0; + public static final double minElevatorHeightInches = 0; //PID public static final double kP = 1; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 07b7b30..013adfe 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -18,33 +18,43 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ //Master - private SparkMax masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); - private SparkMaxConfig masterConfig = new SparkMaxConfig(); - private RelativeEncoder masterEncoder = masterMotor.getEncoder(); + private SparkMax masterMotor; + private SparkMaxConfig masterConfig; + private RelativeEncoder masterEncoder; //Follower - private SparkMax followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); - private SparkMaxConfig followerConfig = new SparkMaxConfig(); - private RelativeEncoder followerEncoder = followerMotor.getEncoder(); - - //Servo - private Servo climbServo = new Servo(Constants.Elevatorc.climbServoPort); - - //Absolute Encoder - private AbsoluteEncoder primaryEncoder = masterMotor.getAbsoluteEncoder(); - + private SparkMax followerMotor; + private SparkMaxConfig followerConfig; + private RelativeEncoder followerEncoder; + // Caliberation + private DigitalInput limitSwitch; //Vars private double heightGoal; //PID - private final SparkClosedLoopController pidElevatorController = masterMotor.getClosedLoopController(); + private SparkClosedLoopController pidElevatorController; public Elevator() { + //motors + masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); + masterConfig = new SparkMaxConfig(); + masterEncoder = masterMotor.getEncoder(); + followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); + followerConfig = new SparkMaxConfig(); + followerEncoder = followerMotor.getEncoder(); configureMotors(); + + //Calibration + limitSwitch = new DigitalInput(Constants.Elevatorc.elevatorLimitSwitchPort); + + //PID + pidElevatorController = masterMotor.getClosedLoopController(); } private void configureMotors () { @@ -84,11 +94,13 @@ public double getGoal() { } public double getCurrentHeight() { - return masterEncoder.getPosition() * Constants.Elevatorc.masterPositionConversionFactor; + return masterEncoder.getPosition(); } public void updateEncoders() { - masterEncoder.setPosition(primaryEncoder.getPosition()*Constants.Elevatorc.masterGearRatio); + if (!limitSwitch.get()) { + masterEncoder.setPosition(Constants.Elevatorc.maxElevatorHeightInches); + } } public void getToGoal() { @@ -107,15 +119,11 @@ public double getEncoderValue() { return masterEncoder.getPosition(); } - public void lockClimb() { - climbServo.set(1); - } - - public void unlockClimb(){ - climbServo.set(0); - } @Override public void periodic() { + SmartDashboard.putBoolean("MaxHeight", limitSwitch.get()); + SmartDashboard.putNumber("Elevator Height", getCurrentHeight()); + updateEncoders(); getToGoal(); } } From dba8187fa72949c2c417226208ccd0e5f243af03 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 20 Feb 2025 13:43:39 -0800 Subject: [PATCH 088/153] Created a seperate branch for algae and added arm stuff --- .../java/org/carlmontrobotics/Constants.java | 13 +-- .../org/carlmontrobotics/RobotContainer.java | 13 +-- .../commands/IntakeCoral.java | 44 --------- .../commands/OuttakeCoral.java | 45 --------- .../subsystems/AlgaeEffector.java | 22 ++++- .../subsystems/CoralEffector.java | 97 ------------------- 6 files changed, 21 insertions(+), 213 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/IntakeCoral.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java delete mode 100644 src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index cd69add..b8b79c6 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -46,6 +46,7 @@ public static final class AlgaeEffectorc { public static final int upperMotorID = 1; public static final int lowerMotorID = 2; public static final int pinchMotorID = 3; + public static final int armMotorID = 4; public static final int TopkS = 1; public static final int BottomkS = 1; public static final int PincherkS = 1; @@ -58,17 +59,5 @@ public static final class AlgaeEffectorc { public static double outtakeBottomRPM = 2100; public static double outtakePincherRPM = 2100; - } - public static final class CoralEffectorc { - - public static final double kP = 0.0; - public static final double kI = 0.0; - public static final double kD = 0.0; - public static double intakeRPM = -1000; - public static double outtakeRPM = 2100; - //TODO: Change after testing - public static final int DETECT_DISTANCE_INCHES = 3; - public static final int effectorMotorID = 0; - public static final int effectorDistanceSensorID = 0; } } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index e39e162..b46e8d3 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -14,10 +14,9 @@ import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; -import org.carlmontrobotics.subsystems.CoralEffector; + import org.carlmontrobotics.commands.IntakeAlgae; -import org.carlmontrobotics.commands.IntakeCoral; -import org.carlmontrobotics.commands.OuttakeCoral; + //controllers @@ -34,7 +33,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; //control bindings -import static org.carlmontrobotics.Constants.CoralEffectorc.*; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -46,7 +44,6 @@ public class RobotContainer { public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); public final GenericHID manipulatorController = new GenericHID(OI.Manipulator.manipulatorPort); private final AlgaeEffector algaeEffector = new AlgaeEffector(); - private final CoralEffector coralEffector = new CoralEffector(); public RobotContainer() { setBindingsDriver(); setBindingsManipulator(); @@ -71,12 +68,6 @@ private void setBindingsDriver() { private void setBindingsManipulator() { - axisTrigger(manipulatorController, OI.Manipulator.IntakeTrigger) - .whileTrue(new IntakeCoral(coralEffector)); - - axisTrigger(manipulatorController, OI.Manipulator.OuttakeTrigger) - .whileTrue(new OuttakeCoral(coralEffector)); - new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) .whileTrue(new IntakeAlgae(algaeEffector)); diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java b/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java deleted file mode 100644 index 980387f..0000000 --- a/src/main/java/org/carlmontrobotics/commands/IntakeCoral.java +++ /dev/null @@ -1,44 +0,0 @@ -package org.carlmontrobotics.commands; - -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.CoralEffector; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class IntakeCoral extends Command { - private final CoralEffector Coral; - private final Timer timer = new Timer(); - public IntakeCoral(CoralEffector Coral) { - addRequirements(this.Coral = Coral); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - Coral.setRPM(Constants.CoralEffectorc.intakeRPM); - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Coral.stopEffector(); - //TODO: Test different times - } - - @Override - public boolean isFinished() { - //distance sensor doesn't detect coral - //TODO: make distance sensor stuff - //TODO: add smartdashboard - return (Coral.coralDetects() && Coral.limitDetects()) || timer.get() > 4; - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java b/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java deleted file mode 100644 index 40436ed..0000000 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeCoral.java +++ /dev/null @@ -1,45 +0,0 @@ -package org.carlmontrobotics.commands; - -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.CoralEffector; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class OuttakeCoral extends Command { - private final CoralEffector Coral; - private final Timer timer = new Timer(); - public OuttakeCoral(CoralEffector Coral) { - addRequirements(this.Coral = Coral); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - Coral.setRPM(Constants.CoralEffectorc.outtakeRPM); - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() {} - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Coral.stopEffector(); - } - - @Override - public boolean isFinished() { - //distance sensor doesn't detect coral - //TODO: make distance sensor stuff - //TODO: add smartdashboard - return (!Coral.coralDetects() && !Coral.limitDetects()) || timer.get() > 4; - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 89ae98a..6a0029a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -7,11 +7,15 @@ import static org.carlmontrobotics.RobotContainer.*; +import java.beans.Encoder; + import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -19,6 +23,7 @@ import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; @@ -50,17 +55,22 @@ public class AlgaeEffector extends SubsystemBase { - private SparkFlex topMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkMax bottomMotor = new SparkMax(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? + private final SparkFlex topMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); + private final SparkFlex bottomMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); + private final SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); + private final SparkMax armMotor = new SparkMax(Constants.AlgaeEffectorc.armMotorID, MotorType.kBrushless); + private final RelativeEncoder topEncoder = topMotor.getEncoder(); private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); + private final RelativeEncoder armEncoder = armMotor.getEncoder(); + private final Encoder armAbsoluteEncoder = new Encoder(); private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); + private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopkS], Constants.kV[Constants.AlgaeEffectorc.TopkS], Constants.kA[Constants.AlgaeEffectorc.TopkS]); private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomkS], Constants.kV[Constants.AlgaeEffectorc.BottomkS], Constants.kA[Constants.AlgaeEffectorc.BottomkS]); @@ -75,8 +85,9 @@ public boolean limitDetects() { //-------------------------------------------------------------------------------------------- public AlgaeEffector() { SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); - SparkMaxConfig bottomMotorConfig = new SparkMaxConfig(); + SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); SparkFlexConfig topMotorConfig = new SparkFlexConfig(); + SparkMaxConfig armMotorConfig = new SparkMaxConfig(); topMotorConfig.closedLoop.pid( @@ -99,6 +110,9 @@ public AlgaeEffector() { Constants.kD[Constants.AlgaeEffectorc.PincherkS] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + + armMotorConfig.idleMode(IdleMode.kBrake); + armMotorConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); } //---------------------------------------------------------------------------------------- diff --git a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java b/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java deleted file mode 100644 index ba500c9..0000000 --- a/src/main/java/org/carlmontrobotics/subsystems/CoralEffector.java +++ /dev/null @@ -1,97 +0,0 @@ -package org.carlmontrobotics.subsystems; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; - -import com.playingwithfusion.TimeOfFlight; -import com.playingwithfusion.TimeOfFlight.RangingMode; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkFlex; -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.ClosedLoopSlot; -import com.revrobotics.spark.SparkClosedLoopController; - -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.util.datalog.DataLogEntry; -import edu.wpi.first.util.datalog.StringLogEntry; -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; -import com.revrobotics.spark.SparkFlex; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - -public class CoralEffector extends SubsystemBase { - private SparkMax effectorMotor = new SparkMax(Constants.CoralEffectorc.effectorMotorID, MotorType.kBrushless); //why is there a vortex motor on coral effectors? - private final RelativeEncoder effectorEncoder = effectorMotor.getEncoder(); - private final SparkClosedLoopController pidControllerEffector = effectorMotor.getClosedLoopController(); - // private final SimpleMotorFeedforward effectorFeedforward = new SimpleMotorFeedforward(kS[OUTTAKE], kV[OUTTAKE], kA[OUTTAKE]); - private double goalOutakeRPM = effectorEncoder.getVelocity(); - DigitalInput limitSwitch = new DigitalInput(0); //TODO: change channel after wired - private Timer effectorTOFTimer = new Timer(); - private TimeOfFlight effectorDistanceSensor = new TimeOfFlight(Constants.CoralEffectorc.effectorDistanceSensorID); - - private double lastValidDistance = Double.POSITIVE_INFINITY; - - public boolean coralDetects() { - return lastValidDistance < DETECT_DISTANCE_INCHES; - } - - public boolean limitDetects() { - return limitSwitch.get(); - } - - public CoralEffector() { - SparkFlexConfig effectorConfig = new SparkFlexConfig(); - effectorConfig.closedLoop.pid( - Constants.CoralEffectorc.kP, - Constants.CoralEffectorc.kI, - Constants.CoralEffectorc.kD - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - effectorMotor.configure(effectorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - //Will add distance sensor later - - } - - public void setSpeed(double speed) { - effectorMotor.set(speed); - } - - public void setRPM(double rpm) { - pidControllerEffector.setReference(rpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - } - - public void stopEffector() { - setRPM(0); - } - - public void setRPM() { - setRPM(2100); - } - - public void updateValues() { - if (effectorDistanceSensor.isRangeValid()) { - if (lastValidDistance != 5.75) { - effectorTOFTimer.reset(); - } else - effectorTOFTimer.start(); - lastValidDistance = Units.metersToInches(effectorDistanceSensor.getRange()); - } - } - - @Override - public void periodic() { - updateValues(); - limitDetects(); - } -} From 80852e57c058171a7835f2165e07e6bcac0b78d0 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 20 Feb 2025 13:44:05 -0800 Subject: [PATCH 089/153] Fix errors --- src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index ee38226..6c47d47 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -1,10 +1,7 @@ package org.carlmontrobotics.commands; -import static org.carlmontrobotics.Constants.CoralEffectorc.*; - import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; -import org.carlmontrobotics.subsystems.CoralEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; From 400b2b01b0799ce749391235b5048242efd6a323 Mon Sep 17 00:00:00 2001 From: AikenB Date: Thu, 20 Feb 2025 16:48:40 -0800 Subject: [PATCH 090/153] modified variables in constants and set up commands and functions for algae arm --- .../java/org/carlmontrobotics/Constants.java | 21 +++++---- .../commands/SetArmToAngle.java | 40 ++++++++++++++++ .../subsystems/AlgaeEffector.java | 46 +++++++++++++------ 3 files changed, 85 insertions(+), 22 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b8b79c6..c43b55f 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -10,13 +10,13 @@ public final class Constants { - public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; - public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; - public static final double[] kD = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; + public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; + public static final double[] kD = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; - public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; - public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0}; + public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; + public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; + public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; public static final class OI { public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; @@ -47,9 +47,12 @@ public static final class AlgaeEffectorc { public static final int lowerMotorID = 2; public static final int pinchMotorID = 3; public static final int armMotorID = 4; - public static final int TopkS = 1; - public static final int BottomkS = 1; - public static final int PincherkS = 1; + + public static final int TopArrayOrder = 0; + public static final int BottomArrayOrder = 1; + public static final int PincherArrayOrder = 2; + public static final int ArmArrayOrder = 3; + //the ArrayOrder variables replaced the ones for the kS since they just indicate the order and are the same for all PID values public static double intakeTopRPM = 1000; public static double intakeBottomRPM = 1000; diff --git a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java new file mode 100644 index 0000000..aa5ad7b --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java @@ -0,0 +1,40 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class setArmAngle extends Command { + AlgaeEffector Algae; + double angle; + private final Timer timer = new Timer(); + public SetArmToAngle(AlgaeEffector algaeEffector, double angle ) { + addRequirements(this.Algae = algaeEffector); + this.angle = angle; + } + + @Override + public void initialize() { + AlgaeEffector.setArmAngle(angle); + + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Algae.stopMotors(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 6a0029a..2053e43 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -72,11 +72,13 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopkS], Constants.kV[Constants.AlgaeEffectorc.TopkS], Constants.kA[Constants.AlgaeEffectorc.TopkS]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomkS], Constants.kV[Constants.AlgaeEffectorc.BottomkS], Constants.kA[Constants.AlgaeEffectorc.BottomkS]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.PincherkS], Constants.kV[Constants.AlgaeEffectorc.PincherkS], Constants.kA[Constants.AlgaeEffectorc.PincherkS]); - //TODO: add feedforward + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopArrayOrder], Constants.kV[Constants.AlgaeEffectorc.TopArrayOrder], Constants.kA[Constants.AlgaeEffectorc.TopArrayOrder]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomArrayOrder], Constants.kV[Constants.AlgaeEffectorc.BottomArrayOrder], Constants.kA[Constants.AlgaeEffectorc.BottomArrayOrder]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.PincherArrayOrder], Constants.kV[Constants.AlgaeEffectorc.PincherArrayOrder], Constants.kA[Constants.AlgaeEffectorc.PincherArrayOrder]); + private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.ArmArrayOrder], Constants.kV[Constants.AlgaeEffectorc.ArmArrayOrder], Constants.kA[Constants.AlgaeEffectorc.ArmArrayOrder]); + //feedforward for arm was added + private final double GoalAngle=0; DigitalInput limitSwitch = new DigitalInput(1); public boolean limitDetects() { @@ -91,26 +93,32 @@ public AlgaeEffector() { topMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.TopkS], - Constants.kI[Constants.AlgaeEffectorc.TopkS], - Constants.kD[Constants.AlgaeEffectorc.TopkS] + Constants.kP[Constants.AlgaeEffectorc.TopArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.TopArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.TopArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); bottomMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.BottomkS], - Constants.kI[Constants.AlgaeEffectorc.BottomkS], - Constants.kD[Constants.AlgaeEffectorc.BottomkS] + Constants.kP[Constants.AlgaeEffectorc.BottomArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.BottomArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.BottomArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); pincherMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.PincherkS], - Constants.kI[Constants.AlgaeEffectorc.PincherkS], - Constants.kD[Constants.AlgaeEffectorc.PincherkS] + Constants.kP[Constants.AlgaeEffectorc.PincherArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.PincherArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.PincherArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + armMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.armArrayOrder, + Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.idleMode(IdleMode.kBrake); armMotorConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); } @@ -131,6 +139,10 @@ public void setPincherRPM(double pincherrpm) { pincherFeedforward.calculate(pincherrpm); } + public void setArmAngle(double armangle) { + GoalAngle = armangle; + } + public void runRPM() { //TODO: Change RPM according to design setTopRPM(1000); @@ -157,4 +169,12 @@ public void setSpeed(double speed) { bottomMotor.set(speed); pincherMotor.set(speed); } + + @override + + public void periodic() { + pidControllerArm.setReference(goalAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); + pincherFeedforward.calculate(goalAngle); + } + } From 1051602ed1d3b38ac978acf3a53d667bb5a86048 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 20 Feb 2025 21:08:45 -0800 Subject: [PATCH 091/153] Few mini fixes --- .../java/org/carlmontrobotics/Constants.java | 8 +++--- .../subsystems/AlgaeEffector.java | 26 ++++++++----------- 2 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index c43b55f..ad9f4f2 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -48,10 +48,10 @@ public static final class AlgaeEffectorc { public static final int pinchMotorID = 3; public static final int armMotorID = 4; - public static final int TopArrayOrder = 0; - public static final int BottomArrayOrder = 1; - public static final int PincherArrayOrder = 2; - public static final int ArmArrayOrder = 3; + public static final int topArrayOrder = 0; + public static final int bottomArrayOrder = 1; + public static final int pincherArrayOrder = 2; + public static final int armArrayOrder = 3; //the ArrayOrder variables replaced the ones for the kS since they just indicate the order and are the same for all PID values public static double intakeTopRPM = 1000; diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 2053e43..c5639b7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -79,11 +79,7 @@ public class AlgaeEffector extends SubsystemBase { //feedforward for arm was added private final double GoalAngle=0; - DigitalInput limitSwitch = new DigitalInput(1); - - public boolean limitDetects() { - return limitSwitch.get(); - } + //-------------------------------------------------------------------------------------------- public AlgaeEffector() { SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); @@ -93,29 +89,29 @@ public AlgaeEffector() { topMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.TopArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.TopArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.TopArrayOrder] + Constants.kP[Constants.AlgaeEffectorc.topArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.topArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.topArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); bottomMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.BottomArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.BottomArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.BottomArrayOrder] + Constants.kP[Constants.AlgaeEffectorc.bottomArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.bottomArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.bottomArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); pincherMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.PincherArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.PincherArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.PincherArrayOrder] + Constants.kP[Constants.AlgaeEffectorc.pincherArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.pincherArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.pincherArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.closedLoop.pid( Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.armArrayOrder, + Constants.kI[Constants.AlgaeEffectorc.armArrayOrder], Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); From 51be17b9b1057a5f660a0df7af93a4efeb8bfe21 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 20 Feb 2025 21:27:42 -0800 Subject: [PATCH 092/153] Cleaner code plus added many constants --- .../java/org/carlmontrobotics/Constants.java | 19 +++++++++- .../subsystems/AlgaeEffector.java | 38 +++++++++++-------- 2 files changed, 41 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ad9f4f2..864cb8e 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -47,13 +47,15 @@ public static final class AlgaeEffectorc { public static final int lowerMotorID = 2; public static final int pinchMotorID = 3; public static final int armMotorID = 4; + public static final int aChannelEnc = 0; + public static final int bChannelEnc = 1; public static final int topArrayOrder = 0; public static final int bottomArrayOrder = 1; public static final int pincherArrayOrder = 2; public static final int armArrayOrder = 3; //the ArrayOrder variables replaced the ones for the kS since they just indicate the order and are the same for all PID values - + //TODO find these values out vvv public static double intakeTopRPM = 1000; public static double intakeBottomRPM = 1000; public static double intakePincherRPM = 1000; @@ -62,5 +64,20 @@ public static final class AlgaeEffectorc { public static double outtakeBottomRPM = 2100; public static double outtakePincherRPM = 2100; + public static final double TBE_PPR = 2048.0; //Through-Bore Encoder + public static final double TBE_DPP = 360.0/TBE_PPR; //Degrees per pulse + + //TODO figure the zero out once encoder is on + public static final double armToZero = 0; //Pure vertical down + //TODO ask samo for angle to intake algae from pure vertical down + public static final double armIntakeAngle = 0; + //TODO Figure these two out if we will be shooting algae + public static final double armRampingUpAngle = 0; + public static final double armShootingAngle = 0; + //TODO Figure angle for dealgafying + public static final double armDealgafyngAngle = 0; + //TODO figure out resting angle of the arm + public static final double armRestingAngle = 0; + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index c5639b7..20747e9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -7,8 +7,6 @@ import static org.carlmontrobotics.RobotContainer.*; -import java.beans.Encoder; - import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; @@ -52,6 +50,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.Encoder; public class AlgaeEffector extends SubsystemBase { @@ -60,34 +59,39 @@ public class AlgaeEffector extends SubsystemBase { private final SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); private final SparkMax armMotor = new SparkMax(Constants.AlgaeEffectorc.armMotorID, MotorType.kBrushless); + private SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); + private SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); + private SparkFlexConfig topMotorConfig = new SparkFlexConfig(); + private SparkMaxConfig armMotorConfig = new SparkMaxConfig(); private final RelativeEncoder topEncoder = topMotor.getEncoder(); private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); private final RelativeEncoder armEncoder = armMotor.getEncoder(); - private final Encoder armAbsoluteEncoder = new Encoder(); + private final Encoder armAbsoluteEncoder = new Encoder(Constants.AlgaeEffectorc.aChannelEnc, Constants.AlgaeEffectorc.bChannelEnc); private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.TopArrayOrder], Constants.kV[Constants.AlgaeEffectorc.TopArrayOrder], Constants.kA[Constants.AlgaeEffectorc.TopArrayOrder]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.BottomArrayOrder], Constants.kV[Constants.AlgaeEffectorc.BottomArrayOrder], Constants.kA[Constants.AlgaeEffectorc.BottomArrayOrder]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.PincherArrayOrder], Constants.kV[Constants.AlgaeEffectorc.PincherArrayOrder], Constants.kA[Constants.AlgaeEffectorc.PincherArrayOrder]); - private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.ArmArrayOrder], Constants.kV[Constants.AlgaeEffectorc.ArmArrayOrder], Constants.kA[Constants.AlgaeEffectorc.ArmArrayOrder]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.topArrayOrder], Constants.kV[Constants.AlgaeEffectorc.topArrayOrder], Constants.kA[Constants.AlgaeEffectorc.topArrayOrder]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.bottomArrayOrder], Constants.kV[Constants.AlgaeEffectorc.bottomArrayOrder], Constants.kA[Constants.AlgaeEffectorc.bottomArrayOrder]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.pincherArrayOrder], Constants.kV[Constants.AlgaeEffectorc.pincherArrayOrder], Constants.kA[Constants.AlgaeEffectorc.pincherArrayOrder]); + private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.armArrayOrder], Constants.kV[Constants.AlgaeEffectorc.armArrayOrder], Constants.kA[Constants.AlgaeEffectorc.armArrayOrder]); //feedforward for arm was added - private final double GoalAngle=0; + private final double armGoalAngle = 0; //-------------------------------------------------------------------------------------------- public AlgaeEffector() { - SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); - SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); - SparkFlexConfig topMotorConfig = new SparkFlexConfig(); - SparkMaxConfig armMotorConfig = new SparkMaxConfig(); - + configureMotors(); + armAbsoluteEncoder.setDistancePerPulse(Constants.AlgaeEffectorc.TBE_DPP); + + } + //---------------------------------------------------------------------------------------- + private void configureMotors () { topMotorConfig.closedLoop.pid( Constants.kP[Constants.AlgaeEffectorc.topArrayOrder], Constants.kI[Constants.AlgaeEffectorc.topArrayOrder], @@ -116,9 +120,13 @@ public AlgaeEffector() { ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.idleMode(IdleMode.kBrake); - armMotorConfig.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder); + armMotorConfig.closedLoop + .pid( + Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.armArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); } - //---------------------------------------------------------------------------------------- public void setTopRPM(double toprpm) { pidControllerTop.setReference(toprpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); From 0ac5a027fc53bf975cbe34f86d06585a32ff4079 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 21 Feb 2025 08:36:28 -0800 Subject: [PATCH 093/153] Bug fixes + finished defining encoder --- .../java/org/carlmontrobotics/Constants.java | 10 ++++-- .../commands/SetArmToAngle.java | 13 ++++---- .../subsystems/AlgaeEffector.java | 31 +++++++++---------- 3 files changed, 30 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 864cb8e..f47abf7 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -1,5 +1,9 @@ package org.carlmontrobotics; +import java.security.spec.EncodedKeySpec; + +import edu.wpi.first.wpilibj.CounterBase; +import edu.wpi.first.wpilibj.Encoder; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.XboxController.Button; @@ -66,6 +70,8 @@ public static final class AlgaeEffectorc { public static final double TBE_PPR = 2048.0; //Through-Bore Encoder public static final double TBE_DPP = 360.0/TBE_PPR; //Degrees per pulse + public static final boolean invertedTBE = false; //if the encoder needs to read invertedly + public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k4X; //TODO figure the zero out once encoder is on public static final double armToZero = 0; //Pure vertical down @@ -76,8 +82,8 @@ public static final class AlgaeEffectorc { public static final double armShootingAngle = 0; //TODO Figure angle for dealgafying public static final double armDealgafyngAngle = 0; - //TODO figure out resting angle of the arm - public static final double armRestingAngle = 0; + //TODO figure out resting angle of the arm while algae inside + public static final double armRestingAngleWhileIntakeAlgae = 0; } } diff --git a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java index aa5ad7b..f2f445c 100644 --- a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java +++ b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java @@ -1,22 +1,23 @@ package org.carlmontrobotics.commands; + import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -public class setArmAngle extends Command { - AlgaeEffector Algae; +public class SetArmToAngle extends Command { + AlgaeEffector algaeEffector; double angle; private final Timer timer = new Timer(); public SetArmToAngle(AlgaeEffector algaeEffector, double angle ) { - addRequirements(this.Algae = algaeEffector); + addRequirements(this.algaeEffector = algaeEffector); this.angle = angle; } @Override public void initialize() { - AlgaeEffector.setArmAngle(angle); + algaeEffector.setArmAngle(angle); timer.start(); } @@ -29,12 +30,12 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - Algae.stopMotors(); + algaeEffector.stopMotors(); } // Returns true when the command should end. @Override public boolean isFinished() { - return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return algaeEffector.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 20747e9..4d4e0d4 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -68,7 +68,7 @@ public class AlgaeEffector extends SubsystemBase { private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); private final RelativeEncoder armEncoder = armMotor.getEncoder(); - private final Encoder armAbsoluteEncoder = new Encoder(Constants.AlgaeEffectorc.aChannelEnc, Constants.AlgaeEffectorc.bChannelEnc); + private final Encoder armAbsoluteEncoder = new Encoder(Constants.AlgaeEffectorc.aChannelEnc, Constants.AlgaeEffectorc.bChannelEnc, Constants.AlgaeEffectorc.invertedTBE, Constants.AlgaeEffectorc.encodingType); private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); @@ -81,7 +81,7 @@ public class AlgaeEffector extends SubsystemBase { private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.armArrayOrder], Constants.kV[Constants.AlgaeEffectorc.armArrayOrder], Constants.kA[Constants.AlgaeEffectorc.armArrayOrder]); //feedforward for arm was added - private final double armGoalAngle = 0; + private double armGoalAngle = 0; //-------------------------------------------------------------------------------------------- public AlgaeEffector() { @@ -120,12 +120,11 @@ private void configureMotors () { ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.idleMode(IdleMode.kBrake); - armMotorConfig.closedLoop - .pid( - Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + armMotorConfig.closedLoop.pid( + Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], + Constants.kI[Constants.AlgaeEffectorc.armArrayOrder], + Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] + ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); } public void setTopRPM(double toprpm) { @@ -144,7 +143,7 @@ public void setPincherRPM(double pincherrpm) { } public void setArmAngle(double armangle) { - GoalAngle = armangle; + armGoalAngle = armangle; } public void runRPM() { @@ -168,17 +167,17 @@ public boolean checkIfAtBottomRPM(double rpm) { return bottomEncoder.getVelocity() == rpm; } - public void setSpeed(double speed) { - topMotor.set(speed); - bottomMotor.set(speed); - pincherMotor.set(speed); + public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpeed) { + topMotor.set(topSpeed); + bottomMotor.set(bottomSpeed); + pincherMotor.set(pincherSpeed); } - @override + @Override public void periodic() { - pidControllerArm.setReference(goalAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(goalAngle); + pidControllerArm.setReference(armGoalAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); + pincherFeedforward.calculate(goalAngle); //What is this for } } From 72187ad1524ef0c0206f964d402addbf01cec24c Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Fri, 21 Feb 2025 09:27:25 -0800 Subject: [PATCH 094/153] Updated encoder reading --- 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 f47abf7..90b838d 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -71,7 +71,7 @@ public static final class AlgaeEffectorc { public static final double TBE_PPR = 2048.0; //Through-Bore Encoder public static final double TBE_DPP = 360.0/TBE_PPR; //Degrees per pulse public static final boolean invertedTBE = false; //if the encoder needs to read invertedly - public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k4X; + public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; //TODO figure the zero out once encoder is on public static final double armToZero = 0; //Pure vertical down From ca5e0208859e4a4c17acd2f22a6dbae5f8f06c57 Mon Sep 17 00:00:00 2001 From: AikenB Date: Fri, 21 Feb 2025 14:59:26 -0800 Subject: [PATCH 095/153] Added method for getarmpos() pls check to see if it works I kind of copied and pasted it from 2024 code and changed the values of the constants --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++++ .../org/carlmontrobotics/subsystems/AlgaeEffector.java | 8 ++++++++ 2 files changed, 12 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 90b838d..7a66dd8 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -85,5 +85,9 @@ public static final class AlgaeEffectorc { //TODO figure out resting angle of the arm while algae inside public static final double armRestingAngleWhileIntakeAlgae = 0; + public static final double UPPER_ANGLE_LIMIT_RAD = 2.6; + public static final double LOWER_ANGLE_LIMIT_RAD = 0; + public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 4d4e0d4..688e5ab 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -146,6 +146,14 @@ public void setArmAngle(double armangle) { armGoalAngle = armangle; } + public double getArmPos() { + + return MathUtil.inputModulus(armAbsoluteEncoder.getPosition(), + 0, ARM_DISCONT_RAD); + + + } + public void runRPM() { //TODO: Change RPM according to design setTopRPM(1000); From 51071efe5238f84bf5d62e2b3b6a55d5feb56a21 Mon Sep 17 00:00:00 2001 From: AikenB Date: Fri, 21 Feb 2025 15:31:53 -0800 Subject: [PATCH 096/153] added getArmVel() since it was in the 2024 arm code and it was used in one of the commands --- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 688e5ab..8c1e67c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -149,11 +149,15 @@ public void setArmAngle(double armangle) { public double getArmPos() { return MathUtil.inputModulus(armAbsoluteEncoder.getPosition(), - 0, ARM_DISCONT_RAD); + ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI); } + public double getArmVel() { + return armAbsoluteEncoder.getVelocity(); + } + public void runRPM() { //TODO: Change RPM according to design setTopRPM(1000); From 0e2d6fdd63feaddaef1b0b22414ae92f37d02a25 Mon Sep 17 00:00:00 2001 From: AikenB Date: Fri, 21 Feb 2025 16:30:28 -0800 Subject: [PATCH 097/153] added gear ratio in constants --- src/main/java/org/carlmontrobotics/Constants.java | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7a66dd8..1e94c92 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -73,6 +73,8 @@ public static final class AlgaeEffectorc { public static final boolean invertedTBE = false; //if the encoder needs to read invertedly public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; + public static final double armGearing = 39.375; + //TODO figure the zero out once encoder is on public static final double armToZero = 0; //Pure vertical down //TODO ask samo for angle to intake algae from pure vertical down @@ -85,6 +87,7 @@ public static final class AlgaeEffectorc { //TODO figure out resting angle of the arm while algae inside public static final double armRestingAngleWhileIntakeAlgae = 0; + //max angle in radians and minimum angle in radians public static final double UPPER_ANGLE_LIMIT_RAD = 2.6; public static final double LOWER_ANGLE_LIMIT_RAD = 0; public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; 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 098/153] 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 d78acc41091174a1120d7506374e3cc0331a3af8 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 22 Feb 2025 11:23:43 -0800 Subject: [PATCH 099/153] Update to TBE, and bug fixes --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 6 +++--- 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 1e94c92..21d7cea 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -68,8 +68,8 @@ public static final class AlgaeEffectorc { public static double outtakeBottomRPM = 2100; public static double outtakePincherRPM = 2100; - public static final double TBE_PPR = 2048.0; //Through-Bore Encoder - public static final double TBE_DPP = 360.0/TBE_PPR; //Degrees per pulse + public static final int TBE_CPR = 8192; //Through-Bore Encoder + public static final double TBE_DPP = 360.0/TBE_CPR; //Degrees per pulse public static final boolean invertedTBE = false; //if the encoder needs to read invertedly public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 8c1e67c..c775e2f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -25,6 +25,7 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkMaxAlternateEncoder; import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; @@ -68,7 +69,7 @@ public class AlgaeEffector extends SubsystemBase { private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); private final RelativeEncoder armEncoder = armMotor.getEncoder(); - private final Encoder armAbsoluteEncoder = new Encoder(Constants.AlgaeEffectorc.aChannelEnc, Constants.AlgaeEffectorc.bChannelEnc, Constants.AlgaeEffectorc.invertedTBE, Constants.AlgaeEffectorc.encodingType); + private final RelativeEncoder armAbsoluteEncoder = armMotor.getAlternateEncoder(); private final SparkClosedLoopController pidControllerTop = topMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); @@ -86,7 +87,6 @@ public class AlgaeEffector extends SubsystemBase { //-------------------------------------------------------------------------------------------- public AlgaeEffector() { configureMotors(); - armAbsoluteEncoder.setDistancePerPulse(Constants.AlgaeEffectorc.TBE_DPP); } //---------------------------------------------------------------------------------------- @@ -149,7 +149,7 @@ public void setArmAngle(double armangle) { public double getArmPos() { return MathUtil.inputModulus(armAbsoluteEncoder.getPosition(), - ARM_DISCONT_RAD, ARM_DISCONT_RAD + 2 * Math.PI); + Constants.AlgaeEffectorc.ARM_DISCONT_RAD, Constants.AlgaeEffectorc.ARM_DISCONT_RAD + 2 * Math.PI); } From d54003eaf2dca8363744cecabbffcd6ec1c44fa8 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 22 Feb 2025 11:47:27 -0800 Subject: [PATCH 100/153] Started working on intake command --- .../java/org/carlmontrobotics/Constants.java | 15 +++++++++------ .../commands/IntakeAlgae.java | 15 ++++++++++----- .../commands/OuttakeAlgae.java | 2 +- .../commands/SetArmToAngle.java | 4 +--- .../subsystems/AlgaeEffector.java | 19 +++++++++++-------- 5 files changed, 32 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 21d7cea..a295bc6 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -85,12 +85,15 @@ public static final class AlgaeEffectorc { //TODO Figure angle for dealgafying public static final double armDealgafyngAngle = 0; //TODO figure out resting angle of the arm while algae inside - public static final double armRestingAngleWhileIntakeAlgae = 0; - - //max angle in radians and minimum angle in radians - public static final double UPPER_ANGLE_LIMIT_RAD = 2.6; - public static final double LOWER_ANGLE_LIMIT_RAD = 0; - public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; + public static final double armRestingAngleWhileIntakeAlgae = 0.0; + //TODO figure out current threshold for pincher wheels + public static final double pincherCurrentThreshold = 15.0; + + //Bad idea to make the arm think like it can spin 360 degrees CAUSE IT CANNOT #StopGaslightingTheArm + // //max angle in radians and minimum angle in radians + // public static final double UPPER_ANGLE_LIMIT_RAD = 2.6; + // public static final double LOWER_ANGLE_LIMIT_RAD = 0; + // public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; } } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 6c47d47..e37fd71 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -18,15 +18,20 @@ public IntakeAlgae(AlgaeEffector Algae) { public void initialize() { timer.reset(); timer.start(); - Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); + Algae.setArmAngle(Constants.AlgaeEffectorc.armIntakeAngle); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + if (Algae.getArmPos() > Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) { + + Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); + } + } // Called once the command ends or is interrupted. @Override @@ -40,6 +45,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 10d885e..3985b28 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -34,6 +34,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return Algae.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java index f2f445c..dd8220a 100644 --- a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java +++ b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java @@ -18,7 +18,6 @@ public SetArmToAngle(AlgaeEffector algaeEffector, double angle ) { @Override public void initialize() { algaeEffector.setArmAngle(angle); - timer.start(); } @@ -30,12 +29,11 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - algaeEffector.stopMotors(); } // Returns true when the command should end. @Override public boolean isFinished() { - return algaeEffector.limitDetects() || timer.get() > 1; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index c775e2f..0eb3d7f 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -147,16 +147,14 @@ public void setArmAngle(double armangle) { } public double getArmPos() { - - return MathUtil.inputModulus(armAbsoluteEncoder.getPosition(), - Constants.AlgaeEffectorc.ARM_DISCONT_RAD, Constants.AlgaeEffectorc.ARM_DISCONT_RAD + 2 * Math.PI); - - + //figures out the position of the arm in degrees based off pure vertical down + //TODO update the arm to get in degrees after someone will figure out what the .getPosition gets for the TBE + return armAbsoluteEncoder.getPosition() - Constants.AlgaeEffectorc.armToZero; } - public double getArmVel() { - return armAbsoluteEncoder.getVelocity(); - } + // public double getArmVel() { + // return armAbsoluteEncoder.getVelocity(); + // } public void runRPM() { //TODO: Change RPM according to design @@ -185,6 +183,11 @@ public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpe pincherMotor.set(pincherSpeed); } + public boolean isAlgaeIntaked() { + return pincherMotor.getOutputCurrent() > Constants.AlgaeEffectorc.pincherCurrentThreshold; + } + + @Override public void periodic() { 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 101/153] 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 702f9de782ba4c5868825463dedd4f93c5c59443 Mon Sep 17 00:00:00 2001 From: AikenB Date: Sat, 22 Feb 2025 17:51:54 -0800 Subject: [PATCH 102/153] added intake and outtake commands --- src/main/java/org/carlmontrobotics/Constants.java | 4 +++- .../carlmontrobotics/commands/IntakeAlgae.java | 9 ++++++++- .../carlmontrobotics/commands/OuttakeAlgae.java | 11 +++++++++++ .../subsystems/AlgaeEffector.java | 15 ++++++++++++--- 4 files changed, 34 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index a295bc6..16abbf2 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -73,7 +73,9 @@ public static final class AlgaeEffectorc { public static final boolean invertedTBE = false; //if the encoder needs to read invertedly public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; - public static final double armGearing = 39.375; + public static final double armChainGearing = 16.0/34; + public static final double armGearRatio = 1.0/3; + //TODO figure the zero out once encoder is on public static final double armToZero = 0; //Pure vertical down diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index e37fd71..a2f6dba 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -25,11 +25,18 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + Algae.setArmAngle(Constants.AlgaeEffectorc.armIntakeAngle) if (Algae.getArmPos() > Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) { - Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); + if (Algae.isAlgaeIntaked()) { + Algae.setTopRPM(0); + Algae.setBottomRPM(0); + Algae.setArmAngle(Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) + } + + } } diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 3985b28..5c78724 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -4,6 +4,8 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.util.Units; + public class OuttakeAlgae extends Command { AlgaeEffector Algae; @@ -17,12 +19,21 @@ public void initialize() { Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); + Algae.setArmAngle(Constants.AlgaeEffectorc.armIntakeAngle); timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreesToRadians(20)) + + // if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreestoradian(5)) { + // Algae.setTopRPM(-1 * Constants.AlgaeEffectorc.intakeTopRPM); + // Algae.setBottomRPM(-1 * Constants.AlgaeEffectorc.intakeBottomRPM); + // Algae.setPincherRPM(-1 * Constants.AlgaeEffectorc.intakePincherRPM); + // } + } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 0eb3d7f..0722c83 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -142,16 +142,25 @@ public void setPincherRPM(double pincherrpm) { pincherFeedforward.calculate(pincherrpm); } - public void setArmAngle(double armangle) { - armGoalAngle = armangle; + public void setArmPosition(double armangle) { + pidControllerArm.setReference(armangle, ControlType.kVelocity, ClosedLoopSlot.kSlot0); + pincherFeedforward.calculate(pincherrpm); } + + public double getArmPos() { //figures out the position of the arm in degrees based off pure vertical down //TODO update the arm to get in degrees after someone will figure out what the .getPosition gets for the TBE - return armAbsoluteEncoder.getPosition() - Constants.AlgaeEffectorc.armToZero; + return armAbsoluteEncoder.getPosition() * Constants.AlgaeEffectorc.armChainGearing - Constants.AlgaeEffectorc.armToZero; } + public void stopArm() { + armMotor.set(0); + + } + + // public double getArmVel() { // return armAbsoluteEncoder.getVelocity(); // } From 216dc42f946e078d43c3e693ed1e95ce1e4f79ff Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 23 Feb 2025 10:46:05 -0800 Subject: [PATCH 103/153] rename some constants - make them upper --- .../java/org/carlmontrobotics/Constants.java | 20 ++++---- .../subsystems/AlgaeEffector.java | 48 ++++++++++--------- 2 files changed, 37 insertions(+), 31 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 16abbf2..ba38456 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -47,17 +47,17 @@ public static final class Manipulator { } } public static final class AlgaeEffectorc { - public static final int upperMotorID = 1; - public static final int lowerMotorID = 2; - public static final int pinchMotorID = 3; - public static final int armMotorID = 4; + public static final int UPPER_MOTOR_PORT = 1; + public static final int LOWER_MOTOR_PORT = 2; + public static final int PINCH_MOTOR_PORT = 3; + public static final int ARM_MOTOR_PORT = 4; public static final int aChannelEnc = 0; public static final int bChannelEnc = 1; - public static final int topArrayOrder = 0; - public static final int bottomArrayOrder = 1; - public static final int pincherArrayOrder = 2; - public static final int armArrayOrder = 3; + public static final int TOP_ARRAY_ORDER = 0; + public static final int BOTTOM_ARRAY_ORDER = 1; + public static final int PINCHER_ARRAY_ORDER = 2; + public static final int ARM_ARRAY_ORDER = 3; //the ArrayOrder variables replaced the ones for the kS since they just indicate the order and are the same for all PID values //TODO find these values out vvv public static double intakeTopRPM = 1000; @@ -98,4 +98,8 @@ public static final class AlgaeEffectorc { // public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; } + public static final class AlgaeArmc{ + public static final int ARM_MOTOR_PORT_MASTER = 1; //to change + public static final int ARM_MOTOR_PORT_FOLLOWER = 2; //to change + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 0722c83..d97e08c 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -53,12 +53,14 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj.Encoder; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; + public class AlgaeEffector extends SubsystemBase { - private final SparkFlex topMotor = new SparkFlex(Constants.AlgaeEffectorc.upperMotorID, MotorType.kBrushless); - private final SparkFlex bottomMotor = new SparkFlex(Constants.AlgaeEffectorc.lowerMotorID, MotorType.kBrushless); - private final SparkFlex pincherMotor = new SparkFlex(Constants.AlgaeEffectorc.pinchMotorID, MotorType.kBrushless); - private final SparkMax armMotor = new SparkMax(Constants.AlgaeEffectorc.armMotorID, MotorType.kBrushless); + private final SparkFlex topMotor = new SparkFlex(UPPER_MOTOR_PORT, MotorType.kBrushless); + private final SparkFlex bottomMotor = new SparkFlex(LOWER_MOTOR_PORT, MotorType.kBrushless); + private final SparkFlex pincherMotor = new SparkFlex(PINCH_MOTOR_PORT, MotorType.kBrushless); + private final SparkMax armMotor = new SparkMax(ARM_MOTOR_PORT, MotorType.kBrushless); private SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); private SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); @@ -76,10 +78,10 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.topArrayOrder], Constants.kV[Constants.AlgaeEffectorc.topArrayOrder], Constants.kA[Constants.AlgaeEffectorc.topArrayOrder]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.bottomArrayOrder], Constants.kV[Constants.AlgaeEffectorc.bottomArrayOrder], Constants.kA[Constants.AlgaeEffectorc.bottomArrayOrder]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.pincherArrayOrder], Constants.kV[Constants.AlgaeEffectorc.pincherArrayOrder], Constants.kA[Constants.AlgaeEffectorc.pincherArrayOrder]); - private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[Constants.AlgaeEffectorc.armArrayOrder], Constants.kV[Constants.AlgaeEffectorc.armArrayOrder], Constants.kA[Constants.AlgaeEffectorc.armArrayOrder]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[TOP_ARRAY_ORDER], Constants.kV[TOP_ARRAY_ORDER], Constants.kA[TOP_ARRAY_ORDER]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[BOTTOM_ARRAY_ORDER], Constants.kV[BOTTOM_ARRAY_ORDER], Constants.kA[BOTTOM_ARRAY_ORDER]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[PINCHER_ARRAY_ORDER], Constants.kV[PINCHER_ARRAY_ORDER], Constants.kA[PINCHER_ARRAY_ORDER]); + private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[ARM_ARRAY_ORDER], Constants.kV[ARM_ARRAY_ORDER], Constants.kA[ARM_ARRAY_ORDER]); //feedforward for arm was added private double armGoalAngle = 0; @@ -93,37 +95,37 @@ public AlgaeEffector() { private void configureMotors () { topMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.topArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.topArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.topArrayOrder] + Constants.kP[TOP_ARRAY_ORDER], + Constants.kI[TOP_ARRAY_ORDER], + Constants.kD[TOP_ARRAY_ORDER] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); bottomMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.bottomArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.bottomArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.bottomArrayOrder] + Constants.kP[BOTTOM_ARRAY_ORDER], + Constants.kI[BOTTOM_ARRAY_ORDER], + Constants.kD[BOTTOM_ARRAY_ORDER] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); pincherMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.pincherArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.pincherArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.pincherArrayOrder] + Constants.kP[PINCHER_ARRAY_ORDER], + Constants.kI[PINCHER_ARRAY_ORDER], + Constants.kD[PINCHER_ARRAY_ORDER] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] + Constants.kP[ARM_ARRAY_ORDER], + Constants.kI[ARM_ARRAY_ORDER], + Constants.kD[ARM_ARRAY_ORDER] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.idleMode(IdleMode.kBrake); armMotorConfig.closedLoop.pid( - Constants.kP[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kI[Constants.AlgaeEffectorc.armArrayOrder], - Constants.kD[Constants.AlgaeEffectorc.armArrayOrder] + Constants.kP[ARM_ARRAY_ORDER], + Constants.kI[ARM_ARRAY_ORDER], + Constants.kD[ARM_ARRAY_ORDER] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); } From 67a31d789cc727dc609a95f0ee094f6fb3ba72fe Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 23 Feb 2025 10:56:18 -0800 Subject: [PATCH 104/153] added trap profile --- .../subsystems/AlgaeEffector.java | 43 ++++++++++++++++--- 1 file changed, 38 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index d97e08c..3698d47 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -54,9 +54,12 @@ import edu.wpi.first.wpilibj.Encoder; import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; +import static org.carlmontrobotics.Constants.*; public class AlgaeEffector extends SubsystemBase { + + //motors private final SparkFlex topMotor = new SparkFlex(UPPER_MOTOR_PORT, MotorType.kBrushless); private final SparkFlex bottomMotor = new SparkFlex(LOWER_MOTOR_PORT, MotorType.kBrushless); private final SparkFlex pincherMotor = new SparkFlex(PINCH_MOTOR_PORT, MotorType.kBrushless); @@ -78,12 +81,19 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(Constants.kS[TOP_ARRAY_ORDER], Constants.kV[TOP_ARRAY_ORDER], Constants.kA[TOP_ARRAY_ORDER]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(Constants.kS[BOTTOM_ARRAY_ORDER], Constants.kV[BOTTOM_ARRAY_ORDER], Constants.kA[BOTTOM_ARRAY_ORDER]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(Constants.kS[PINCHER_ARRAY_ORDER], Constants.kV[PINCHER_ARRAY_ORDER], Constants.kA[PINCHER_ARRAY_ORDER]); - private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(Constants.kS[ARM_ARRAY_ORDER], Constants.kV[ARM_ARRAY_ORDER], Constants.kA[ARM_ARRAY_ORDER]); + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[TOP_ARRAY_ORDER], kV[TOP_ARRAY_ORDER], kA[TOP_ARRAY_ORDER]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[BOTTOM_ARRAY_ORDER], kV[BOTTOM_ARRAY_ORDER], kA[BOTTOM_ARRAY_ORDER]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[PINCHER_ARRAY_ORDER], kV[PINCHER_ARRAY_ORDER], kA[PINCHER_ARRAY_ORDER]); + private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(kS[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); //feedforward for arm was added + + + //Arm Trapezoid Profile + private TrapezoidProfile armTrapProfile; + private TrapezoidProfile.State armGoalState = new TrapezoidProfile.State(0,0); //position,velocity (0,0) + + private double armGoalAngle = 0; //-------------------------------------------------------------------------------------------- @@ -143,11 +153,29 @@ public void setPincherRPM(double pincherrpm) { pidControllerPincher.setReference(pincherrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); pincherFeedforward.calculate(pincherrpm); } - + //arm methods public void setArmPosition(double armangle) { pidControllerArm.setReference(armangle, ControlType.kVelocity, ClosedLoopSlot.kSlot0); pincherFeedforward.calculate(pincherrpm); } + + public void setArmTarget(double targetPost){ + + } + + //returns the arm position and velocity based on encoder and position + public TrapezoidProfile.State getArmState(){ + TrapezoidProfile.State armState = new TrapezoidProfile.State(getArmPos(), getArmVel()); + return armState; + } + + public boolean armAtGoal(){ + //TODO: + //reutrns if arm is at goal state - need to add toelrance + return false; + } + + @@ -156,6 +184,9 @@ public double getArmPos() { //TODO update the arm to get in degrees after someone will figure out what the .getPosition gets for the TBE return armAbsoluteEncoder.getPosition() * Constants.AlgaeEffectorc.armChainGearing - Constants.AlgaeEffectorc.armToZero; } + public double getArmVel(){ + return armAbsoluteEncoder.getVelocity(); + } public void stopArm() { armMotor.set(0); @@ -197,6 +228,8 @@ public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpe public boolean isAlgaeIntaked() { return pincherMotor.getOutputCurrent() > Constants.AlgaeEffectorc.pincherCurrentThreshold; } + + From e730b2ee3f840f16e09d7a8899cfdb7d7bd28c43 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 23 Feb 2025 11:02:41 -0800 Subject: [PATCH 105/153] constants and trap profile --- .../java/org/carlmontrobotics/Constants.java | 31 +++++++++---------- .../subsystems/AlgaeEffector.java | 6 ++-- 2 files changed, 17 insertions(+), 20 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ba38456..a535683 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -60,36 +60,36 @@ public static final class AlgaeEffectorc { public static final int ARM_ARRAY_ORDER = 3; //the ArrayOrder variables replaced the ones for the kS since they just indicate the order and are the same for all PID values //TODO find these values out vvv - public static double intakeTopRPM = 1000; - public static double intakeBottomRPM = 1000; - public static double intakePincherRPM = 1000; + public static double INTAKE_TOP_RPM = 1000; + public static double INTAKE_BOTTOM_RPM = 1000; + public static double INTAKE_PINCHER_RPM = 1000; - public static double outtakeTopRPM = 2100; - public static double outtakeBottomRPM = 2100; - public static double outtakePincherRPM = 2100; + public static double OUTTAKE_TOP_RPM = 2100; + public static double OUTTAKE_BOTTOM_RPM = 2100; + public static double OUTTAKE_PINCHER_RPM = 2100; public static final int TBE_CPR = 8192; //Through-Bore Encoder public static final double TBE_DPP = 360.0/TBE_CPR; //Degrees per pulse public static final boolean invertedTBE = false; //if the encoder needs to read invertedly public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; - public static final double armChainGearing = 16.0/34; - public static final double armGearRatio = 1.0/3; + public static final double ARM_CHAIN_GEARING = 16.0/34; + public static final double ARM_GEAR_RATIO = 1.0/3; //TODO figure the zero out once encoder is on - public static final double armToZero = 0; //Pure vertical down + public static final double ARM_TO_ZERO = 0; //Pure vertical down //TODO ask samo for angle to intake algae from pure vertical down - public static final double armIntakeAngle = 0; + public static final double ARM_INTAKE_ANGLE = 0; //TODO Figure these two out if we will be shooting algae - public static final double armRampingUpAngle = 0; - public static final double armShootingAngle = 0; + public static final double ARM_RAMP_UP_ANGLE = 0; + public static final double ARM_SHOOT_ANGLE = 0; //TODO Figure angle for dealgafying public static final double armDealgafyngAngle = 0; //TODO figure out resting angle of the arm while algae inside public static final double armRestingAngleWhileIntakeAlgae = 0.0; //TODO figure out current threshold for pincher wheels - public static final double pincherCurrentThreshold = 15.0; + public static final double PINCHER_CURRENT_THRESHOLD = 15.0; //Bad idea to make the arm think like it can spin 360 degrees CAUSE IT CANNOT #StopGaslightingTheArm // //max angle in radians and minimum angle in radians @@ -98,8 +98,5 @@ public static final class AlgaeEffectorc { // public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; } - public static final class AlgaeArmc{ - public static final int ARM_MOTOR_PORT_MASTER = 1; //to change - public static final int ARM_MOTOR_PORT_FOLLOWER = 2; //to change - } + } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 3698d47..adb53b6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -156,7 +156,7 @@ public void setPincherRPM(double pincherrpm) { //arm methods public void setArmPosition(double armangle) { pidControllerArm.setReference(armangle, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(pincherrpm); + pincherFeedforward.calculate(pincherrpm); //WHAT IS THIS } public void setArmTarget(double targetPost){ @@ -182,7 +182,7 @@ public boolean armAtGoal(){ public double getArmPos() { //figures out the position of the arm in degrees based off pure vertical down //TODO update the arm to get in degrees after someone will figure out what the .getPosition gets for the TBE - return armAbsoluteEncoder.getPosition() * Constants.AlgaeEffectorc.armChainGearing - Constants.AlgaeEffectorc.armToZero; + return armAbsoluteEncoder.getPosition() * ARM_CHAIN_GEARING - ARM_TO_ZERO; } public double getArmVel(){ return armAbsoluteEncoder.getVelocity(); @@ -226,7 +226,7 @@ public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpe } public boolean isAlgaeIntaked() { - return pincherMotor.getOutputCurrent() > Constants.AlgaeEffectorc.pincherCurrentThreshold; + return pincherMotor.getOutputCurrent() > PINCHER_CURRENT_THRESHOLD; } From 74f485e4ed6965d3ef378bb23d4aa67dc755aa18 Mon Sep 17 00:00:00 2001 From: AikenB Date: Sun, 23 Feb 2025 11:19:20 -0800 Subject: [PATCH 106/153] fixed variable names in the commands since they were capitalized in constants.java --- .../java/org/carlmontrobotics/Constants.java | 9 +++- .../commands/IntakeAlgae.java | 10 ++--- .../commands/OuttakeAlgae.java | 8 ++-- .../carlmontrobotics/commands/ShootAlgae.java | 43 +++++++++++++++++++ 4 files changed, 60 insertions(+), 10 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/ShootAlgae.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index a535683..e774c2f 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -68,6 +68,10 @@ public static final class AlgaeEffectorc { public static double OUTTAKE_BOTTOM_RPM = 2100; public static double OUTTAKE_PINCHER_RPM = 2100; + public static double SHOOT_TOP_RPM = 2100;//ask design + public static double SHOOT_BOTTOM_RPM = 2100; + public static double SHOOT_PINCHER_RPM = 2100; + public static final int TBE_CPR = 8192; //Through-Bore Encoder public static final double TBE_DPP = 360.0/TBE_CPR; //Degrees per pulse public static final boolean invertedTBE = false; //if the encoder needs to read invertedly @@ -98,5 +102,8 @@ public static final class AlgaeEffectorc { // public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; } - + public static final class AlgaeArmc{ + public static final int ARM_MOTOR_PORT_MASTER = 1; //to change + public static final int ARM_MOTOR_PORT_FOLLOWER = 2; //to change + } } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index a2f6dba..3f98a08 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -18,18 +18,18 @@ public IntakeAlgae(AlgaeEffector Algae) { public void initialize() { timer.reset(); timer.start(); - Algae.setArmAngle(Constants.AlgaeEffectorc.armIntakeAngle); + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Algae.setArmAngle(Constants.AlgaeEffectorc.armIntakeAngle) + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) if (Algae.getArmPos() > Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) { - Algae.setTopRPM(Constants.AlgaeEffectorc.intakeTopRPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.intakeBottomRPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.intakePincherRPM); + Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); if (Algae.isAlgaeIntaked()) { Algae.setTopRPM(0); Algae.setBottomRPM(0); diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 5c78724..e98bee1 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -16,10 +16,10 @@ public OuttakeAlgae(AlgaeEffector algaeEffector) { @Override public void initialize() { - Algae.setTopRPM(Constants.AlgaeEffectorc.outtakeTopRPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.outtakeBottomRPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.outtakePincherRPM); - Algae.setArmAngle(Constants.AlgaeEffectorc.armIntakeAngle); + Algae.setTopRPM(Constants.AlgaeEffectorc.OUTTAKE_TOP_RPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.OUTTAKE_BOTTOM_RPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.OUTTAKE_PINCHER_RPM); + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); timer.start(); } diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java new file mode 100644 index 0000000..05e5da1 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -0,0 +1,43 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.util.Units; + + +public class OuttakeAlgae extends Command { + AlgaeEffector Algae; + private final Timer timer = new Timer(); + public OuttakeAlgae(AlgaeEffector algaeEffector) { + addRequirements(this.Algae = algaeEffector); + } + + @Override + public void initialize() { + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armShootingAngle) <= Units.degreesToRadians(5)) + Algae.setPincherRPM(Constants.AlgaeEffectorc.sho); + Algae.setTopRPM(Constants.AlgaeEffectorc.ShootingTopRPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.ShootingBottomRPM); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Algae.stopMotors(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file From 1232b58cb9fa8fb9510e76217e5f3025cfad12d6 Mon Sep 17 00:00:00 2001 From: AikenB Date: Sun, 23 Feb 2025 11:46:41 -0800 Subject: [PATCH 107/153] added dealgafication command and made the code for the commands a bit more organized --- .../java/org/carlmontrobotics/Constants.java | 20 +++++---- .../commands/Dealgafication.java | 41 +++++++++++++++++++ .../commands/IntakeAlgae.java | 10 ++--- .../commands/OuttakeAlgae.java | 6 +-- .../carlmontrobotics/commands/ShootAlgae.java | 4 +- 5 files changed, 60 insertions(+), 21 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/Dealgafication.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index e774c2f..96ed5fd 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -64,13 +64,17 @@ public static final class AlgaeEffectorc { public static double INTAKE_BOTTOM_RPM = 1000; public static double INTAKE_PINCHER_RPM = 1000; - public static double OUTTAKE_TOP_RPM = 2100; - public static double OUTTAKE_BOTTOM_RPM = 2100; - public static double OUTTAKE_PINCHER_RPM = 2100; + public static double OUTTAKE_TOP_RPM = -2100; + public static double OUTTAKE_BOTTOM_RPM = -2100; + public static double OUTTAKE_PINCHER_RPM = -2100; - public static double SHOOT_TOP_RPM = 2100;//ask design - public static double SHOOT_BOTTOM_RPM = 2100; - public static double SHOOT_PINCHER_RPM = 2100; + public static double SHOOT_TOP_RPM = -2100;//ask design + public static double SHOOT_BOTTOM_RPM = -2100; + public static double SHOOT_PINCHER_RPM = -2100; + + public static double DEALGAFY_TOP_RPM = 1000; + public static double DEALGAFY_BOTTOM_RPM = 1000; + public static double DEALGAFY_PINCHER_RPM = 1000; public static final int TBE_CPR = 8192; //Through-Bore Encoder public static final double TBE_DPP = 360.0/TBE_CPR; //Degrees per pulse @@ -89,9 +93,9 @@ public static final class AlgaeEffectorc { public static final double ARM_RAMP_UP_ANGLE = 0; public static final double ARM_SHOOT_ANGLE = 0; //TODO Figure angle for dealgafying - public static final double armDealgafyngAngle = 0; + public static final double ARM_DEALGAFYING_ANGLE = 0; //TODO figure out resting angle of the arm while algae inside - public static final double armRestingAngleWhileIntakeAlgae = 0.0; + public static final double ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE = 0.0; //TODO figure out current threshold for pincher wheels public static final double PINCHER_CURRENT_THRESHOLD = 15.0; diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java new file mode 100644 index 0000000..a3a8048 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -0,0 +1,41 @@ +package org.carlmontrobotics.commands; +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.math.util.Units; + + +public class OuttakeAlgae extends Command { + AlgaeEffector Algae; + private final Timer timer = new Timer(); + public OuttakeAlgae(AlgaeEffector algaeEffector) { + addRequirements(this.Algae = algaeEffector); + } + + @Override + public void initialize() { + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE); + timer.start(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE) <= Units.degreesToRadians(5)) + Algae.setMotorSpeed(Constants.AlgaeEffectorc.DEALGAFY_TOP_RPM, Constants.AlgaeEffectorc.DEALGAFY_BOTTOM_RPM, Constants.AlgaeEffectorc.SHOOT_DEALGAFY_PINCHER_RPM); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Algae.stopMotors(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 3f98a08..fc64ee5 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -25,15 +25,13 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (Algae.getArmPos() > Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) { - Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); + //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) + if (Algae.getArmPos() > Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) { + Algae.setMotorSpeed(Constants.AlgaeEffectorc.INTAKE_TOP_RPM,Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM, Constants.AlgaeEffectorc.INTAKE_TOP_RPM); if (Algae.isAlgaeIntaked()) { Algae.setTopRPM(0); Algae.setBottomRPM(0); - Algae.setArmAngle(Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE) } diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index e98bee1..762d3b8 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -16,9 +16,6 @@ public OuttakeAlgae(AlgaeEffector algaeEffector) { @Override public void initialize() { - Algae.setTopRPM(Constants.AlgaeEffectorc.OUTTAKE_TOP_RPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.OUTTAKE_BOTTOM_RPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.OUTTAKE_PINCHER_RPM); Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); timer.start(); } @@ -27,7 +24,8 @@ public void initialize() { @Override public void execute() { if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreesToRadians(20)) - + Algae.setMotorSpeed(Constants.AlgaeEffectorc.INTAKE_TOP_RPM, Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM, Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); + // if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreestoradian(5)) { // Algae.setTopRPM(-1 * Constants.AlgaeEffectorc.intakeTopRPM); // Algae.setBottomRPM(-1 * Constants.AlgaeEffectorc.intakeBottomRPM); diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index 05e5da1..8d0ada8 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -24,9 +24,7 @@ public void initialize() { @Override public void execute() { if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armShootingAngle) <= Units.degreesToRadians(5)) - Algae.setPincherRPM(Constants.AlgaeEffectorc.sho); - Algae.setTopRPM(Constants.AlgaeEffectorc.ShootingTopRPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.ShootingBottomRPM); + Algae.setMotorSpeed(Constants.AlgaeEffectorc.SHOOT_TOP_RPM, Constants.AlgaeEffectorc.SHOOT_BOTTOM_RPM, Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); } // Called once the command ends or is interrupted. From 230ced6daf9adaa7cac25f057b42925400ec3bfb Mon Sep 17 00:00:00 2001 From: AikenB Date: Sun, 23 Feb 2025 12:25:44 -0800 Subject: [PATCH 108/153] replaced setmotorspeed methods in commands since they dont use PID and added command to stop intake since the check intake method may be unreliable --- .../commands/Dealgafication.java | 6 ++- .../commands/IntakeAlgae.java | 4 +- .../commands/OuttakeAlgae.java | 6 ++- .../carlmontrobotics/commands/ShootAlgae.java | 3 +- .../commands/StopAlgaeIntake.java | 50 +++++++++++++++++++ 5 files changed, 63 insertions(+), 6 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index a3a8048..6df5837 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -23,8 +23,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE) <= Units.degreesToRadians(5)) - Algae.setMotorSpeed(Constants.AlgaeEffectorc.DEALGAFY_TOP_RPM, Constants.AlgaeEffectorc.DEALGAFY_BOTTOM_RPM, Constants.AlgaeEffectorc.SHOOT_DEALGAFY_PINCHER_RPM); + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE) <= 5) + Algae.setTopRPM(Constants.AlgaeEffectorc.DEALGAFY_TOP_RPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.DEALGAFY_BOTTOM_RPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.DEALGAFY_PINCHER_RPM); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index fc64ee5..ac0399f 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -27,7 +27,9 @@ public void initialize() { public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) if (Algae.getArmPos() > Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) { - Algae.setMotorSpeed(Constants.AlgaeEffectorc.INTAKE_TOP_RPM,Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM, Constants.AlgaeEffectorc.INTAKE_TOP_RPM); + Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); if (Algae.isAlgaeIntaked()) { Algae.setTopRPM(0); Algae.setBottomRPM(0); diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 762d3b8..e2cac61 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -23,8 +23,10 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreesToRadians(20)) - Algae.setMotorSpeed(Constants.AlgaeEffectorc.INTAKE_TOP_RPM, Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM, Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= 20) + Algae.setTopRPM(Constants.AlgaeEffectorc.OUTTAKE_TOP_RPM); + Algae.setBottomRPM(Constants.AlgaeEffectorc.OUTTAKE_BOTTOM_RPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.OUTTAKE_PINCHER_RPM); // if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreestoradian(5)) { // Algae.setTopRPM(-1 * Constants.AlgaeEffectorc.intakeTopRPM); diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index 8d0ada8..762df60 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -23,7 +23,8 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armShootingAngle) <= Units.degreesToRadians(5)) + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armShootingAngle) <= 5) + Algae.setMotorSpeed(Constants.AlgaeEffectorc.SHOOT_TOP_RPM, Constants.AlgaeEffectorc.SHOOT_BOTTOM_RPM, Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); } diff --git a/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java b/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java new file mode 100644 index 0000000..5a4c5ee --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java @@ -0,0 +1,50 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class IntakeAlgae extends Command { + private final AlgaeEffector Algae; + private final Timer timer = new Timer(); + public IntakeAlgae(AlgaeEffector Algae) { + addRequirements(this.Algae = Algae); + } + + @Override + public void initialize() { + timer.reset(); + timer.start(); + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); + } + + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() + + Algae.setTopRPM(0); + Algae.setBottomRPM(0); + Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE) + + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Algae.stopMotors(); + //TODO: Test different times + } + + @Override + public boolean isFinished() { + //distance sensor doesn't detect coral + //TODO: make distance sensor stuff + //TODO: add smartdashboard + return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + } +} \ No newline at end of file From 5bbf1026dcab32dcab29f7d7d4061f0798f62548 Mon Sep 17 00:00:00 2001 From: AikenB Date: Sun, 23 Feb 2025 12:38:29 -0800 Subject: [PATCH 109/153] Added Motor ID constants --- src/main/java/org/carlmontrobotics/Constants.java | 7 +++++-- .../org/carlmontrobotics/subsystems/AlgaeEffector.java | 2 +- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 96ed5fd..25b7642 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -27,7 +27,10 @@ public static final class OI { public static final class Driver { public static final int driverPort = 0; - public static final int effectorMotorID = 4; + public static final int EFFECTOR_TOP_MOTOR_ID = 31; + public static final int EFFECTOR_BOTTOM_MOTOR_ID = 33; + public static final int EFFECTOR_PINCHER_MOTOR_ID = 31; + public static final int EFFECTOR_ARM_MOTOR_ID = 34; public static final int effectorDistanceSensorID = 5; /*public static final int A = 1; public static final int B = 2; @@ -108,6 +111,6 @@ public static final class AlgaeEffectorc { } public static final class AlgaeArmc{ public static final int ARM_MOTOR_PORT_MASTER = 1; //to change - public static final int ARM_MOTOR_PORT_FOLLOWER = 2; //to change + //public static final int ARM_MOTOR_PORT_FOLLOWER = 2; } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index adb53b6..e4565ad 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -156,7 +156,7 @@ public void setPincherRPM(double pincherrpm) { //arm methods public void setArmPosition(double armangle) { pidControllerArm.setReference(armangle, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(pincherrpm); //WHAT IS THIS + pincherFeedforward.calculate(armangle); } public void setArmTarget(double targetPost){ 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 110/153] 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 111/153] 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 adc2a225605ccd7d633510f0d2d811ed35d429a9 Mon Sep 17 00:00:00 2001 From: Team 199 Driver Station Computer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 23 Feb 2025 15:53:29 -0800 Subject: [PATCH 112/153] fixed bunch of errors and added angle limits --- .../java/org/carlmontrobotics/Constants.java | 12 +++---- .../commands/Dealgafication.java | 6 ++-- .../commands/IntakeAlgae.java | 4 +-- .../commands/OuttakeAlgae.java | 4 +-- .../commands/SetArmToAngle.java | 2 +- .../carlmontrobotics/commands/ShootAlgae.java | 12 ++++--- .../commands/StopAlgaeIntake.java | 10 +++--- .../subsystems/AlgaeEffector.java | 32 ++++++++++++++++--- 8 files changed, 53 insertions(+), 29 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 25b7642..98cbac9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -21,7 +21,8 @@ public final class Constants { public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - + public static final double[] kG = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; + public static final class OI { public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; @@ -102,11 +103,10 @@ public static final class AlgaeEffectorc { //TODO figure out current threshold for pincher wheels public static final double PINCHER_CURRENT_THRESHOLD = 15.0; - //Bad idea to make the arm think like it can spin 360 degrees CAUSE IT CANNOT #StopGaslightingTheArm - // //max angle in radians and minimum angle in radians - // public static final double UPPER_ANGLE_LIMIT_RAD = 2.6; - // public static final double LOWER_ANGLE_LIMIT_RAD = 0; - // public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; + public static final double UPPER_ANGLE_LIMIT = 0; + public static final double LOWER_ANGLE_LIMIT = -70; + public static final double ROTATION_TO_DEG = 360; + public static final double ARM_DISCONT_DEG = -35; } public static final class AlgaeArmc{ diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index 6df5837..f28096e 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -7,16 +7,16 @@ import edu.wpi.first.math.util.Units; -public class OuttakeAlgae extends Command { +public class Dealgafication extends Command { AlgaeEffector Algae; private final Timer timer = new Timer(); - public OuttakeAlgae(AlgaeEffector algaeEffector) { + public Dealgafication(AlgaeEffector algaeEffector) { addRequirements(this.Algae = algaeEffector); } @Override public void initialize() { - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE); + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE); timer.start(); } diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index ac0399f..3f7231d 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -18,7 +18,7 @@ public IntakeAlgae(AlgaeEffector Algae) { public void initialize() { timer.reset(); timer.start(); - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); } @@ -33,7 +33,7 @@ public void execute() { if (Algae.isAlgaeIntaked()) { Algae.setTopRPM(0); Algae.setBottomRPM(0); - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE) + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); } diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index e2cac61..a448139 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -16,14 +16,14 @@ public OuttakeAlgae(AlgaeEffector algaeEffector) { @Override public void initialize() { - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= 20) + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE) <= 20) Algae.setTopRPM(Constants.AlgaeEffectorc.OUTTAKE_TOP_RPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.OUTTAKE_BOTTOM_RPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.OUTTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java index dd8220a..c4838c1 100644 --- a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java +++ b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java @@ -17,7 +17,7 @@ public SetArmToAngle(AlgaeEffector algaeEffector, double angle ) { @Override public void initialize() { - algaeEffector.setArmAngle(angle); + algaeEffector.setArmPosition(angle); timer.start(); } diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index 762df60..bb264dd 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -7,25 +7,27 @@ import edu.wpi.first.math.util.Units; -public class OuttakeAlgae extends Command { +public class ShootAlgae extends Command { AlgaeEffector Algae; private final Timer timer = new Timer(); - public OuttakeAlgae(AlgaeEffector algaeEffector) { + public ShootAlgae(AlgaeEffector algaeEffector) { addRequirements(this.Algae = algaeEffector); } @Override public void initialize() { - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armShootingAngle) <= 5) + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE) <= 5) - Algae.setMotorSpeed(Constants.AlgaeEffectorc.SHOOT_TOP_RPM, Constants.AlgaeEffectorc.SHOOT_BOTTOM_RPM, Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); + Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); + Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); + Algae.setPincherRPM(Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java b/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java index 5a4c5ee..6e4be55 100644 --- a/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java +++ b/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java @@ -7,10 +7,10 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -public class IntakeAlgae extends Command { +public class StopAlgaeIntake extends Command { private final AlgaeEffector Algae; private final Timer timer = new Timer(); - public IntakeAlgae(AlgaeEffector Algae) { + public StopAlgaeIntake(AlgaeEffector Algae) { addRequirements(this.Algae = Algae); } @@ -18,17 +18,17 @@ public IntakeAlgae(AlgaeEffector Algae) { public void initialize() { timer.reset(); timer.start(); - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() + public void execute() { Algae.setTopRPM(0); Algae.setBottomRPM(0); - Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE) + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index e4565ad..50cb3fc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -93,12 +93,18 @@ public class AlgaeEffector extends SubsystemBase { private TrapezoidProfile armTrapProfile; private TrapezoidProfile.State armGoalState = new TrapezoidProfile.State(0,0); //position,velocity (0,0) - + private static double kDt; + private TrapezoidProfile.State setPoint; private double armGoalAngle = 0; + private double armFeedVolts; + private double kG; + // private final ArmFeedforward armFeed = new ArmFeedforward(kS[ARM_ARRAY_ORDER], kG[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); //-------------------------------------------------------------------------------------------- public AlgaeEffector() { configureMotors(); + kDt = 0.02; + setPoint = getArmState(); } //---------------------------------------------------------------------------------------- @@ -137,6 +143,7 @@ private void configureMotors () { Constants.kI[ARM_ARRAY_ORDER], Constants.kD[ARM_ARRAY_ORDER] ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + armMotorConfig.encoder.positionConversionFactor(ROTATION_TO_DEG); } public void setTopRPM(double toprpm) { @@ -155,8 +162,17 @@ public void setPincherRPM(double pincherrpm) { } //arm methods public void setArmPosition(double armangle) { - pidControllerArm.setReference(armangle, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(armangle); + armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); + + armFeedVolts = armFeedforward.calculate(setPoint.position, setPoint.velocity); + if ((getArmPos() < LOWER_ANGLE_LIMIT) + || (getArmPos() > UPPER_ANGLE_LIMIT)) { + armFeedVolts = armFeedforward.calculate(getArmPos(), 0); + + } + + pidControllerArm.setReference(setPoint.position, ControlType.kPosition); + //((setPoint.position),ControlType.kPosition,armFeedVolts); } public void setArmTarget(double targetPost){ @@ -176,7 +192,13 @@ public boolean armAtGoal(){ } - + public double getArmClappedGoal(double goal) { + return MathUtil.clamp( + MathUtil.inputModulus(armGoalAngle, ARM_DISCONT_DEG, + ARM_DISCONT_DEG + 360), + LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT + ); + } public double getArmPos() { @@ -236,7 +258,7 @@ public boolean isAlgaeIntaked() { @Override public void periodic() { pidControllerArm.setReference(armGoalAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(goalAngle); //What is this for + pincherFeedforward.calculate(armGoalAngle); //What is this for } } From 259bd66091923bc84b6ab140ab89516fe9e4a728 Mon Sep 17 00:00:00 2001 From: FriedLongJohns - WindowsLaptop <81837862+FriedLongJohns@users.noreply.github.com> Date: Sun, 23 Feb 2025 18:32:30 -0600 Subject: [PATCH 113/153] Added Bindings and some SmartDashboard Values --- .../org/carlmontrobotics/RobotContainer.java | 14 +++++- .../commands/Dealgafication.java | 3 +- .../commands/IntakeAlgae.java | 16 +++--- .../commands/OuttakeAlgae.java | 4 +- .../commands/SetArmToAngle.java | 39 --------------- .../carlmontrobotics/commands/ShootAlgae.java | 2 +- .../commands/StopAlgaeIntake.java | 50 ------------------- .../subsystems/AlgaeEffector.java | 11 ++-- 8 files changed, 31 insertions(+), 108 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b46e8d3..a9e8994 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -13,14 +13,16 @@ import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI.Manipulator; import org.carlmontrobotics.commands.OuttakeAlgae; +import org.carlmontrobotics.commands.ShootAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; - +import org.carlmontrobotics.commands.Dealgafication; import org.carlmontrobotics.commands.IntakeAlgae; //controllers import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj.XboxController.Axis; @@ -73,7 +75,15 @@ private void setBindingsManipulator() { new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) .whileFalse(new OuttakeAlgae(algaeEffector)); - } + + new JoystickButton(manipulatorController, XboxController.Button.kA.value) + .onTrue(new Dealgafication(algaeEffector)); + + new JoystickButton(manipulatorController, XboxController.Button.kB.value) + .onTrue(new ShootAlgae(algaeEffector)); + new JoystickButton(manipulatorController, XboxController.Button.kY.value) + .onTrue(new InstantCommand(()->{algaeEffector.stopMotors();})); + } private Trigger axisTrigger(GenericHID controller, Axis axis) { diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index f28096e..e44a89c 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -38,6 +38,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return timer.getFPGATimestamp()>5 || Algae.isAlgaeIntaked(); + //return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 3f7231d..5ab8755 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -10,6 +10,7 @@ public class IntakeAlgae extends Command { private final AlgaeEffector Algae; private final Timer timer = new Timer(); + private boolean done = false; public IntakeAlgae(AlgaeEffector Algae) { addRequirements(this.Algae = Algae); } @@ -26,17 +27,14 @@ public void initialize() { @Override public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (Algae.getArmPos() > Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) { + if (Algae.getArmPos() > Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) {//FIXME do the eror thing Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); - if (Algae.isAlgaeIntaked()) { - Algae.setTopRPM(0); - Algae.setBottomRPM(0); - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); - } - - + } + if (Algae.isAlgaeIntaked()) { + Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); + done = true; } } @@ -52,6 +50,6 @@ public boolean isFinished() { //distance sensor doesn't detect coral //TODO: make distance sensor stuff //TODO: add smartdashboard - return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return done || timer.getFPGATimestamp()>5; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index a448139..b4e8fb7 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -44,7 +44,7 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override - public boolean isFinished() { - return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + public boolean isFinished() {//FIXME DO THIS + return timer.getFPGATimestamp()>3; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java b/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java deleted file mode 100644 index c4838c1..0000000 --- a/src/main/java/org/carlmontrobotics/commands/SetArmToAngle.java +++ /dev/null @@ -1,39 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AlgaeEffector; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; - -public class SetArmToAngle extends Command { - AlgaeEffector algaeEffector; - double angle; - private final Timer timer = new Timer(); - public SetArmToAngle(AlgaeEffector algaeEffector, double angle ) { - addRequirements(this.algaeEffector = algaeEffector); - this.angle = angle; - } - - @Override - public void initialize() { - algaeEffector.setArmPosition(angle); - timer.start(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - } - - // 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; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index bb264dd..f0b714c 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -39,6 +39,6 @@ public void end(boolean interrupted) { // Returns true when the command should end. @Override public boolean isFinished() { - return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) + return timer.getFPGATimestamp() > 3; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java b/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java deleted file mode 100644 index 6e4be55..0000000 --- a/src/main/java/org/carlmontrobotics/commands/StopAlgaeIntake.java +++ /dev/null @@ -1,50 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants; -import org.carlmontrobotics.subsystems.AlgaeEffector; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; - -public class StopAlgaeIntake extends Command { - private final AlgaeEffector Algae; - private final Timer timer = new Timer(); - public StopAlgaeIntake(AlgaeEffector Algae) { - addRequirements(this.Algae = Algae); - } - - @Override - public void initialize() { - timer.reset(); - timer.start(); - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); - } - - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - Algae.setTopRPM(0); - Algae.setBottomRPM(0); - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); - - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - Algae.stopMotors(); - //TODO: Test different times - } - - @Override - public boolean isFinished() { - //distance sensor doesn't detect coral - //TODO: make distance sensor stuff - //TODO: add smartdashboard - return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) - } -} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 50cb3fc..712fc85 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -186,9 +186,8 @@ public TrapezoidProfile.State getArmState(){ } public boolean armAtGoal(){ - //TODO: - //reutrns if arm is at goal state - need to add toelrance - return false; + + return Math.abs(getArmPos()-armGoalAngle) <= 5; } @@ -238,7 +237,7 @@ public boolean checkIfAtTopRPM(double rpm) { } public boolean checkIfAtBottomRPM(double rpm) { - return bottomEncoder.getVelocity() == rpm; + return bottomEncoder.getVelocity() == rpm;//give like a 5% error or somethin } public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpeed) { @@ -259,6 +258,10 @@ public boolean isAlgaeIntaked() { public void periodic() { pidControllerArm.setReference(armGoalAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); pincherFeedforward.calculate(armGoalAngle); //What is this for + SmartDashboard.putNumber("Arm Angle", getArmPos()); + SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); + SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); + } } From f73fb8daa466833f67206dc976c5693bbbd1ef81 Mon Sep 17 00:00:00 2001 From: AikenB Date: Sun, 23 Feb 2025 18:38:21 -0800 Subject: [PATCH 114/153] added buttons for commands for smartdashboard --- .../java/org/carlmontrobotics/commands/IntakeAlgae.java | 2 +- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 5ab8755..c05efd4 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -27,7 +27,7 @@ public void initialize() { @Override public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (Algae.getArmPos() > Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) {//FIXME do the eror thing + if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE)) { Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 712fc85..e8f023a 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -191,7 +191,7 @@ public boolean armAtGoal(){ } - public double getArmClappedGoal(double goal) { + public double getArmClappedGoal() { return MathUtil.clamp( MathUtil.inputModulus(armGoalAngle, ARM_DISCONT_DEG, ARM_DISCONT_DEG + 360), @@ -261,6 +261,10 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", getArmPos()); SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); + SmartDashboard.putData("Dealgafication", new Dealgafication()); + SmartDashboard.putData("Intake Algae", new IntakeAlgae()); + SmartDashboard.putData("Outtake Algae", new OuttakeAlgae()); + SmartDashboard.putData("Shoot Algae", new ShootAlgae()); } From 5e3a667eff3e3d3f6ec4591595f75c89960af644 Mon Sep 17 00:00:00 2001 From: AikenB Date: Sun, 23 Feb 2025 19:27:20 -0800 Subject: [PATCH 115/153] set variable armGoalAngle to the setArmPosition() input and simplified some if statements in the commands --- .../java/org/carlmontrobotics/commands/Dealgafication.java | 2 +- src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java | 2 +- .../java/org/carlmontrobotics/commands/OuttakeAlgae.java | 2 +- src/main/java/org/carlmontrobotics/commands/ShootAlgae.java | 2 +- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 5 +++-- 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index e44a89c..ef26929 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE) <= 5) + if (armAtGoal(5)) Algae.setTopRPM(Constants.AlgaeEffectorc.DEALGAFY_TOP_RPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.DEALGAFY_BOTTOM_RPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.DEALGAFY_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index c05efd4..d93f327 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -27,7 +27,7 @@ public void initialize() { @Override public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE)) { + if (armAtGoal(20)) { Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index b4e8fb7..abe48ca 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE) <= 20) + if (armAtGoal(20)) Algae.setTopRPM(Constants.AlgaeEffectorc.OUTTAKE_TOP_RPM); Algae.setBottomRPM(Constants.AlgaeEffectorc.OUTTAKE_BOTTOM_RPM); Algae.setPincherRPM(Constants.AlgaeEffectorc.OUTTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index f0b714c..51aa39b 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE) <= 5) + if (armAtGoal(5)) Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index e8f023a..c0b15c7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -162,6 +162,7 @@ public void setPincherRPM(double pincherrpm) { } //arm methods public void setArmPosition(double armangle) { + armGoalAngle = armangle; armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); armFeedVolts = armFeedforward.calculate(setPoint.position, setPoint.velocity); @@ -185,9 +186,9 @@ public TrapezoidProfile.State getArmState(){ return armState; } - public boolean armAtGoal(){ + public boolean armAtGoal(ErrorMargin){ - return Math.abs(getArmPos()-armGoalAngle) <= 5; + return Math.abs(getArmPos()-armGoalAngle) <= ErrorMargin; } From 9ca1011b5fead1402f16752899c4c00ec8ab7b36 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sun, 23 Feb 2025 22:18:35 -0800 Subject: [PATCH 116/153] Added magnetic limit switch, more smartdashboard values --- .../java/org/carlmontrobotics/Constants.java | 8 ++-- .../carlmontrobotics/subsystems/Elevator.java | 47 +++++++++++++++---- 2 files changed, 44 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 2501464..b9734ea 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -16,9 +16,11 @@ public static final class Manipulator { } public static final class Elevatorc { //ports - public static final int masterPort = 19; - public static final int followerPort = 20; - public static final int elevatorLimitSwitchPort = 1; + public static final int masterPort = 20; + public static final int followerPort = 21; + public static final int elevatorTopLimitSwitchPort = 1; + public static final int elevatorBottomLimitSwitchPort = 2; + //Config //TODO figure these parts out diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 013adfe..faf6a26 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Servo; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -33,12 +34,15 @@ public class Elevator extends SubsystemBase { private SparkMax followerMotor; private SparkMaxConfig followerConfig; private RelativeEncoder followerEncoder; - // Caliberation - private DigitalInput limitSwitch; + // Limit Switches + private DigitalInput topLimitSwitch; + private DigitalInput bottomLimitSwitch; //Vars private double heightGoal; + private int elevatorState; //PID private SparkClosedLoopController pidElevatorController; + private Timer timer; public Elevator() { //motors @@ -50,10 +54,13 @@ public Elevator() { followerEncoder = followerMotor.getEncoder(); configureMotors(); - //Calibration - limitSwitch = new DigitalInput(Constants.Elevatorc.elevatorLimitSwitchPort); + //Calibration + topLimitSwitch = new DigitalInput(Constants.Elevatorc.elevatorTopLimitSwitchPort); + bottomLimitSwitch = new DigitalInput(Constants.Elevatorc.elevatorBottomLimitSwitchPort); + timer = new Timer(); + timer.start(); - //PID + //PID pidElevatorController = masterMotor.getClosedLoopController(); } @@ -96,10 +103,25 @@ public double getGoal() { public double getCurrentHeight() { return masterEncoder.getPosition(); } - + + public boolean elevatorAtMax() { + return !topLimitSwitch.get(); + } + + public boolean elevatorAtMin() { + return !bottomLimitSwitch.get(); + } + public void updateEncoders() { - if (!limitSwitch.get()) { + if (elevatorAtMax()) { masterEncoder.setPosition(Constants.Elevatorc.maxElevatorHeightInches); + timer.reset(); + timer.start(); + } + else if (elevatorAtMin()) { + masterEncoder.setPosition(Constants.Elevatorc.minElevatorHeightInches); + timer.reset(); + timer.start(); } } @@ -121,8 +143,17 @@ public double getEncoderValue() { @Override public void periodic() { - SmartDashboard.putBoolean("MaxHeight", limitSwitch.get()); + if (elevatorAtMax()){ + SmartDashboard.putString("ElevatorState", "🔴STOP🔴"); + } + else if (elevatorAtMin()) { + SmartDashboard.putString("ElevatorState", "🟢GO🟢"); + } + else { + SmartDashboard.putString("ElevatorState", "🟡CAUTION🟡"); + } SmartDashboard.putNumber("Elevator Height", getCurrentHeight()); + SmartDashboard.putNumber("Since Calibrated", timer.get()); updateEncoders(); getToGoal(); } From 00291ee33ee73c9d7ed30da1b81aeb968357bbec Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 23 Feb 2025 23:14:30 -0800 Subject: [PATCH 117/153] update arm code, create arm to position --- .../java/org/carlmontrobotics/Constants.java | 7 ++-- .../commands/ArmToPosition.java | 33 +++++++++++++++++++ .../subsystems/AlgaeEffector.java | 17 +++++++--- 3 files changed, 48 insertions(+), 9 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/ArmToPosition.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 98cbac9..c4ac500 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -108,9 +108,8 @@ public static final class AlgaeEffectorc { public static final double ROTATION_TO_DEG = 360; public static final double ARM_DISCONT_DEG = -35; + public static final double ARM_TOLERANCE = 1.0; //need to change + } - public static final class AlgaeArmc{ - public static final int ARM_MOTOR_PORT_MASTER = 1; //to change - //public static final int ARM_MOTOR_PORT_FOLLOWER = 2; - } + } diff --git a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java new file mode 100644 index 0000000..ace9f44 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java @@ -0,0 +1,33 @@ +package org.carlmontrobotics.commands; + +import org.carlmontrobotics.subsystems.AlgaeEffector; + +import edu.wpi.first.wpilibj2.command.Command; + + +public class ArmToPosition extends Command{ + AlgaeEffector algaeArm; + private double goalPosition; //radians - need angle for dealgfication and ground pickup + + public ArmToPosition(AlgaeEffector algaeArm, double goalPosition){ + addRequirements(this.algaeArm = algaeArm); + this.goalPosition = goalPosition; + } + + @Override + public void initialize(){ + algaeArm.setArmTarget(goalPosition); //sets target to position in rads + + + } + public void execute(){ + + } + public void end(){ + + } + public boolean isFinished(){ + return algaeArm.armAtGoal(); + + } +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index c0b15c7..85363c6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -9,6 +9,10 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; +import org.carlmontrobotics.commands.Dealgafication; +import org.carlmontrobotics.commands.IntakeAlgae; +import org.carlmontrobotics.commands.OuttakeAlgae; +import org.carlmontrobotics.commands.ShootAlgae; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; @@ -176,7 +180,10 @@ public void setArmPosition(double armangle) { //((setPoint.position),ControlType.kPosition,armFeedVolts); } - public void setArmTarget(double targetPost){ + public void setArmTarget(double targetPos){ + armGoalState.position = getArmClappedGoal(targetPos); + armGoalState.velocity = 0; + } } @@ -186,15 +193,15 @@ public TrapezoidProfile.State getArmState(){ return armState; } - public boolean armAtGoal(ErrorMargin){ + public boolean armAtGoal(){ //TODO: make error margin a constant - return Math.abs(getArmPos()-armGoalAngle) <= ErrorMargin; + return Math.abs(getArmPos()-armGoalAngle) <= ARM_TOLERANCE; } - public double getArmClappedGoal() { + public double getArmClappedGoal(double goalAngle) { return MathUtil.clamp( - MathUtil.inputModulus(armGoalAngle, ARM_DISCONT_DEG, + MathUtil.inputModulus(goalAngle, ARM_DISCONT_DEG, ARM_DISCONT_DEG + 360), LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT ); From d4574101c146bd3e356d3a26d0240d8c487fbcbd Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Sun, 23 Feb 2025 23:22:16 -0800 Subject: [PATCH 118/153] fixed variable names --- .../java/org/carlmontrobotics/Constants.java | 9 ++++--- .../commands/Dealgafication.java | 20 ++++++++-------- .../commands/IntakeAlgae.java | 24 +++++++++---------- .../commands/OuttakeAlgae.java | 20 ++++++++-------- .../carlmontrobotics/commands/ShootAlgae.java | 2 +- .../subsystems/AlgaeEffector.java | 2 +- 6 files changed, 40 insertions(+), 37 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index c4ac500..80c44f2 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -51,6 +51,10 @@ public static final class Manipulator { } } public static final class AlgaeEffectorc { + + //EFFECTOR + + public static final int UPPER_MOTOR_PORT = 1; public static final int LOWER_MOTOR_PORT = 2; public static final int PINCH_MOTOR_PORT = 3; @@ -87,8 +91,6 @@ public static final class AlgaeEffectorc { public static final double ARM_CHAIN_GEARING = 16.0/34; public static final double ARM_GEAR_RATIO = 1.0/3; - - //TODO figure the zero out once encoder is on public static final double ARM_TO_ZERO = 0; //Pure vertical down //TODO ask samo for angle to intake algae from pure vertical down @@ -108,7 +110,8 @@ public static final class AlgaeEffectorc { public static final double ROTATION_TO_DEG = 360; public static final double ARM_DISCONT_DEG = -35; - public static final double ARM_TOLERANCE = 1.0; //need to change + public static final double ARM_TOLERANCE = 1.0; //need to change, tolerance for armATPOSITION + } diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index ef26929..51b2a88 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -1,5 +1,5 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.Constants; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; @@ -8,37 +8,37 @@ public class Dealgafication extends Command { - AlgaeEffector Algae; + AlgaeEffector algae; private final Timer timer = new Timer(); public Dealgafication(AlgaeEffector algaeEffector) { - addRequirements(this.Algae = algaeEffector); + addRequirements(this.algae = algaeEffector); } @Override public void initialize() { - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_DEALGAFYING_ANGLE); + algae.setArmPosition(ARM_DEALGAFYING_ANGLE); timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (armAtGoal(5)) - Algae.setTopRPM(Constants.AlgaeEffectorc.DEALGAFY_TOP_RPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.DEALGAFY_BOTTOM_RPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.DEALGAFY_PINCHER_RPM); + if (algae.armAtGoal()) + algae.setTopRPM(DEALGAFY_TOP_RPM); + algae.setBottomRPM(DEALGAFY_BOTTOM_RPM); + algae.setPincherRPM(DEALGAFY_PINCHER_RPM); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - Algae.stopMotors(); + algae.stopMotors(); } // Returns true when the command should end. @Override public boolean isFinished() { - return timer.getFPGATimestamp()>5 || Algae.isAlgaeIntaked(); + return timer.getFPGATimestamp()>5 || algae.isAlgaeIntaked(); //return false; //Simulator doesnt work propperly because limiswtich is non existant (only for simulator) } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index d93f327..5dc46e4 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -2,24 +2,24 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; - +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; public class IntakeAlgae extends Command { - private final AlgaeEffector Algae; + private final AlgaeEffector algae; private final Timer timer = new Timer(); private boolean done = false; - public IntakeAlgae(AlgaeEffector Algae) { - addRequirements(this.Algae = Algae); + public IntakeAlgae(AlgaeEffector algae) { + addRequirements(this.algae = algae); } @Override public void initialize() { timer.reset(); timer.start(); - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); + algae.setArmPosition(ARM_INTAKE_ANGLE); } @@ -27,13 +27,13 @@ public void initialize() { @Override public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (armAtGoal(20)) { - Algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); + if (algae.armAtGoal()) { + algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); + algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); + algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); } - if (Algae.isAlgaeIntaked()) { - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); + if (algae.isAlgaeIntaked()) { + algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); done = true; } } @@ -41,7 +41,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - Algae.stopMotors(); + algae.stopMotors(); //TODO: Test different times } diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index abe48ca..76145d0 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -1,32 +1,32 @@ package org.carlmontrobotics.commands; -import org.carlmontrobotics.Constants; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; import org.carlmontrobotics.subsystems.AlgaeEffector; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.math.util.Units; + public class OuttakeAlgae extends Command { - AlgaeEffector Algae; + AlgaeEffector algae; private final Timer timer = new Timer(); public OuttakeAlgae(AlgaeEffector algaeEffector) { - addRequirements(this.Algae = algaeEffector); + addRequirements(this.algae = algaeEffector); } @Override public void initialize() { - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE); + algae.setArmPosition(ARM_INTAKE_ANGLE); timer.start(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (armAtGoal(20)) - Algae.setTopRPM(Constants.AlgaeEffectorc.OUTTAKE_TOP_RPM); - Algae.setBottomRPM(Constants.AlgaeEffectorc.OUTTAKE_BOTTOM_RPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.OUTTAKE_PINCHER_RPM); + if (algae.armAtGoal()) + algae.setTopRPM(OUTTAKE_TOP_RPM); + algae.setBottomRPM(OUTTAKE_BOTTOM_RPM); + algae.setPincherRPM(OUTTAKE_PINCHER_RPM); // if (Math.abs(Algae.getArmPos()-Constants.AlgaeEffectorc.armRestingAngleWhileIntakeAlgae) <= Units.degreestoradian(5)) { // Algae.setTopRPM(-1 * Constants.AlgaeEffectorc.intakeTopRPM); @@ -39,7 +39,7 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - Algae.stopMotors(); + algae.stopMotors(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index 51aa39b..749f75a 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (armAtGoal(5)) + if (Algae.armAtGoal()) Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 85363c6..239cd59 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -269,7 +269,7 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", getArmPos()); SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); - SmartDashboard.putData("Dealgafication", new Dealgafication()); + SmartDashboard.putData("Dealgafication", new Dealgafication());//need params for these SmartDashboard.putData("Intake Algae", new IntakeAlgae()); SmartDashboard.putData("Outtake Algae", new OuttakeAlgae()); SmartDashboard.putData("Shoot Algae", new ShootAlgae()); From 3d4cd09e3f22e2674271f75c428452984ec31398 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Mon, 24 Feb 2025 16:04:14 -0800 Subject: [PATCH 119/153] changes to constants --- src/main/java/org/carlmontrobotics/Constants.java | 3 ++- .../java/org/carlmontrobotics/commands/ArmToPosition.java | 7 +++++++ .../java/org/carlmontrobotics/commands/Dealgafication.java | 2 +- .../java/org/carlmontrobotics/commands/IntakeAlgae.java | 2 +- .../java/org/carlmontrobotics/commands/OuttakeAlgae.java | 2 +- .../java/org/carlmontrobotics/commands/ShootAlgae.java | 4 +++- .../org/carlmontrobotics/subsystems/AlgaeEffector.java | 4 ++-- 7 files changed, 17 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 80c44f2..2afdf24 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -110,7 +110,8 @@ public static final class AlgaeEffectorc { public static final double ROTATION_TO_DEG = 360; public static final double ARM_DISCONT_DEG = -35; - public static final double ARM_TOLERANCE = 1.0; //need to change, tolerance for armATPOSITION + public static final double ARM_TOLERANCE_SHOOT = 5; //degrees + public static final double ARM_TOLERANCE_INTAKE = 20;//degress } diff --git a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java index ace9f44..a9df76f 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java @@ -8,10 +8,17 @@ public class ArmToPosition extends Command{ AlgaeEffector algaeArm; private double goalPosition; //radians - need angle for dealgfication and ground pickup + private boolean isShooting; + private boolean isIntake; public ArmToPosition(AlgaeEffector algaeArm, double goalPosition){ addRequirements(this.algaeArm = algaeArm); this.goalPosition = goalPosition; + this.isShooting = shoot; + this.isIntake = intake; + + + } @Override diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index 51b2a88..6992d33 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (algae.armAtGoal()) + if (algae.armAtGoal(ARM_TOLERANCE_SHOOT)) //TODO: is this the right tolerance?? algae.setTopRPM(DEALGAFY_TOP_RPM); algae.setBottomRPM(DEALGAFY_BOTTOM_RPM); algae.setPincherRPM(DEALGAFY_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index 5dc46e4..b7a0936 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -27,7 +27,7 @@ public void initialize() { @Override public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (algae.armAtGoal()) { + if (algae.armAtGoal(ARM_TOLERANCE_INTAKE)) { algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index 76145d0..e6c554d 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (algae.armAtGoal()) + if (algae.armAtGoal(ARM_TOLERANCE_SHOOT)) algae.setTopRPM(OUTTAKE_TOP_RPM); algae.setBottomRPM(OUTTAKE_BOTTOM_RPM); algae.setPincherRPM(OUTTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index 749f75a..626c57b 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -1,4 +1,6 @@ package org.carlmontrobotics.commands; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE; + import org.carlmontrobotics.Constants; import org.carlmontrobotics.subsystems.AlgaeEffector; @@ -23,7 +25,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Algae.armAtGoal()) + if (Algae.armAtGoal(ARM_SHOOT_ANGLE)) Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 239cd59..964e4bf 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -193,9 +193,9 @@ public TrapezoidProfile.State getArmState(){ return armState; } - public boolean armAtGoal(){ //TODO: make error margin a constant + public boolean armAtGoal(double errorMargin){ //TODO: make error margin a constant - return Math.abs(getArmPos()-armGoalAngle) <= ARM_TOLERANCE; + return Math.abs(getArmPos()-armGoalAngle) <= errorMargin; } From 3850265236799b8c398fb9209ae5829046226ab6 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Tue, 25 Feb 2025 16:12:00 -0800 Subject: [PATCH 120/153] armToPos --- src/main/java/org/carlmontrobotics/Constants.java | 3 +-- .../carlmontrobotics/commands/ArmToPosition.java | 6 ++---- .../carlmontrobotics/commands/Dealgafication.java | 9 +++++---- .../carlmontrobotics/commands/IntakeAlgae.java | 15 +++++++++------ .../carlmontrobotics/commands/OuttakeAlgae.java | 2 +- .../org/carlmontrobotics/commands/ShootAlgae.java | 2 +- .../subsystems/AlgaeEffector.java | 14 +++++++++----- 7 files changed, 28 insertions(+), 23 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 2afdf24..debb9b2 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -110,8 +110,7 @@ public static final class AlgaeEffectorc { public static final double ROTATION_TO_DEG = 360; public static final double ARM_DISCONT_DEG = -35; - public static final double ARM_TOLERANCE_SHOOT = 5; //degrees - public static final double ARM_TOLERANCE_INTAKE = 20;//degress + public static final double ARM_ERROR_MARGIN = 1; } diff --git a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java index a9df76f..e5c7971 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java @@ -8,14 +8,12 @@ public class ArmToPosition extends Command{ AlgaeEffector algaeArm; private double goalPosition; //radians - need angle for dealgfication and ground pickup - private boolean isShooting; - private boolean isIntake; + public ArmToPosition(AlgaeEffector algaeArm, double goalPosition){ addRequirements(this.algaeArm = algaeArm); this.goalPosition = goalPosition; - this.isShooting = shoot; - this.isIntake = intake; + diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java index 6992d33..b2c5d88 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/Dealgafication.java @@ -23,10 +23,11 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (algae.armAtGoal(ARM_TOLERANCE_SHOOT)) //TODO: is this the right tolerance?? - algae.setTopRPM(DEALGAFY_TOP_RPM); - algae.setBottomRPM(DEALGAFY_BOTTOM_RPM); - algae.setPincherRPM(DEALGAFY_PINCHER_RPM); + //call command arm toposition + + algae.setTopRPM(DEALGAFY_TOP_RPM); + algae.setBottomRPM(DEALGAFY_BOTTOM_RPM); + algae.setPincherRPM(DEALGAFY_PINCHER_RPM); } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java index b7a0936..cc1fa14 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java @@ -27,15 +27,18 @@ public void initialize() { @Override public void execute() { //Algae.setArmAngle(Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE) - if (algae.armAtGoal(ARM_TOLERANCE_INTAKE)) { - algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); - algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); - algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); - } - if (algae.isAlgaeIntaked()) { + + algae.setTopRPM(Constants.AlgaeEffectorc.INTAKE_TOP_RPM); + algae.setBottomRPM(Constants.AlgaeEffectorc.INTAKE_BOTTOM_RPM); + algae.setPincherRPM(Constants.AlgaeEffectorc.INTAKE_PINCHER_RPM); + + /* + * if (algae.isAlgaeIntaked()) { algae.setArmPosition(Constants.AlgaeEffectorc.ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE); done = true; } + */ + } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index e6c554d..a92706f 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (algae.armAtGoal(ARM_TOLERANCE_SHOOT)) + algae.setTopRPM(OUTTAKE_TOP_RPM); algae.setBottomRPM(OUTTAKE_BOTTOM_RPM); algae.setPincherRPM(OUTTAKE_PINCHER_RPM); diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index 626c57b..cc2e5fd 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -25,7 +25,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (Algae.armAtGoal(ARM_SHOOT_ANGLE)) + Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 964e4bf..ea1664e 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -185,18 +185,22 @@ public void setArmTarget(double targetPos){ armGoalState.velocity = 0; } - } + //returns the arm position and velocity based on encoder and position public TrapezoidProfile.State getArmState(){ TrapezoidProfile.State armState = new TrapezoidProfile.State(getArmPos(), getArmVel()); return armState; } + //overload public boolean armAtGoal(double errorMargin){ //TODO: make error margin a constant return Math.abs(getArmPos()-armGoalAngle) <= errorMargin; } + public boolean armAtGoal(){ + return Math.abs(getArmPos()-armGoalAngle) <= ARM_ERROR_MARGIN; + } public double getArmClappedGoal(double goalAngle) { @@ -269,10 +273,10 @@ public void periodic() { SmartDashboard.putNumber("Arm Angle", getArmPos()); SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); - SmartDashboard.putData("Dealgafication", new Dealgafication());//need params for these - SmartDashboard.putData("Intake Algae", new IntakeAlgae()); - SmartDashboard.putData("Outtake Algae", new OuttakeAlgae()); - SmartDashboard.putData("Shoot Algae", new ShootAlgae()); + SmartDashboard.putData("Dealgafication", new Dealgafication(this));//need params for these + SmartDashboard.putData("Intake Algae", new IntakeAlgae(this)); + SmartDashboard.putData("Outtake Algae", new OuttakeAlgae(this)); + SmartDashboard.putData("Shoot Algae", new ShootAlgae(this)); } From 64e2180d0328e2963df92d030ccb18080140558a Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Tue, 25 Feb 2025 16:41:01 -0800 Subject: [PATCH 121/153] j --- .../java/org/carlmontrobotics/Constants.java | 2 +- .../org/carlmontrobotics/RobotContainer.java | 18 ++++++++++++++++-- .../commands/ArmToPosition.java | 3 --- 3 files changed, 17 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index debb9b2..99eb7d1 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -47,7 +47,7 @@ public static final class Manipulator { public static final Axis OuttakeTrigger = Axis.kRightTrigger; public static final Axis IntakeTrigger = Axis.kLeftTrigger; public static final int OuttakeBumper = Button.kRightBumper.value; - public static final int IntakeBumper = Button.kLeftBumper.value; + public static final int INTAKE_BUMPER = Button.kLeftBumper.value; } } public static final class AlgaeEffectorc { diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index a9e8994..d4a9a1d 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -8,6 +8,7 @@ // import org.carlmontrobotics.subsystems.*; // import org.carlmontrobotics.commands.*; import static org.carlmontrobotics.Constants.OI.*; +import static org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.Constants.OI; import org.carlmontrobotics.Constants.OI.Manipulator.*; @@ -15,9 +16,12 @@ import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.commands.ShootAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; +import org.carlmontrobotics.commands.ArmToPosition; import org.carlmontrobotics.commands.Dealgafication; import org.carlmontrobotics.commands.IntakeAlgae; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; + //controllers @@ -70,8 +74,18 @@ private void setBindingsDriver() { private void setBindingsManipulator() { - new JoystickButton(manipulatorController, OI.Manipulator.IntakeBumper) - .whileTrue(new IntakeAlgae(algaeEffector)); + //intake + new JoystickButton(manipulatorController, INTAKE_BUMPER) + .whileTrue(new SequentialCommandGroup( + new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), + new IntakeAlgae(algaeEffector) + )); + + + //outake + + //dealgify + new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) .whileFalse(new OuttakeAlgae(algaeEffector)); diff --git a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java index e5c7971..b167cec 100644 --- a/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java +++ b/src/main/java/org/carlmontrobotics/commands/ArmToPosition.java @@ -14,9 +14,6 @@ public ArmToPosition(AlgaeEffector algaeArm, double goalPosition){ addRequirements(this.algaeArm = algaeArm); this.goalPosition = goalPosition; - - - } @Override From 57cbf5e35c7e54c3bf17edbfc787316d55ff9c73 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Tue, 25 Feb 2025 16:52:47 -0800 Subject: [PATCH 122/153] comments --- .../org/carlmontrobotics/subsystems/AlgaeEffector.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index ea1664e..bf4610e 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -165,7 +165,11 @@ public void setPincherRPM(double pincherrpm) { pincherFeedforward.calculate(pincherrpm); } //arm methods + + + //drives arm from set point to goal position public void setArmPosition(double armangle) { + armGoalAngle = armangle; armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); @@ -179,7 +183,8 @@ public void setArmPosition(double armangle) { pidControllerArm.setReference(setPoint.position, ControlType.kPosition); //((setPoint.position),ControlType.kPosition,armFeedVolts); } - + + // public void setArmTarget(double targetPos){ armGoalState.position = getArmClappedGoal(targetPos); armGoalState.velocity = 0; From d70480f1903d44f5b7f41c6a3c9f6e46171083f2 Mon Sep 17 00:00:00 2001 From: Kenneth-Choothakan <134165881+Kenneth-Choothakan@users.noreply.github.com> Date: Tue, 25 Feb 2025 17:18:26 -0800 Subject: [PATCH 123/153] Deleted useless variables and fixed setarmposition() --- .../subsystems/AlgaeEffector.java | 23 +++++++++---------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index ea1664e..744cbfd 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -99,7 +99,7 @@ public class AlgaeEffector extends SubsystemBase { private static double kDt; private TrapezoidProfile.State setPoint; - private double armGoalAngle = 0; + private double armFeedVolts; private double kG; // private final ArmFeedforward armFeed = new ArmFeedforward(kS[ARM_ARRAY_ORDER], kG[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); @@ -165,18 +165,20 @@ public void setPincherRPM(double pincherrpm) { pincherFeedforward.calculate(pincherrpm); } //arm methods - public void setArmPosition(double armangle) { - armGoalAngle = armangle; + public void setArmPosition() { + + setPoint = getArmState(); armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); - armFeedVolts = armFeedforward.calculate(setPoint.position, setPoint.velocity); + armFeedVolts = armFeedforward.calculate(armGoalState.position, armGoalState.velocity); if ((getArmPos() < LOWER_ANGLE_LIMIT) || (getArmPos() > UPPER_ANGLE_LIMIT)) { armFeedVolts = armFeedforward.calculate(getArmPos(), 0); } - pidControllerArm.setReference(setPoint.position, ControlType.kPosition); + + pidControllerArm.setReference(armGoalState.position, ControlType.kPosition,ClosedLoopSlot.kSlot0, armFeedVolts); //((setPoint.position),ControlType.kPosition,armFeedVolts); } @@ -194,12 +196,9 @@ public TrapezoidProfile.State getArmState(){ } //overload - public boolean armAtGoal(double errorMargin){ //TODO: make error margin a constant - - return Math.abs(getArmPos()-armGoalAngle) <= errorMargin; - } + public boolean armAtGoal(){ - return Math.abs(getArmPos()-armGoalAngle) <= ARM_ERROR_MARGIN; + return Math.abs(getArmPos()-armGoalState.position) <= ARM_ERROR_MARGIN; } @@ -268,11 +267,11 @@ public boolean isAlgaeIntaked() { @Override public void periodic() { - pidControllerArm.setReference(armGoalAngle, ControlType.kPosition, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(armGoalAngle); //What is this for + setArmPosition(); SmartDashboard.putNumber("Arm Angle", getArmPos()); SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); + SmartDashboard.putNumber("Arm Velocity", getArmVel()); SmartDashboard.putData("Dealgafication", new Dealgafication(this));//need params for these SmartDashboard.putData("Intake Algae", new IntakeAlgae(this)); SmartDashboard.putData("Outtake Algae", new OuttakeAlgae(this)); From d48b4fce5fc2e7e6432cfd3946bb1d518e70b729 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Tue, 25 Feb 2025 17:24:06 -0800 Subject: [PATCH 124/153] merge --- .../org/carlmontrobotics/RobotContainer.java | 8 ++++---- ...fication.java => DealgaficationIntake.java} | 10 ++++++---- ...IntakeAlgae.java => GroundIntakeAlgae.java} | 7 ++++--- .../commands/OuttakeAlgae.java | 4 +++- .../carlmontrobotics/commands/ShootAlgae.java | 18 ++++++++++-------- .../subsystems/AlgaeEffector.java | 6 +++--- 6 files changed, 30 insertions(+), 23 deletions(-) rename src/main/java/org/carlmontrobotics/commands/{Dealgafication.java => DealgaficationIntake.java} (82%) rename src/main/java/org/carlmontrobotics/commands/{IntakeAlgae.java => GroundIntakeAlgae.java} (90%) diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index d4a9a1d..c6f1e2f 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -17,8 +17,8 @@ import org.carlmontrobotics.commands.ShootAlgae; import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.commands.ArmToPosition; -import org.carlmontrobotics.commands.Dealgafication; -import org.carlmontrobotics.commands.IntakeAlgae; +import org.carlmontrobotics.commands.DealgaficationIntake; +import org.carlmontrobotics.commands.GroundIntakeAlgae; import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; @@ -78,7 +78,7 @@ private void setBindingsManipulator() { new JoystickButton(manipulatorController, INTAKE_BUMPER) .whileTrue(new SequentialCommandGroup( new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), - new IntakeAlgae(algaeEffector) + new GroundIntakeAlgae(algaeEffector) )); @@ -91,7 +91,7 @@ private void setBindingsManipulator() { .whileFalse(new OuttakeAlgae(algaeEffector)); new JoystickButton(manipulatorController, XboxController.Button.kA.value) - .onTrue(new Dealgafication(algaeEffector)); + .onTrue(new DealgaficationIntake(algaeEffector)); new JoystickButton(manipulatorController, XboxController.Button.kB.value) .onTrue(new ShootAlgae(algaeEffector)); diff --git a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java b/src/main/java/org/carlmontrobotics/commands/DealgaficationIntake.java similarity index 82% rename from src/main/java/org/carlmontrobotics/commands/Dealgafication.java rename to src/main/java/org/carlmontrobotics/commands/DealgaficationIntake.java index b2c5d88..89be36a 100644 --- a/src/main/java/org/carlmontrobotics/commands/Dealgafication.java +++ b/src/main/java/org/carlmontrobotics/commands/DealgaficationIntake.java @@ -7,16 +7,17 @@ import edu.wpi.first.math.util.Units; -public class Dealgafication extends Command { +public class DealgaficationIntake extends Command { AlgaeEffector algae; private final Timer timer = new Timer(); - public Dealgafication(AlgaeEffector algaeEffector) { - addRequirements(this.algae = algaeEffector); + public DealgaficationIntake(AlgaeEffector algae) { + addRequirements(this.algae = algae); } @Override public void initialize() { - algae.setArmPosition(ARM_DEALGAFYING_ANGLE); + //algae.setArmPosition(ARM_DEALGAFYING_ANGLE); + timer.reset(); timer.start(); } @@ -34,6 +35,7 @@ public void execute() { @Override public void end(boolean interrupted) { algae.stopMotors(); + timer.stop(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/GroundIntakeAlgae.java similarity index 90% rename from src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java rename to src/main/java/org/carlmontrobotics/commands/GroundIntakeAlgae.java index cc1fa14..81e9678 100644 --- a/src/main/java/org/carlmontrobotics/commands/IntakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/GroundIntakeAlgae.java @@ -7,11 +7,11 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; -public class IntakeAlgae extends Command { +public class GroundIntakeAlgae extends Command { private final AlgaeEffector algae; private final Timer timer = new Timer(); private boolean done = false; - public IntakeAlgae(AlgaeEffector algae) { + public GroundIntakeAlgae(AlgaeEffector algae) { addRequirements(this.algae = algae); } @@ -19,7 +19,7 @@ public IntakeAlgae(AlgaeEffector algae) { public void initialize() { timer.reset(); timer.start(); - algae.setArmPosition(ARM_INTAKE_ANGLE); + //algae.setArmPosition(ARM_INTAKE_ANGLE); } @@ -45,6 +45,7 @@ public void execute() { @Override public void end(boolean interrupted) { algae.stopMotors(); + timer.stop(); //TODO: Test different times } diff --git a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java index a92706f..85a5404 100644 --- a/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/OuttakeAlgae.java @@ -16,7 +16,8 @@ public OuttakeAlgae(AlgaeEffector algaeEffector) { @Override public void initialize() { - algae.setArmPosition(ARM_INTAKE_ANGLE); + //algae.setArmPosition(ARM_INTAKE_ANGLE); + timer.reset(); timer.start(); } @@ -40,6 +41,7 @@ public void execute() { @Override public void end(boolean interrupted) { algae.stopMotors(); + timer.stop(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index cc2e5fd..e7c7c1e 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -10,15 +10,16 @@ public class ShootAlgae extends Command { - AlgaeEffector Algae; + AlgaeEffector algae; private final Timer timer = new Timer(); - public ShootAlgae(AlgaeEffector algaeEffector) { - addRequirements(this.Algae = algaeEffector); + public ShootAlgae(AlgaeEffector algae) { + addRequirements(this.algae = algae); } @Override public void initialize() { - Algae.setArmPosition(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); + algae.setArmPosition(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); + timer.reset(); timer.start(); } @@ -27,15 +28,16 @@ public void initialize() { public void execute() { - Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); - Algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); - Algae.setPincherRPM(Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); + algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); + algae.setTopRPM(Constants.AlgaeEffectorc.SHOOT_TOP_RPM); + algae.setPincherRPM(Constants.AlgaeEffectorc.SHOOT_PINCHER_RPM); } // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - Algae.stopMotors(); + algae.stopMotors(); + timer.stop(); } // Returns true when the command should end. diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 17b87b3..61598b7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -9,8 +9,8 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.RobotContainer; -import org.carlmontrobotics.commands.Dealgafication; -import org.carlmontrobotics.commands.IntakeAlgae; +import org.carlmontrobotics.commands.DealgaficationIntake; +import org.carlmontrobotics.commands.GroundIntakeAlgae; import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.commands.ShootAlgae; @@ -183,7 +183,7 @@ public void setArmPosition() { //((setPoint.position),ControlType.kPosition,armFeedVolts); } - // + // use trapezoid public void setArmTarget(double targetPos){ armGoalState.position = getArmClappedGoal(targetPos); armGoalState.velocity = 0; From c03b2cb62283b546efcdb957f0ad2040ee1a1bf6 Mon Sep 17 00:00:00 2001 From: Sofie Budman Date: Tue, 25 Feb 2025 17:24:36 -0800 Subject: [PATCH 125/153] fix issues with commads --- src/main/java/org/carlmontrobotics/commands/ShootAlgae.java | 2 +- .../java/org/carlmontrobotics/subsystems/AlgaeEffector.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java index e7c7c1e..cc988e5 100644 --- a/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java +++ b/src/main/java/org/carlmontrobotics/commands/ShootAlgae.java @@ -18,7 +18,7 @@ public ShootAlgae(AlgaeEffector algae) { @Override public void initialize() { - algae.setArmPosition(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); + //algae.setArmPosition(Constants.AlgaeEffectorc.ARM_SHOOT_ANGLE); timer.reset(); timer.start(); } diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 61598b7..8e4ba6d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -274,8 +274,8 @@ public void periodic() { SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); SmartDashboard.putNumber("Arm Velocity", getArmVel()); - SmartDashboard.putData("Dealgafication", new Dealgafication(this));//need params for these - SmartDashboard.putData("Intake Algae", new IntakeAlgae(this)); + SmartDashboard.putData("Dealgafication", new DealgaficationIntake(this));//need params for these + SmartDashboard.putData("Intake Algae", new GroundIntakeAlgae(this)); SmartDashboard.putData("Outtake Algae", new OuttakeAlgae(this)); SmartDashboard.putData("Shoot Algae", new ShootAlgae(this)); From 3b71d706a2d9263dfd90ab63b02bc8bdc3697f12 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Tue, 25 Feb 2025 18:34:20 -0800 Subject: [PATCH 126/153] Constants added, tolerance added --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- .../org/carlmontrobotics/subsystems/Elevator.java | 14 ++++++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b9734ea..65fc109 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -42,8 +42,8 @@ public static final class Elevatorc { //Positions public static final double downPos = 0; public static final double l1 = 0; - public static final double l2 = 0; - public static final double l3 = 0; + public static final double l2 = 15; //inches + public static final double l3 = 30; //inches public static final double l4 = 0; public static final double net = 0; public static final double processor = 0; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index faf6a26..316fa51 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -141,6 +141,20 @@ public double getEncoderValue() { return masterEncoder.getPosition(); } + public boolean atGoalHeight() { + if (heightGoal == Constants.Elevatorc.maxElevatorHeightInches) { + return elevatorAtMax(); + } + else if (heightGoal == Constants.Elevatorc.minElevatorHeightInches) { + return elevatorAtMin(); + } + + else { + return (Math.abs(getEncoderValue()) - heightGoal <= Constants.Elevatorc.elevatorTolerance); + } + + } + @Override public void periodic() { if (elevatorAtMax()){ From b449786c6987eb69bbfc4c3014a83fd1b3b082e5 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 26 Feb 2025 07:35:05 -0800 Subject: [PATCH 127/153] Constants update and cleanup --- src/main/java/org/carlmontrobotics/Constants.java | 14 +++++++------- .../org/carlmontrobotics/subsystems/Elevator.java | 2 -- 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 65fc109..dc47a1b 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -32,7 +32,7 @@ public static final class Elevatorc { public static final double followerPositionConversionFactor = 1000; public static final double masterVelocityConversionFactor = 1000; public static final double followerVelocityConversionFactor = 1000; - public static final double maxElevatorHeightInches = 0; + public static final double maxElevatorHeightInches = 53.2; public static final double minElevatorHeightInches = 0; //PID @@ -42,13 +42,13 @@ public static final class Elevatorc { //Positions public static final double downPos = 0; public static final double l1 = 0; - public static final double l2 = 15; //inches - public static final double l3 = 30; //inches - public static final double l4 = 0; - public static final double net = 0; + public static final double l2 = 6.5; //inches + public static final double l3 = 22.5; //inches + public static final double l4 = 52.64; + public static final double net = 53.2; public static final double processor = 0; - public static final double bottomAlgaeRemoval = 0; - public static final double uppperAlgaeRemoval = 0; + public static final double bottomAlgaeRemoval = 22.5; + public static final double uppperAlgaeRemoval = 38.35; //ScoreENUM public enum ElevatorPos { DOWN(downPos), diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 316fa51..967b93e 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -7,7 +7,6 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -19,7 +18,6 @@ import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; From 1e9fa309fe0d4c874399cecd5fd7414c3b3a5433 Mon Sep 17 00:00:00 2001 From: TuskAct2 <112046931+TuskAct2@users.noreply.github.com> Date: Wed, 26 Feb 2025 11:33:02 -0800 Subject: [PATCH 128/153] Solved Merge conflicts --- .../java/org/carlmontrobotics/Constants.java | 750 +++++++++--------- .../org/carlmontrobotics/RobotContainer.java | 401 +++++----- .../carlmontrobotics/commands/ElevatorL1.java | 2 + .../commands/TeleopDrive.java | 15 +- .../subsystems/AlgaeEffector.java | 188 ++--- 5 files changed, 625 insertions(+), 731 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index cbcb043..45c51a4 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -5,15 +5,9 @@ package org.carlmontrobotics; -import java.security.spec.EncodedKeySpec; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import org.carlmontrobotics.lib199.swerve.SwerveConfig; import org.carlmontrobotics.lib199.swerve.SwerveConfig; - -import com.pathplanner.lib.config.RobotConfig; import com.pathplanner.lib.path.PathConstraints; import static org.carlmontrobotics.Config.CONFIG; @@ -22,73 +16,8 @@ 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; - -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.path.PathConstraints; - -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; -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; - - import edu.wpi.first.wpilibj.CounterBase; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; - -import java.security.spec.EncodedKeySpec; - -import edu.wpi.first.wpilibj.CounterBase; -import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; - -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; - -import org.carlmontrobotics.lib199.swerve.SwerveConfig; -import org.carlmontrobotics.lib199.swerve.SwerveConfig; - -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.path.PathConstraints; - -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; -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; - -import com.pathplanner.lib.config.RobotConfig; -import com.pathplanner.lib.path.PathConstraints; - -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; -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 @@ -104,238 +33,263 @@ */ public final class Constants { 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 = 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)); - // The gearing reduction from the drive motor controller to the wheels - // 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.0 / 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[] { 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, + // #region Subsystem Constants + 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)); + // The gearing reduction from the drive motor controller to the wheels + // 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.0 / 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[] { 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, // 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[] 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 }; - - // 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 }; - - // 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, 1.75}; //{2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, 2.015/100, + 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.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 }; + + // 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, 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() - ? new boolean[] { false, false, false, 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[] 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[] 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, 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 - 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 = 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 = 1; //0 - public static final int canCoderPortFR = 2; - public static final int canCoderPortBL = 3; - public static final int canCoderPortBR = 0; //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 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. + 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 } + : 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[] 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[] 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, 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 + 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 = 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 = 1; // 0 + public static final int canCoderPortFR = 2; + public static final int canCoderPortBL = 3; + public static final int canCoderPortBR = 0; // 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 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. } } + public static final class Limelightc { - + public static final class Apriltag { } } + public static final double[] kP = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kI = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kD = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kS = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kV = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kA = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; + public static final double[] kG = { /* /Top/ */0.0, /* /Bottom/ */0.0, /* /Pincher/ */0.0, /* /Arm/ */0.0 }; - - public static final double[] kP = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final double[] kI = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final double[] kD = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - - public static final double[] kS = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final double[] kV = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final double[] kA = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final double[] kG = {/*/Top/*/0.0, /*/Bottom/*/0.0, /*/Pincher/*/0.0, /*/Arm/*/0.0}; - public static final class OI { - public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; + public static final double MIN_AXIS_TRIGGER_VALUE = 0.2; public static final class Driver { public static final int driverPort = 0; - public static final int EFFECTOR_TOP_MOTOR_ID = 31; - public static final int EFFECTOR_BOTTOM_MOTOR_ID = 33; - public static final int EFFECTOR_PINCHER_MOTOR_ID = 31; - public static final int EFFECTOR_ARM_MOTOR_ID = 34; - public static final int effectorDistanceSensorID = 5; - /*public static final int A = 1; - public static final int B = 2; - public static final int X = 3; - public static final int Y = 4;*/ - //Not neccesary - public static final int leftBumper = 5; - public static final int rightBumper = 6; + public static final int EFFECTOR_TOP_MOTOR_ID = 31; + public static final int EFFECTOR_BOTTOM_MOTOR_ID = 33; + public static final int EFFECTOR_PINCHER_MOTOR_ID = 31; + public static final int EFFECTOR_ARM_MOTOR_ID = 34; + public static final int effectorDistanceSensorID = 5; + /* + * public static final int A = 1; + * public static final int B = 2; + * public static final int X = 3; + * public static final int Y = 4; + */ + // Not neccesary + public static final int leftBumper = 5; + public static final int rightBumper = 6; public static final int slowDriveButton = Button.kLeftBumper.value; public static final int resetFieldOrientationButton = Button.kRightBumper.value; @@ -348,141 +302,143 @@ public static final class Driver { } public static final class Manipulator { - public static final int manipulatorPort = 2; - //public static final int X = 0; - public static final Axis OuttakeTrigger = Axis.kRightTrigger; - public static final Axis IntakeTrigger = Axis.kLeftTrigger; - public static final int OuttakeBumper = Button.kRightBumper.value; - public static final int INTAKE_BUMPER = Button.kLeftBumper.value; - } + public static final int manipulatorPort = 2; + // public static final int X = 0; + public static final Axis OuttakeTrigger = Axis.kRightTrigger; + public static final Axis IntakeTrigger = Axis.kLeftTrigger; + public static final int OuttakeBumper = Button.kRightBumper.value; + public static final int INTAKE_BUMPER = Button.kLeftBumper.value; + } public static final double JOY_THRESH = 0.01; - public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. } - public static final class Elevatorc { - //ports - public static final int masterPort = 20; - public static final int followerPort = 21; + + public static final class Elevatorc { + // ports + public static final int masterPort = 20; + public static final int followerPort = 21; public static final int elevatorTopLimitSwitchPort = 1; - public static final int elevatorBottomLimitSwitchPort = 2; - - - //Config - //TODO figure these parts out - public static final IdleMode masterIdleMode = IdleMode.kBrake; - public static final IdleMode followerIdleMode = IdleMode.kBrake; - public static final boolean masterInverted = true; - public static final boolean followerInverted = true; - public static final double masterPositionConversionFactor = 1000; - public static final double followerPositionConversionFactor = 1000; - public static final double masterVelocityConversionFactor = 1000; - public static final double followerVelocityConversionFactor = 1000; - public static final double maxElevatorHeightInches = 0; - public static final double minElevatorHeightInches = 0; - - //PID - public static final double kP = 1; - public static final double kI = 0; - public static final double kD = 0; - //Positions - public static final double downPos = 0; - public static final double l1 = 0; - public static final double l2 = 0; - public static final double l3 = 0; - public static final double l4 = 0; - public static final double net = 0; - public static final double processor = 0; - public static final double bottomAlgaeRemoval = 0; - public static final double uppperAlgaeRemoval = 0; - //ScoreENUM - public enum ElevatorPos { - DOWN(downPos), - L1(l1), - L2(l2), - L3(l3), - L4(l4), - NET(net), - PROCESSOR(processor), - BOTTOMALGAE(bottomAlgaeRemoval), - UPPERALGAE(uppperAlgaeRemoval); - - public final double positionInches; - ElevatorPos(double positionInches) { - this.positionInches = positionInches; - } - - public double getPosition() { - return positionInches; - } - } - //Tolerance - public static final double elevatorTolerance = 0.4; - - } + public static final int elevatorBottomLimitSwitchPort = 2; + + // Config + // TODO figure these parts out + public static final IdleMode masterIdleMode = IdleMode.kBrake; + public static final IdleMode followerIdleMode = IdleMode.kBrake; + public static final boolean masterInverted = true; + public static final boolean followerInverted = true; + public static final double masterPositionConversionFactor = 1000; + public static final double followerPositionConversionFactor = 1000; + public static final double masterVelocityConversionFactor = 1000; + public static final double followerVelocityConversionFactor = 1000; + public static final double maxElevatorHeightInches = 0; + public static final double minElevatorHeightInches = 0; + + // PID + public static final double kP = 1; + public static final double kI = 0; + public static final double kD = 0; + // Positions + public static final double downPos = 0; + public static final double l1 = 0; + public static final double l2 = 0; + public static final double l3 = 0; + public static final double l4 = 0; + public static final double net = 0; + public static final double processor = 0; + public static final double bottomAlgaeRemoval = 0; + public static final double uppperAlgaeRemoval = 0; + + // ScoreENUM + public enum ElevatorPos { + DOWN(downPos), + L1(l1), + L2(l2), + L3(l3), + L4(l4), + NET(net), + PROCESSOR(processor), + BOTTOMALGAE(bottomAlgaeRemoval), + UPPERALGAE(uppperAlgaeRemoval); + + public final double positionInches; + + ElevatorPos(double positionInches) { + this.positionInches = positionInches; + } + + public double getPosition() { + return positionInches; + } + } + + // Tolerance + public static final double elevatorTolerance = 0.4; + + } + public static final class AlgaeEffectorc { - //EFFECTOR + // EFFECTOR - - public static final int UPPER_MOTOR_PORT = 1; + public static final int UPPER_MOTOR_PORT = 1; public static final int LOWER_MOTOR_PORT = 2; - public static final int PINCH_MOTOR_PORT = 3; - public static final int ARM_MOTOR_PORT = 4; - public static final int aChannelEnc = 0; - public static final int bChannelEnc = 1; + public static final int PINCH_MOTOR_PORT = 3; + public static final int ARM_MOTOR_PORT = 4; + public static final int aChannelEnc = 0; + public static final int bChannelEnc = 1; public static final int TOP_ARRAY_ORDER = 0; public static final int BOTTOM_ARRAY_ORDER = 1; public static final int PINCHER_ARRAY_ORDER = 2; - public static final int ARM_ARRAY_ORDER = 3; - //the ArrayOrder variables replaced the ones for the kS since they just indicate the order and are the same for all PID values - //TODO find these values out vvv - public static double INTAKE_TOP_RPM = 1000; - public static double INTAKE_BOTTOM_RPM = 1000; - public static double INTAKE_PINCHER_RPM = 1000; - - public static double OUTTAKE_TOP_RPM = -2100; - public static double OUTTAKE_BOTTOM_RPM = -2100; - public static double OUTTAKE_PINCHER_RPM = -2100; - - public static double SHOOT_TOP_RPM = -2100;//ask design - public static double SHOOT_BOTTOM_RPM = -2100; - public static double SHOOT_PINCHER_RPM = -2100; - - public static double DEALGAFY_TOP_RPM = 1000; - public static double DEALGAFY_BOTTOM_RPM = 1000; - public static double DEALGAFY_PINCHER_RPM = 1000; - - public static final int TBE_CPR = 8192; //Through-Bore Encoder - public static final double TBE_DPP = 360.0/TBE_CPR; //Degrees per pulse - public static final boolean invertedTBE = false; //if the encoder needs to read invertedly - public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; - - public static final double ARM_CHAIN_GEARING = 16.0/34; - public static final double ARM_GEAR_RATIO = 1.0/3; - //TODO figure the zero out once encoder is on - public static final double ARM_TO_ZERO = 0; //Pure vertical down - //TODO ask samo for angle to intake algae from pure vertical down - public static final double ARM_INTAKE_ANGLE = 0; - //TODO Figure these two out if we will be shooting algae - public static final double ARM_RAMP_UP_ANGLE = 0; - public static final double ARM_SHOOT_ANGLE = 0; - //TODO Figure angle for dealgafying - public static final double ARM_DEALGAFYING_ANGLE = 0; - //TODO figure out resting angle of the arm while algae inside - public static final double ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE = 0.0; - //TODO figure out current threshold for pincher wheels - public static final double PINCHER_CURRENT_THRESHOLD = 15.0; - - public static final double UPPER_ANGLE_LIMIT = 0; - public static final double LOWER_ANGLE_LIMIT = -70; - public static final double ROTATION_TO_DEG = 360; - public static final double ARM_DISCONT_DEG = -35; - - public static final double ARM_ERROR_MARGIN = 1; - + public static final int ARM_ARRAY_ORDER = 3; + // the ArrayOrder variables replaced the ones for the kS since they just + // indicate the order and are the same for all PID values + // TODO find these values out vvv + public static double INTAKE_TOP_RPM = 1000; + public static double INTAKE_BOTTOM_RPM = 1000; + public static double INTAKE_PINCHER_RPM = 1000; + + public static double OUTTAKE_TOP_RPM = -2100; + public static double OUTTAKE_BOTTOM_RPM = -2100; + public static double OUTTAKE_PINCHER_RPM = -2100; + + public static double SHOOT_TOP_RPM = -2100;// ask design + public static double SHOOT_BOTTOM_RPM = -2100; + public static double SHOOT_PINCHER_RPM = -2100; + + public static double DEALGAFY_TOP_RPM = 1000; + public static double DEALGAFY_BOTTOM_RPM = 1000; + public static double DEALGAFY_PINCHER_RPM = 1000; + + public static final int TBE_CPR = 8192; // Through-Bore Encoder + public static final double TBE_DPP = 360.0 / TBE_CPR; // Degrees per pulse + public static final boolean invertedTBE = false; // if the encoder needs to read invertedly + public static final CounterBase.EncodingType encodingType = Encoder.EncodingType.k2X; + + public static final double ARM_CHAIN_GEARING = 16.0 / 34; + public static final double ARM_GEAR_RATIO = 1.0 / 3; + // TODO figure the zero out once encoder is on + public static final double ARM_TO_ZERO = 0; // Pure vertical down + // TODO ask samo for angle to intake algae from pure vertical down + public static final double ARM_INTAKE_ANGLE = 0; + // TODO Figure these two out if we will be shooting algae + public static final double ARM_RAMP_UP_ANGLE = 0; + public static final double ARM_SHOOT_ANGLE = 0; + // TODO Figure angle for dealgafying + public static final double ARM_DEALGAFYING_ANGLE = 0; + // TODO figure out resting angle of the arm while algae inside + public static final double ARM_RESTING_ANGLE_WHILE_INTAKE_ALGAE = 0.0; + // TODO figure out current threshold for pincher wheels + public static final double PINCHER_CURRENT_THRESHOLD = 15.0; + + public static final double UPPER_ANGLE_LIMIT = 0; + public static final double LOWER_ANGLE_LIMIT = -70; + public static final double ROTATION_TO_DEG = 360; + public static final double ARM_DISCONT_DEG = -35; + + public static final double ARM_ERROR_MARGIN = 1; } - + } diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index bfa774e..69eeaed 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -10,38 +10,27 @@ 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; import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.Constants.OI.Manipulator.*; import org.carlmontrobotics.commands.*; -import static org.carlmontrobotics.Constants.OI.*; import static org.carlmontrobotics.Constants.OI.Manipulator.*; -import org.carlmontrobotics.Constants.OI; -import org.carlmontrobotics.Constants.OI.Manipulator.*; -import org.carlmontrobotics.Constants.OI.Manipulator; - import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; - - 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; -import edu.wpi.first.wpilibj.PS5Controller.Button; import edu.wpi.first.wpilibj.XboxController.Axis; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; @@ -49,132 +38,121 @@ 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.ConditionalCommand; 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; 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(OI.Driver.port); - public final GenericHID manipulatorController = new GenericHID(Manipulator.port); - - private final Drivetrain drivetrain = new 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 simplePz - // straight - private List autoCommands; - private SendableChooser autoSelector = new SendableChooser(); - - private boolean hasSetupAutos = false; - private final String[] autoNames = new String[] {}; - private final AlgaeEffector algaeEffector = new AlgaeEffector(); - - 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()); - - 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); + 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(OI.Driver.driverPort); + public final GenericHID manipulatorController = new GenericHID(Manipulator.manipulatorPort); + + private final Drivetrain drivetrain = new 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 simplePz + // straight + private List autoCommands; + private SendableChooser autoSelector = new SendableChooser(); + + private boolean hasSetupAutos = false; + private final String[] autoNames = new String[] {}; + private final AlgaeEffector algaeEffector = new AlgaeEffector(); + + 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()); + + 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(); + setBindingsManipulator(); } - setDefaultCommands(); - setBindingsDriver(); - setBindingsManipulator(); - } - - 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))); - } - private void setBindingsDriver() { - // new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new RunAlgae(algaeEffector, 1, false)); //wrong - // new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new RunAlgae(algaeEffector, 2, false)); - // new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new RunAlgae(algaeEffector, 3, false)); - // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new RunAlgae(algaeEffector, 0, true)); - - } - - - private void setBindingsManipulator() { - //intake - new JoystickButton(manipulatorController, INTAKE_BUMPER) - .whileTrue(new SequentialCommandGroup( - new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), - new GroundIntakeAlgae(algaeEffector) - )); - - - //outake - - //dealgify - - - new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) - .whileFalse(new OuttakeAlgae(algaeEffector)); - - new JoystickButton(manipulatorController, XboxController.Button.kA.value) - .onTrue(new DealgaficationIntake(algaeEffector)); - - new JoystickButton(manipulatorController, XboxController.Button.kB.value) - .onTrue(new ShootAlgae(algaeEffector)); - new JoystickButton(manipulatorController, XboxController.Button.kY.value) - .onTrue(new InstantCommand(()->{algaeEffector.stopMotors();})); - } - + 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))); + } + + private void setBindingsDriver() { + // new Trigger(() -> driverController.getRawButton(OI.Driver.X)).onTrue(new + // RunAlgae(algaeEffector, 1, false)); //wrong + // new Trigger(() -> driverController.getRawButton(OI.Driver.Y)).onTrue(new + // RunAlgae(algaeEffector, 2, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.B)).onTrue(new + // RunAlgae(algaeEffector, 3, false)); + // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new + // RunAlgae(algaeEffector, 0, true)); - private Trigger axisTrigger(GenericHID controller, Axis axis) { - return new Trigger(() -> Math - .abs(getStickValue(controller, axis)) > Constants.OI.MIN_AXIS_TRIGGER_VALUE); } + private void setBindingsManipulator() { + // intake + new JoystickButton(manipulatorController, INTAKE_BUMPER) + .whileTrue(new SequentialCommandGroup( + new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), + new GroundIntakeAlgae(algaeEffector))); + + // outake + + // dealgify - public Command getAutonomousCommand() { - return Commands.print("No autonomous command configured"); - } + new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) + .whileFalse(new OuttakeAlgae(algaeEffector)); + + new JoystickButton(manipulatorController, XboxController.Button.kA.value) + .onTrue(new DealgaficationIntake(algaeEffector)); + + new JoystickButton(manipulatorController, XboxController.Button.kB.value) + .onTrue(new ShootAlgae(algaeEffector)); + new JoystickButton(manipulatorController, XboxController.Button.kY.value) + .onTrue(new InstantCommand(() -> { + algaeEffector.stopMotors(); + })); + } - /** + /** * Flips an axis' Y coordinates upside down, but only if the select axis is a * joystick axis * @@ -183,71 +161,70 @@ public Command getAutonomousCommand() { * @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() { + 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)); } @@ -255,7 +232,7 @@ private void registerAutoCommands() { private void setupAutos() { //// CREATING PATHS from files if (!hasSetupAutos) { - autoCommands=new ArrayList();//clear old/nonexistent autos + autoCommands = new ArrayList();// clear old/nonexistent autos for (int i = 0; i < autoNames.length; i++) { String name = autoNames[i]; @@ -263,14 +240,15 @@ private void setupAutos() { 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)); + * // 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; @@ -279,13 +257,14 @@ private void setupAutos() { 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))); + // 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 + autoCommands.set(2, constructPPSimpleAuto());// overwrite this slot each time auto runs } public Command constructPPSimpleAuto() { @@ -297,27 +276,29 @@ public Command constructPPSimpleAuto() { * (guesses!) */ // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER - Pose2d currPos = drivetrain.getPose(); + 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. + // 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))); + ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) + : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); 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 */ + // 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, null, 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))); + new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), // align drivetrain wheels. + AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); } public Command getAutonomousCommand() { diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java index a8a6073..2862f1b 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java @@ -19,6 +19,8 @@ public void initialize() { @Override public void execute() { elevator.setGoal(ElevatorPos.L1); + + } @Override public void end(boolean interrupted) { diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java index 7fd2932..49486b9 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -1,5 +1,6 @@ package org.carlmontrobotics.commands; +import static org.carlmontrobotics.Constants.AlgaeEffectorc.ARM_INTAKE_ANGLE; import static org.carlmontrobotics.Constants.Drivetrainc.*; import java.util.function.BooleanSupplier; @@ -7,7 +8,7 @@ import org.carlmontrobotics.Constants; import org.carlmontrobotics.Constants.Elevatorc; -import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.AlgaeEffector; import org.carlmontrobotics.Robot; import org.carlmontrobotics.subsystems.Drivetrain; import static org.carlmontrobotics.RobotContainer.*; @@ -31,9 +32,8 @@ public class TeleopDrive extends Command { private double currentStrafeVel = 0; private double prevTimestamp; private boolean babyMode; - private boolean ElevatorMode; - private boolean elavatorUp; Elevator elevator; + AlgaeEffector arm; private double height = elevator.getCurrentHeight(); /** * Creates a new TeleopDrive. @@ -111,11 +111,10 @@ public double[] getRequestedSpeeds() { driveMultiplier = kBabyDriveSpeed; rotationMultiplier = kBabyDriveRotation; } - if( height >= Elevatorc.l1 ) - { - rotationMultiplier = 0; - strafe = 0; - } + + + + // double driveMultiplier = kNormalDriveSpeed; // double rotationMultiplier = kNormalDriveRotation; diff --git a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java index 8e4ba6d..d5bb99d 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java +++ b/src/main/java/org/carlmontrobotics/subsystems/AlgaeEffector.java @@ -1,23 +1,13 @@ package org.carlmontrobotics.subsystems; - -import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; - - -import static org.carlmontrobotics.RobotContainer.*; - import org.carlmontrobotics.Constants; -import org.carlmontrobotics.RobotContainer; import org.carlmontrobotics.commands.DealgaficationIntake; import org.carlmontrobotics.commands.GroundIntakeAlgae; import org.carlmontrobotics.commands.OuttakeAlgae; import org.carlmontrobotics.commands.ShootAlgae; -import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.ClosedLoopSlot; -import com.revrobotics.spark.SparkAbsoluteEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; @@ -29,51 +19,27 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkMaxAlternateEncoder; - -import edu.wpi.first.hal.SimDouble; import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.util.Units; - -import edu.wpi.first.units.Measure; -import edu.wpi.first.units.MutableMeasure; - -import edu.wpi.first.util.sendable.SendableBuilder; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.RobotBase; -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.simulation.SimDeviceSim; 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.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.Encoder; - import static org.carlmontrobotics.Constants.AlgaeEffectorc.*; import static org.carlmontrobotics.Constants.*; - public class AlgaeEffector extends SubsystemBase { - //motors + // motors private final SparkFlex topMotor = new SparkFlex(UPPER_MOTOR_PORT, MotorType.kBrushless); - private final SparkFlex bottomMotor = new SparkFlex(LOWER_MOTOR_PORT, MotorType.kBrushless); + private final SparkFlex bottomMotor = new SparkFlex(LOWER_MOTOR_PORT, MotorType.kBrushless); private final SparkFlex pincherMotor = new SparkFlex(PINCH_MOTOR_PORT, MotorType.kBrushless); private final SparkMax armMotor = new SparkMax(ARM_MOTOR_PORT, MotorType.kBrushless); private SparkFlexConfig pincherMotorConfig = new SparkFlexConfig(); private SparkFlexConfig bottomMotorConfig = new SparkFlexConfig(); private SparkFlexConfig topMotorConfig = new SparkFlexConfig(); - private SparkMaxConfig armMotorConfig = new SparkMaxConfig(); - + private SparkMaxConfig armMotorConfig = new SparkMaxConfig(); + private final RelativeEncoder topEncoder = topMotor.getEncoder(); private final RelativeEncoder bottomEncoder = bottomMotor.getEncoder(); private final RelativeEncoder pincherEncoder = pincherMotor.getEncoder(); @@ -84,69 +50,68 @@ public class AlgaeEffector extends SubsystemBase { private final SparkClosedLoopController pidControllerBottom = bottomMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerPincher = pincherMotor.getClosedLoopController(); private final SparkClosedLoopController pidControllerArm = armMotor.getClosedLoopController(); - - private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[TOP_ARRAY_ORDER], kV[TOP_ARRAY_ORDER], kA[TOP_ARRAY_ORDER]); - private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[BOTTOM_ARRAY_ORDER], kV[BOTTOM_ARRAY_ORDER], kA[BOTTOM_ARRAY_ORDER]); - private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[PINCHER_ARRAY_ORDER], kV[PINCHER_ARRAY_ORDER], kA[PINCHER_ARRAY_ORDER]); - private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(kS[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); - //feedforward for arm was added - - - //Arm Trapezoid Profile + private final SimpleMotorFeedforward topFeedforward = new SimpleMotorFeedforward(kS[TOP_ARRAY_ORDER], + kV[TOP_ARRAY_ORDER], kA[TOP_ARRAY_ORDER]); + private final SimpleMotorFeedforward bottomFeedforward = new SimpleMotorFeedforward(kS[BOTTOM_ARRAY_ORDER], + kV[BOTTOM_ARRAY_ORDER], kA[BOTTOM_ARRAY_ORDER]); + private final SimpleMotorFeedforward pincherFeedforward = new SimpleMotorFeedforward(kS[PINCHER_ARRAY_ORDER], + kV[PINCHER_ARRAY_ORDER], kA[PINCHER_ARRAY_ORDER]); + private final SimpleMotorFeedforward armFeedforward = new SimpleMotorFeedforward(kS[ARM_ARRAY_ORDER], + kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); + // feedforward for arm was added + + // Arm Trapezoid Profile private TrapezoidProfile armTrapProfile; - private TrapezoidProfile.State armGoalState = new TrapezoidProfile.State(0,0); //position,velocity (0,0) + private TrapezoidProfile.State armGoalState = new TrapezoidProfile.State(0, 0); // position,velocity (0,0) private static double kDt; private TrapezoidProfile.State setPoint; - + private double armFeedVolts; private double kG; - // private final ArmFeedforward armFeed = new ArmFeedforward(kS[ARM_ARRAY_ORDER], kG[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], kA[ARM_ARRAY_ORDER]); + // private final ArmFeedforward armFeed = new + // ArmFeedforward(kS[ARM_ARRAY_ORDER], kG[ARM_ARRAY_ORDER], kV[ARM_ARRAY_ORDER], + // kA[ARM_ARRAY_ORDER]); - //-------------------------------------------------------------------------------------------- + // -------------------------------------------------------------------------------------------- public AlgaeEffector() { configureMotors(); kDt = 0.02; setPoint = getArmState(); } - //---------------------------------------------------------------------------------------- + // ---------------------------------------------------------------------------------------- - private void configureMotors () { + private void configureMotors() { topMotorConfig.closedLoop.pid( - Constants.kP[TOP_ARRAY_ORDER], - Constants.kI[TOP_ARRAY_ORDER], - Constants.kD[TOP_ARRAY_ORDER] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + Constants.kP[TOP_ARRAY_ORDER], + Constants.kI[TOP_ARRAY_ORDER], + Constants.kD[TOP_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); topMotor.configure(topMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - + bottomMotorConfig.closedLoop.pid( - Constants.kP[BOTTOM_ARRAY_ORDER], - Constants.kI[BOTTOM_ARRAY_ORDER], - Constants.kD[BOTTOM_ARRAY_ORDER] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - + Constants.kP[BOTTOM_ARRAY_ORDER], + Constants.kI[BOTTOM_ARRAY_ORDER], + Constants.kD[BOTTOM_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + bottomMotor.configure(bottomMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + pincherMotorConfig.closedLoop.pid( - Constants.kP[PINCHER_ARRAY_ORDER], - Constants.kI[PINCHER_ARRAY_ORDER], - Constants.kD[PINCHER_ARRAY_ORDER] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + Constants.kP[PINCHER_ARRAY_ORDER], + Constants.kI[PINCHER_ARRAY_ORDER], + Constants.kD[PINCHER_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.closedLoop.pid( - Constants.kP[ARM_ARRAY_ORDER], - Constants.kI[ARM_ARRAY_ORDER], - Constants.kD[ARM_ARRAY_ORDER] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + Constants.kP[ARM_ARRAY_ORDER], + Constants.kI[ARM_ARRAY_ORDER], + Constants.kD[ARM_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); pincherMotor.configure(pincherMotorConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); armMotorConfig.idleMode(IdleMode.kBrake); armMotorConfig.closedLoop.pid( - Constants.kP[ARM_ARRAY_ORDER], - Constants.kI[ARM_ARRAY_ORDER], - Constants.kD[ARM_ARRAY_ORDER] - ).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + Constants.kP[ARM_ARRAY_ORDER], + Constants.kI[ARM_ARRAY_ORDER], + Constants.kD[ARM_ARRAY_ORDER]).feedbackSensor(FeedbackSensor.kPrimaryEncoder); armMotorConfig.encoder.positionConversionFactor(ROTATION_TO_DEG); } @@ -162,63 +127,59 @@ public void setBottomRPM(double bottomrpm) { public void setPincherRPM(double pincherrpm) { pidControllerPincher.setReference(pincherrpm, ControlType.kVelocity, ClosedLoopSlot.kSlot0); - pincherFeedforward.calculate(pincherrpm); + pincherFeedforward.calculate(pincherrpm); } - //arm methods - //drives arm from set point to goal position + + // arm methods + // drives arm from set point to goal position public void setArmPosition() { - + setPoint = getArmState(); - armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); + armGoalState = armTrapProfile.calculate(kDt, setPoint, armGoalState); armFeedVolts = armFeedforward.calculate(armGoalState.position, armGoalState.velocity); if ((getArmPos() < LOWER_ANGLE_LIMIT) - || (getArmPos() > UPPER_ANGLE_LIMIT)) { + || (getArmPos() > UPPER_ANGLE_LIMIT)) { armFeedVolts = armFeedforward.calculate(getArmPos(), 0); } - - pidControllerArm.setReference(armGoalState.position, ControlType.kPosition,ClosedLoopSlot.kSlot0, armFeedVolts); - //((setPoint.position),ControlType.kPosition,armFeedVolts); + pidControllerArm.setReference(armGoalState.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, + armFeedVolts); + // ((setPoint.position),ControlType.kPosition,armFeedVolts); } - - // use trapezoid - public void setArmTarget(double targetPos){ - armGoalState.position = getArmClappedGoal(targetPos); + + // use trapezoid + public void setArmTarget(double targetPos) { + armGoalState.position = getArmClappedGoal(targetPos); armGoalState.velocity = 0; } - - - //returns the arm position and velocity based on encoder and position - public TrapezoidProfile.State getArmState(){ + // returns the arm position and velocity based on encoder and position + public TrapezoidProfile.State getArmState() { TrapezoidProfile.State armState = new TrapezoidProfile.State(getArmPos(), getArmVel()); return armState; } - //overload - - - public boolean armAtGoal(){ - return Math.abs(getArmPos()-armGoalState.position) <= ARM_ERROR_MARGIN; + // overload + public boolean armAtGoal() { + return Math.abs(getArmPos() - armGoalState.position) <= ARM_ERROR_MARGIN; } - public double getArmClappedGoal(double goalAngle) { return MathUtil.clamp( - MathUtil.inputModulus(goalAngle, ARM_DISCONT_DEG, - ARM_DISCONT_DEG + 360), - LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT - ); + MathUtil.inputModulus(goalAngle, ARM_DISCONT_DEG, + ARM_DISCONT_DEG + 360), + LOWER_ANGLE_LIMIT, UPPER_ANGLE_LIMIT); } - public double getArmPos() { - //figures out the position of the arm in degrees based off pure vertical down - //TODO update the arm to get in degrees after someone will figure out what the .getPosition gets for the TBE + // figures out the position of the arm in degrees based off pure vertical down + // TODO update the arm to get in degrees after someone will figure out what the + // .getPosition gets for the TBE return armAbsoluteEncoder.getPosition() * ARM_CHAIN_GEARING - ARM_TO_ZERO; } - public double getArmVel(){ + + public double getArmVel() { return armAbsoluteEncoder.getVelocity(); } @@ -227,13 +188,12 @@ public void stopArm() { } - // public double getArmVel() { - // return armAbsoluteEncoder.getVelocity(); + // return armAbsoluteEncoder.getVelocity(); // } public void runRPM() { - //TODO: Change RPM according to design + // TODO: Change RPM according to design setTopRPM(1000); setBottomRPM(2100); setPincherRPM(2100); @@ -250,7 +210,7 @@ public boolean checkIfAtTopRPM(double rpm) { } public boolean checkIfAtBottomRPM(double rpm) { - return bottomEncoder.getVelocity() == rpm;//give like a 5% error or somethin + return bottomEncoder.getVelocity() == rpm;// give like a 5% error or somethin } public void setMotorSpeed(double topSpeed, double bottomSpeed, double pincherSpeed) { @@ -263,10 +223,6 @@ public boolean isAlgaeIntaked() { return pincherMotor.getOutputCurrent() > PINCHER_CURRENT_THRESHOLD; } - - - - @Override public void periodic() { setArmPosition(); @@ -274,7 +230,7 @@ public void periodic() { SmartDashboard.putNumber("Raw Arm Angle", armAbsoluteEncoder.getPosition()); SmartDashboard.putBoolean("Algae Intaked?", isAlgaeIntaked()); SmartDashboard.putNumber("Arm Velocity", getArmVel()); - SmartDashboard.putData("Dealgafication", new DealgaficationIntake(this));//need params for these + SmartDashboard.putData("Dealgafication", new DealgaficationIntake(this));// need params for these SmartDashboard.putData("Intake Algae", new GroundIntakeAlgae(this)); SmartDashboard.putData("Outtake Algae", new OuttakeAlgae(this)); SmartDashboard.putData("Shoot Algae", new ShootAlgae(this)); From 7ba801ebfa94f7e9dc49b63fe0cf1e8e6c5cbc2a Mon Sep 17 00:00:00 2001 From: TuskAct2 <112046931+TuskAct2@users.noreply.github.com> Date: Wed, 26 Feb 2025 11:36:04 -0800 Subject: [PATCH 129/153] added a fw and bw only drive when getting alge and placing coral --- .../java/org/carlmontrobotics/commands/TeleopDrive.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java index 49486b9..b94a165 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -111,6 +111,12 @@ public double[] getRequestedSpeeds() { driveMultiplier = kBabyDriveSpeed; rotationMultiplier = kBabyDriveRotation; } + + if( height >= Elevatorc.l1 && arm.getArmPos() >= ARM_INTAKE_ANGLE) + { + rotationMultiplier = 0; + strafe = 0; + } From 74888002a051381fab62b07f4f17b5831495b8b8 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Wed, 5 Mar 2025 07:31:42 -0800 Subject: [PATCH 130/153] Update Constants.java --- src/main/java/org/carlmontrobotics/Constants.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index fac7162..7625a37 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -338,6 +338,7 @@ public static final class Elevatorc { public static final double kP = 1; public static final double kI = 0; public static final double kD = 0; + public static final double kS = 0; //Positions public static final double downPos = 0; public static final double l1 = 0; From 22988793d3f598b51a56e6bf25f7db9b2d8362b4 Mon Sep 17 00:00:00 2001 From: Brandon Date: Thu, 6 Mar 2025 11:24:16 -0800 Subject: [PATCH 131/153] updated PositionConversionFactor to reflect real values --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 7625a37..ada4081 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -327,8 +327,8 @@ public static final class Elevatorc { public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = true; public static final boolean followerInverted = true; - public static final double masterPositionConversionFactor = 1000; - public static final double followerPositionConversionFactor = 1000; + public static final double masterPositionConversionFactor = 0.55292030703; //2(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 + public static final double followerPositionConversionFactor = 0.55292030703; public static final double masterVelocityConversionFactor = 1000; public static final double followerVelocityConversionFactor = 1000; public static final double maxElevatorHeightInches = 53.2; From 44bb175e14a8d63dd337b8e8cb7f54744b0af4d8 Mon Sep 17 00:00:00 2001 From: Brandon Date: Thu, 6 Mar 2025 11:39:51 -0800 Subject: [PATCH 132/153] Changed Position Conversion Factor to Code Math --- src/main/java/org/carlmontrobotics/Constants.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index ada4081..35e7f27 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -327,8 +327,8 @@ public static final class Elevatorc { public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = true; public static final boolean followerInverted = true; - public static final double masterPositionConversionFactor = 0.55292030703; //2(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 - public static final double followerPositionConversionFactor = 0.55292030703; + public static final double masterPositionConversionFactor = 2*(Math.PI * 1.76)/20; //2(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 + public static final double followerPositionConversionFactor = 2*(Math.PI * 1.76)/20; public static final double masterVelocityConversionFactor = 1000; public static final double followerVelocityConversionFactor = 1000; public static final double maxElevatorHeightInches = 53.2; From 4d5b5a7b40485a049b286050752ba6d189e185d3 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 6 Mar 2025 20:01:11 -0800 Subject: [PATCH 133/153] Add sysid BUG ERROR Mutable measure not working plz fix --- .../java/org/carlmontrobotics/Constants.java | 8 +- .../org/carlmontrobotics/RobotContainer.java | 6 ++ .../carlmontrobotics/subsystems/Elevator.java | 80 ++++++++++++++++++- 3 files changed, 89 insertions(+), 5 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 35e7f27..f80b677 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -329,8 +329,8 @@ public static final class Elevatorc { public static final boolean followerInverted = true; public static final double masterPositionConversionFactor = 2*(Math.PI * 1.76)/20; //2(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 public static final double followerPositionConversionFactor = 2*(Math.PI * 1.76)/20; - public static final double masterVelocityConversionFactor = 1000; - public static final double followerVelocityConversionFactor = 1000; + public static final double masterVelocityConversionFactor = 2*(Math.PI * 1.76)/20; + public static final double followerVelocityConversionFactor = 2*(Math.PI * 1.76)/20; public static final double maxElevatorHeightInches = 53.2; public static final double minElevatorHeightInches = 0; @@ -338,7 +338,11 @@ public static final class Elevatorc { public static final double kP = 1; public static final double kI = 0; public static final double kD = 0; + //Feedforward public static final double kS = 0; + public static final double kG = 0; + public static final double kV = 0; + public static final double kA = 0; //Positions public static final double downPos = 0; public static final double l1 = 0; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 69eeaed..fa08f45 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -45,6 +45,7 @@ //control bindings import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; public class RobotContainer { private static boolean babyMode = false; @@ -70,6 +71,7 @@ public class RobotContainer { private boolean hasSetupAutos = false; private final String[] autoNames = new String[] {}; private final AlgaeEffector algaeEffector = new AlgaeEffector(); + private final Elevator elevator = new Elevator(); public RobotContainer() { { @@ -124,6 +126,10 @@ private void setBindingsDriver() { // RunAlgae(algaeEffector, 3, false)); // new Trigger(() -> driverController.getRawButton(OI.Driver.A)).onTrue(new // RunAlgae(algaeEffector, 0, true)); + new JoystickButton(driverController, 3).whileTrue(elevator.sysIdQuasistatic(SysIdRoutine.Direction.kForward)); //press x to run + new JoystickButton(driverController, 4).whileTrue(elevator.sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); //press y to run + new JoystickButton(driverController, 1).whileTrue(elevator.sysIdDynamic(SysIdRoutine.Direction.kForward)); //press a to run + new JoystickButton(driverController, 2).whileTrue(elevator.sysIdDynamic(SysIdRoutine.Direction.kReverse)); //press b to run } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 967b93e..5e1f1d6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -4,6 +4,16 @@ package org.carlmontrobotics.subsystems; + +import static edu.wpi.first.units.Units.InchesPerSecond; +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; + +import java.util.Map; +import java.util.function.Consumer; + import org.carlmontrobotics.Constants; import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; @@ -17,10 +27,20 @@ import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +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.Voltage; import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Mechanism; public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ @@ -39,9 +59,12 @@ public class Elevator extends SubsystemBase { private double heightGoal; private int elevatorState; //PID - private SparkClosedLoopController pidElevatorController; + private PIDController pidElevatorController; + private ElevatorFeedforward feedforwardElevatorController; private Timer timer; + private final SysIdRoutine sysIdRoutine; + public Elevator() { //motors masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); @@ -58,8 +81,36 @@ public Elevator() { timer = new Timer(); timer.start(); + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + + //PID - pidElevatorController = masterMotor.getClosedLoopController(); + pidElevatorController = new PIDController(Constants.Elevatorc.kP, Constants.Elevatorc.kI, Constants.Elevatorc.kD); + //FeedForward + feedforwardElevatorController = new ElevatorFeedforward(Constants.Elevatorc.kS, Constants.Elevatorc.kG, Constants.Elevatorc.kV, Constants.Elevatorc.kA); + + + sysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + voltage -> { + masterMotor.setVoltage(voltage); + }, + log -> { + log.motor("Elevator") + .voltage( + m_appliedVoltage.mut_replace( + masterMotor.get() * RobotController.getBatteryVoltage(), Volts)) + .linearPosition(m_distance.mut_replace(masterEncoder.getPosition(), Inches)) + .linearVelocity(m_velocity.mut_replace(masterEncoder.getVelocity(), InchesPerSecond)); + }, + this) + ); } private void configureMotors () { @@ -124,7 +175,9 @@ else if (elevatorAtMin()) { } public void getToGoal() { - pidElevatorController.setReference(heightGoal, ControlType.kPosition); + masterMotor.setVoltage( + pidElevatorController.calculate(masterEncoder.getPosition(), heightGoal) + + feedforwardElevatorController.calculate(heightGoal)); } public void setSpeed(double speed){ @@ -153,6 +206,27 @@ else if (heightGoal == Constants.Elevatorc.minElevatorHeightInches) { } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + + @Override public void periodic() { if (elevatorAtMax()){ From 87695bc924ea2fbf0d5e96d417add84842dfc186 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Thu, 6 Mar 2025 20:11:34 -0800 Subject: [PATCH 134/153] Bug error fixed! --- .../org/carlmontrobotics/subsystems/Elevator.java | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 5e1f1d6..57684b2 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -65,6 +65,13 @@ public class Elevator extends SubsystemBase { private final SysIdRoutine sysIdRoutine; + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + public Elevator() { //motors masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); @@ -81,13 +88,6 @@ public Elevator() { timer = new Timer(); timer.start(); - // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutVoltage m_appliedVoltage = Volts.mutable(0); - // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. - private final MutDistance m_distance = Meters.mutable(0); - // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. - private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); - //PID pidElevatorController = new PIDController(Constants.Elevatorc.kP, Constants.Elevatorc.kI, Constants.Elevatorc.kD); From 20e89f90ef751101c529ef274554a3949c86bc7e Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Fri, 7 Mar 2025 16:53:48 -0800 Subject: [PATCH 135/153] Deleted ElevatorL1-L4 Commmands. Changed to singular ElevatorLPos command --- .../carlmontrobotics/commands/ElevatorL1.java | 33 ------------------- .../carlmontrobotics/commands/ElevatorL3.java | 31 ----------------- .../carlmontrobotics/commands/ElevatorL4.java | 31 ----------------- .../{ElevatorL2.java => ElevatorLPos.java} | 20 +++++------ 4 files changed, 8 insertions(+), 107 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL1.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL3.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorL4.java rename src/main/java/org/carlmontrobotics/commands/{ElevatorL2.java => ElevatorLPos.java} (56%) diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java deleted file mode 100644 index 2862f1b..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL1.java +++ /dev/null @@ -1,33 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorL1 extends Command { - Elevator elevator; - - public ElevatorL1(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.L1); - - - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java deleted file mode 100644 index 899d8c4..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL3.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorL3 extends Command { - Elevator elevator; - - public ElevatorL3(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.L3); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java b/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java deleted file mode 100644 index 5c3fcdb..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL4.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorL4 extends Command { - Elevator elevator; - - public ElevatorL4(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.L4); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java b/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java similarity index 56% rename from src/main/java/org/carlmontrobotics/commands/ElevatorL2.java rename to src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java index 890ea44..afb8d93 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorL2.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java @@ -5,26 +5,22 @@ import edu.wpi.first.wpilibj2.command.Command; -public class ElevatorL2 extends Command { - Elevator elevator; - - public ElevatorL2(Elevator elevator) { - addRequirements(this.elevator = elevator); +public class ElevatorLPos extends Command { + private Elevator elevator; + private ElevatorPos pos; + public ElevatorLPos(Elevator elevator, ElevatorPos pos) { + this.elevator = elevator; + this.pos=pos; + addRequirements(elevator); } @Override public void initialize() { } - @Override public void execute() { - elevator.setGoal(ElevatorPos.L2); + elevator.setGoal(pos); } - @Override - public void end(boolean interrupted) { - - } - @Override public boolean isFinished() { return false; } From a798ce41d4ecc56f219f080ea0e457a212c40fa3 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Fri, 7 Mar 2025 16:58:43 -0800 Subject: [PATCH 136/153] ElevatorLPos now replaces all setGoal commands --- .../commands/ElevatorBottomAlgaeRemoval.java | 31 ------------------- .../commands/ElevatorDown.java | 31 ------------------- .../commands/ElevatorNet.java | 31 ------------------- .../commands/ElevatorProcessor.java | 30 ------------------ .../commands/ElevatorUpperAlgaeRemoval.java | 31 ------------------- 5 files changed, 154 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorDown.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorNet.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java delete mode 100644 src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java b/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java deleted file mode 100644 index bc8cebf..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorBottomAlgaeRemoval.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorBottomAlgaeRemoval extends Command { - Elevator elevator; - - public ElevatorBottomAlgaeRemoval(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.BOTTOMALGAE); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java b/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java deleted file mode 100644 index c5d6ca9..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorDown.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorDown extends Command { - Elevator elevator; - - public ElevatorDown(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.DOWN); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java b/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java deleted file mode 100644 index 5a5dd42..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorNet.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorNet extends Command { - Elevator elevator; - - public ElevatorNet(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.NET); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java b/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java deleted file mode 100644 index c776fb2..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorProcessor.java +++ /dev/null @@ -1,30 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; - -public class ElevatorProcessor extends Command { - private final Elevator elevator; - public ElevatorProcessor(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.PROCESSOR); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java b/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java deleted file mode 100644 index be55a57..0000000 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorUpperAlgaeRemoval.java +++ /dev/null @@ -1,31 +0,0 @@ -package org.carlmontrobotics.commands; - -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; -import org.carlmontrobotics.subsystems.Elevator; - -import edu.wpi.first.wpilibj2.command.Command; - -public class ElevatorUpperAlgaeRemoval extends Command { - Elevator elevator; - - public ElevatorUpperAlgaeRemoval(Elevator elevator) { - addRequirements(this.elevator = elevator); - } - @Override - public void initialize() { - - } - - @Override - public void execute() { - elevator.setGoal(ElevatorPos.UPPERALGAE); - } - @Override - public void end(boolean interrupted) { - - } - @Override - public boolean isFinished() { - return false; - } -} From b837f43028fde993e6fafb260aa854c19008c1fb Mon Sep 17 00:00:00 2001 From: FriedLongJohns - WindowsLaptop <81837862+FriedLongJohns@users.noreply.github.com> Date: Fri, 7 Mar 2025 19:09:55 -0600 Subject: [PATCH 137/153] fix tests --- .../carlmontrobotics/subsystems/Elevator.java | 36 ++++++++++++++----- 1 file changed, 27 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 57684b2..709c6c1 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -29,6 +29,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutVoltage; @@ -66,11 +67,21 @@ public class Elevator extends SubsystemBase { private final SysIdRoutine sysIdRoutine; // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutVoltage m_appliedVoltage = Volts.mutable(0); + private final MutVoltage[] m_appliedVoltage = new MutVoltage[2];//AH: its a holder, not a number. + //Volts.mutable(0); // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. - private final MutDistance m_distance = Meters.mutable(0); + private final MutDistance[] m_distance = new MutDistance[2];//AH: 2 for 2 elevator motors + //Meters.mutable(0); // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. - private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[2];//AH: ITS A HOLDER :o + //MetersPerSecond.mutable(0); + + //AH: need a config to run a test + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Units.Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Units.Seconds.of(5)//AH: maximum sysID test time + ); public Elevator() { //motors @@ -96,18 +107,25 @@ public Elevator() { sysIdRoutine = new SysIdRoutine( - new SysIdRoutine.Config(), + defaultSysIdConfig,//AH: use custom voltage config new SysIdRoutine.Mechanism( voltage -> { masterMotor.setVoltage(voltage); }, log -> { - log.motor("Elevator") + log.motor("Elevator-Mastr")//AH: you have 2 motors, must log both + .voltage( + m_appliedVoltage[0] + .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts))//AH: GET() IS NOT VOLTAGE + .linearPosition(m_distance[0].mut_replace(masterEncoder.getPosition(), Meters)) + .linearVelocity(m_velocity[0].mut_replace(masterEncoder.getVelocity(), MetersPerSecond));//AH: use metric units always + + log.motor("Elevator-Mastr")//AH: you have 2 motors, must log both .voltage( - m_appliedVoltage.mut_replace( - masterMotor.get() * RobotController.getBatteryVoltage(), Volts)) - .linearPosition(m_distance.mut_replace(masterEncoder.getPosition(), Inches)) - .linearVelocity(m_velocity.mut_replace(masterEncoder.getVelocity(), InchesPerSecond)); + m_appliedVoltage[1] + .mut_replace(followerMotor.getBusVoltage() * followerMotor.getAppliedOutput(), Volts))//AH: GET() IS NOT VOLTAGE + .linearPosition(m_distance[1].mut_replace(followerEncoder.getPosition(), Meters)) + .linearVelocity(m_velocity[1].mut_replace(followerEncoder.getVelocity(), MetersPerSecond)); }, this) ); From fe452cb957969c121c0971db40bb17e4bad0d6c4 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Fri, 7 Mar 2025 18:00:27 -0800 Subject: [PATCH 138/153] Changed constants to follow master config | Added (i think a limit on elevator height?). Deleted more unecessary commands --- src/main/java/org/carlmontrobotics/Constants.java | 7 +++---- .../carlmontrobotics/commands/ElevatorLPos.java | 2 +- .../carlmontrobotics/commands/TeleopElevator.java | 5 +++++ .../org/carlmontrobotics/subsystems/Elevator.java | 14 +++++--------- 4 files changed, 14 insertions(+), 14 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/commands/TeleopElevator.java diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index f80b677..0dcaeb7 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -320,6 +320,7 @@ public static final class Elevatorc { public static final int followerPort = 21; public static final int elevatorTopLimitSwitchPort = 1; public static final int elevatorBottomLimitSwitchPort = 2; + public static final double GEAR_RATIO = 0; //TODO: CHANGE TO ACTUAL GEAR RATIO // Config // TODO figure these parts out @@ -327,10 +328,8 @@ public static final class Elevatorc { public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = true; public static final boolean followerInverted = true; - public static final double masterPositionConversionFactor = 2*(Math.PI * 1.76)/20; //2(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 - public static final double followerPositionConversionFactor = 2*(Math.PI * 1.76)/20; - public static final double masterVelocityConversionFactor = 2*(Math.PI * 1.76)/20; - public static final double followerVelocityConversionFactor = 2*(Math.PI * 1.76)/20; + public static final double masterPositionConversionFactor = 2*GEAR_RATIO*(Math.PI * 1.76)/20; // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 + public static final double masterVelocityConversionFactor = 2*GEAR_RATIO*(Math.PI * 1.76)/20; public static final double maxElevatorHeightInches = 53.2; public static final double minElevatorHeightInches = 0; diff --git a/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java b/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java index afb8d93..11241b6 100644 --- a/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java +++ b/src/main/java/org/carlmontrobotics/commands/ElevatorLPos.java @@ -10,7 +10,7 @@ public class ElevatorLPos extends Command { private ElevatorPos pos; public ElevatorLPos(Elevator elevator, ElevatorPos pos) { this.elevator = elevator; - this.pos=pos; + this.pos = pos; addRequirements(elevator); } @Override diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java new file mode 100644 index 0000000..d540a2a --- /dev/null +++ b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java @@ -0,0 +1,5 @@ +package org.carlmontrobotics.commands; + +public class TeleopElevator { + +} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 709c6c1..cb04bf9 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -92,7 +92,6 @@ public Elevator() { followerConfig = new SparkMaxConfig(); followerEncoder = followerMotor.getEncoder(); configureMotors(); - //Calibration topLimitSwitch = new DigitalInput(Constants.Elevatorc.elevatorTopLimitSwitchPort); bottomLimitSwitch = new DigitalInput(Constants.Elevatorc.elevatorBottomLimitSwitchPort); @@ -120,7 +119,7 @@ public Elevator() { .linearPosition(m_distance[0].mut_replace(masterEncoder.getPosition(), Meters)) .linearVelocity(m_velocity[0].mut_replace(masterEncoder.getVelocity(), MetersPerSecond));//AH: use metric units always - log.motor("Elevator-Mastr")//AH: you have 2 motors, must log both + log.motor("Elevator-Follwr")//AH: you have 2 motors, must log both .voltage( m_appliedVoltage[1] .mut_replace(followerMotor.getBusVoltage() * followerMotor.getAppliedOutput(), Volts))//AH: GET() IS NOT VOLTAGE @@ -143,14 +142,9 @@ private void configureMotors () { .pid(Constants.Elevatorc.kP, Constants.Elevatorc.kI,Constants.Elevatorc.kD) .feedbackSensor(FeedbackSensor.kPrimaryEncoder); masterMotor.configure(masterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - //I don't know if this is needed + //I don't know if this is needed. Response: Not rly. Only the follow. //Follower Config - followerConfig - .inverted(Constants.Elevatorc.followerInverted) - .idleMode(Constants.Elevatorc.followerIdleMode); - followerConfig.encoder - .positionConversionFactor(Constants.Elevatorc.followerPositionConversionFactor) - .velocityConversionFactor(Constants.Elevatorc.followerVelocityConversionFactor); + followerConfig.apply(masterConfig); followerConfig.follow(Constants.Elevatorc.masterPort, Constants.Elevatorc.followerInverted); followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); } @@ -193,9 +187,11 @@ else if (elevatorAtMin()) { } public void getToGoal() { + if(!elevatorAtMax() || heightGoal Date: Fri, 7 Mar 2025 18:31:57 -0800 Subject: [PATCH 139/153] added teleopElevator based off last years arm --- .../java/org/carlmontrobotics/Constants.java | 2 +- .../commands/TeleopElevator.java | 63 ++++++++++++++++++- .../carlmontrobotics/subsystems/Elevator.java | 8 ++- 3 files changed, 67 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 0dcaeb7..777ad63 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -326,7 +326,7 @@ public static final class Elevatorc { // TODO figure these parts out public static final IdleMode masterIdleMode = IdleMode.kBrake; public static final IdleMode followerIdleMode = IdleMode.kBrake; - public static final boolean masterInverted = true; + public static final boolean masterInverted = true; //????? Only one of these should be true public static final boolean followerInverted = true; public static final double masterPositionConversionFactor = 2*GEAR_RATIO*(Math.PI * 1.76)/20; // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 public static final double masterVelocityConversionFactor = 2*GEAR_RATIO*(Math.PI * 1.76)/20; diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java index d540a2a..5e486ab 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java @@ -1,5 +1,64 @@ package org.carlmontrobotics.commands; -public class TeleopElevator { - +import java.util.function.DoubleSupplier; + +import org.carlmontrobotics.Constants; +import org.carlmontrobotics.subsystems.Elevator; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class TeleopElevator extends Command { + private Elevator elevator; + private DoubleSupplier joystickSupplier; + private TrapezoidProfile.State goalState; + private double lastTime; + public TeleopElevator(Elevator elevator, DoubleSupplier joystickSupplier) { + this.joystickSupplier = joystickSupplier; + addRequirements(this.elevator = elevator); + } + @Override + public void initialize() { + goalState = new TrapezoidProfile.State(elevator.getCurrentHeight(), elevator.getEleVel()); + lastTime = Timer.getFPGATimestamp(); + } + public double getReqSpeeds() { + return 1*joystickSupplier.getAsDouble(); //(MAX_ACCEL in radians/s^2 times joystick) + } + @Override + public void execute() { + double speeds = getReqSpeeds(); + //SmartDashboard.putNumber("speeds", speeds); + + if (speeds == 0) {// if no input, don't set any goals. + lastTime = Timer.getFPGATimestamp();// update deltaT even when not running + return; + } + + double currTime = Timer.getFPGATimestamp(); + double deltaT = currTime - lastTime;// only move by a tick of distance at once + lastTime = currTime; + + double goalEleRad = goalState.position + speeds * deltaT;// speed*time = dist + + goalEleRad = MathUtil.clamp(goalEleRad, Constants.Elevatorc.minElevatorHeightInches, Constants.Elevatorc.maxElevatorHeightInches); + // goalArmRad = MathUtil.clamp(goalArmRad, + // armSubsystem.getArmPos() + Math.pow(armSubsystem.getMaxVelRad(), 2) / MAX_FF_ACCEL_RAD_P_S, + // armSubsystem.getArmPos() - Math.pow(armSubsystem.getMaxVelRad(), 2) / MAX_FF_ACCEL_RAD_P_S); + + goalState.position = goalEleRad; + goalState.velocity = 0; + // don't put in constants bc it's always zero + elevator.setGoal(goalState.position); + } + @Override + public void end(boolean interrupted) { + + } + @Override + public boolean isFinished() { + return false; + } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index cb04bf9..38cf9e4 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -202,7 +202,7 @@ public void stopElevator(){ masterMotor.set(0); } - public double getEncoderValue() { + public double getPos() { return masterEncoder.getPosition(); } @@ -215,7 +215,7 @@ else if (heightGoal == Constants.Elevatorc.minElevatorHeightInches) { } else { - return (Math.abs(getEncoderValue()) - heightGoal <= Constants.Elevatorc.elevatorTolerance); + return (Math.abs(getPos()) - heightGoal <= Constants.Elevatorc.elevatorTolerance); } } @@ -238,7 +238,9 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { public Command sysIdDynamic(SysIdRoutine.Direction direction) { return sysIdRoutine.dynamic(direction); } - + public double getEleVel() { + return masterEncoder.getVelocity(); + } @Override From d520427f78ca9cfdd3bda37b8ba5092cacab0531 Mon Sep 17 00:00:00 2001 From: FriedLongJohns <81837862+FriedLongJohns@users.noreply.github.com> Date: Fri, 7 Mar 2025 18:40:54 -0800 Subject: [PATCH 140/153] Added teleopelevator default command and fixed more magic numbers --- src/main/java/org/carlmontrobotics/Constants.java | 1 + src/main/java/org/carlmontrobotics/RobotContainer.java | 1 + .../java/org/carlmontrobotics/commands/TeleopElevator.java | 6 +++--- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 777ad63..463dfd9 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -324,6 +324,7 @@ public static final class Elevatorc { // Config // TODO figure these parts out + public static final double MAX_ACCEL_RAD_P_S = 1; public static final IdleMode masterIdleMode = IdleMode.kBrake; public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = true; //????? Only one of these should be true diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index fa08f45..78483e6 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -115,6 +115,7 @@ private void setDefaultCommands() { () -> ProcessedAxisValue(driverController, Axis.kLeftX), () -> ProcessedAxisValue(driverController, Axis.kRightX), () -> driverController.getRawButton(OI.Driver.slowDriveButton))); + elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); } private void setBindingsDriver() { diff --git a/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java index 5e486ab..2adf52f 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopElevator.java @@ -2,7 +2,7 @@ import java.util.function.DoubleSupplier; -import org.carlmontrobotics.Constants; +import static org.carlmontrobotics.Constants.Elevatorc.*; import org.carlmontrobotics.subsystems.Elevator; import edu.wpi.first.math.MathUtil; @@ -25,7 +25,7 @@ public void initialize() { lastTime = Timer.getFPGATimestamp(); } public double getReqSpeeds() { - return 1*joystickSupplier.getAsDouble(); //(MAX_ACCEL in radians/s^2 times joystick) + return MAX_ACCEL_RAD_P_S*joystickSupplier.getAsDouble(); //(MAX_ACCEL in radians/s^2 times joystick) } @Override public void execute() { @@ -43,7 +43,7 @@ public void execute() { double goalEleRad = goalState.position + speeds * deltaT;// speed*time = dist - goalEleRad = MathUtil.clamp(goalEleRad, Constants.Elevatorc.minElevatorHeightInches, Constants.Elevatorc.maxElevatorHeightInches); + goalEleRad = MathUtil.clamp(goalEleRad, minElevatorHeightInches, maxElevatorHeightInches); // goalArmRad = MathUtil.clamp(goalArmRad, // armSubsystem.getArmPos() + Math.pow(armSubsystem.getMaxVelRad(), 2) / MAX_FF_ACCEL_RAD_P_S, // armSubsystem.getArmPos() - Math.pow(armSubsystem.getMaxVelRad(), 2) / MAX_FF_ACCEL_RAD_P_S); From 3ef9ca2ade28b42ed25b9897ca1360521fc8eb6a Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sat, 8 Mar 2025 23:05:20 -0800 Subject: [PATCH 141/153] Synced into meters --- src/main/java/org/carlmontrobotics/Constants.java | 8 ++++---- .../java/org/carlmontrobotics/subsystems/Elevator.java | 2 +- 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 463dfd9..2c7c51d 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -329,8 +329,8 @@ public static final class Elevatorc { public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = true; //????? Only one of these should be true public static final boolean followerInverted = true; - public static final double masterPositionConversionFactor = 2*GEAR_RATIO*(Math.PI * 1.76)/20; // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 - public static final double masterVelocityConversionFactor = 2*GEAR_RATIO*(Math.PI * 1.76)/20; + public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)/20); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 + public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)/20); public static final double maxElevatorHeightInches = 53.2; public static final double minElevatorHeightInches = 0; @@ -371,8 +371,8 @@ public enum ElevatorPos { this.positionInches = positionInches; } - public double getPosition() { - return positionInches; + public double getPositioninMeters() { + return Units.degreesToRadians(positionInches); } } diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 38cf9e4..8c7f011 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -154,7 +154,7 @@ public void setGoal(double goal) { } public void setGoal(ElevatorPos goal) { - heightGoal = goal.getPosition(); + heightGoal = goal.getPositioninMeters(); } public double getGoal() { From c9a35cff35fbf1a32bb0bbc67bb56b35960ee0d9 Mon Sep 17 00:00:00 2001 From: stwiggy Date: Sun, 9 Mar 2025 12:28:46 -0700 Subject: [PATCH 142/153] uses lib199 to create motors; gear ratio and inversion constants --- build.gradle | 2 +- .../java/org/carlmontrobotics/Constants 2.j | 467 ----------------- .../java/org/carlmontrobotics/Constants.java | 6 +- .../org/carlmontrobotics/RobotContainer 2.j | 488 ------------------ .../carlmontrobotics/subsystems/Elevator.java | 81 +-- 5 files changed, 47 insertions(+), 997 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/Constants 2.j delete mode 100644 src/main/java/org/carlmontrobotics/RobotContainer 2.j diff --git a/build.gradle b/build.gradle index 3f08c23..fa2d511 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" } diff --git a/src/main/java/org/carlmontrobotics/Constants 2.j b/src/main/java/org/carlmontrobotics/Constants 2.j deleted file mode 100644 index 2b72303..0000000 --- a/src/main/java/org/carlmontrobotics/Constants 2.j +++ /dev/null @@ -1,467 +0,0 @@ - -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics; - -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; -import edu.wpi.first.wpilibj.util.Color; -import edu.wpi.first.wpilibj.util.Color8Bit; -import edu.wpi.first.wpilibj2.command.button.POVButton; -import edu.wpi.first.wpilibj.util.Color; - -/** - * The Constants class provides a convenient place for teams to hold robot-wide - * numerical or boolean - * constants. This class should not be used for any other purpose. All constants - * should be declared - * globally (i.e. public static). Do not put anything functional in this class. - * - *

- * It is advised to statically import this class (or one of its inner classes) - * wherever the - * constants are needed, to reduce verbosity. - */ -public final class Constants { - public static final double g = 9.81; // meters per second squared - public static final class Led { - - public static final Color8Bit DEFAULT_COLOR_BLUE = new Color8Bit(0, 0, 200); - public static final Color8Bit DETECT_NOTE_YELLOW = new Color8Bit(255, 255, 0); - public static final Color8Bit HOLDING_GREEN = new Color8Bit(0, 250, 0); - public static final Color8Bit RED_NEO_550_MOTOR = new Color8Bit(255,0,0); - public static final int ledPort = 0; - // TODO: figure out how to get port of LED, it could be 0 or - } - - public static final class Effectorc { - // PID values - - public static final int INTAKE = 0; - public static final int OUTTAKE = 1; - // 0.0001184 - - /* - * public static final double kP = 0.0001067; public static final double kI = 0; public - * static final double kD = 0; public static final double kS = 0; public static final double - * kV = 0.11124; public static final double kA = 0.039757; - */ - - public static final double[] kP = {0, 0.0001067}; - public static final double[] kI = { /* /Intake/ */0, /* /Outake/ */0 }; - public static final double[] kD = { /* /Intake/ */0, /* /Outake/ */0 }; - public static final double[] kS = { /* /Intake/ */0.22, /* /Outake/ */0.29753 * 2 }; - public static final double[] kV = {0.122, 0.11124}; - public static final double[] kA = {0, 0.039757}; - public static final int INTAKE_PORT = 9; // port - public static final int OUTTAKE_PORT = 10; // port - public static final int INTAKE_DISTANCE_SENSOR_PORT = 11; // port - public static final int OUTAKE_DISTANCE_SENSOR_PORT = 10; // port - public static final double DISTANCE_BETWEEN_SENSORS_INCHES = 8.189; // inches - public static final double OFFSET_FROM_GROUND_INCHES = 21; // in - public static final double DS_DEPTH_INCHES = 9.97; // Distance sensor Depth - public static final double DETECT_DISTANCE_INCHES = 13; - - public static final double INTAKE_RPM = 6300; - public static final double INTAKE_SLOWDOWN_RPM = 4500; - public static final double MAX_SECONDS_OVERLOAD = 2.0; - public static final double PASS_RPM = 2000; - public static final double TEST_RPM = 3000; - public static final double AMP_RPM = 1000; - public static final double SUBWOOFER_RPM = 2100; - public static final double PODIUM_RPM = 4000; - public static final double SPEAKER_RPM = 2100; - // WTF FAB ISSUE - //public static final double SAFE_RPM = 6000;// WTF FAB ISSUE - - public static final double EJECT_RPM_INTAKE = -2550; - public static final double EJECT_RPM_OUTAKE = -2550; - - public static final double MANUAL_RPM_MAX = 9500; - - public static final double RPM_TOLERANCE = 200; - public static final double SPEAKER_HEIGHT_INCHES = 83; // inches - - public static final boolean INTAKE_MOTOR_INVERSION = true; - public static final boolean OUTAKE_MOTOR_INVERSION = false; - - public static final double EJECT_TIME_SECS = 5.; - public static final double EJECT_MIN_SECS = 1.25; - public static final double INTAKE_TIME_SECS = 4.; - public static final double ledDefaultColorRestoreTime = 3; - public static final Color defaultColor = new Color(0, 0, 200); - public static final Color pickupSuccessColor = new Color(0, 200, 0); - - } - - public static final class Armc { - - // Motor port - public static final int ARM_MOTOR_PORT_MASTER = - CONFIG.isSwimShady() ? 20 : 13; - public final static int ARM_MOTOR_PORT_FOLLOWER = 18; - // Config for motors - public static final boolean MOTOR_INVERTED_MASTER = false; - public static final boolean MOTOR_INVERTED_FOLLOWER = true; // verifyed by design AND physical testing - - public static final double ROTATION_TO_RAD = 2 * Math.PI; - public static final boolean ENCODER_INVERTED = true; - - // difference between CoG and arm is .328 rad - public static final double ENCODER_OFFSET_RAD = 6.132; - // 0.08 + .328 - 0.404; // - 0.6095; - - // TODO: finish understand why this is broken public static final Measure - // INTAKE_ANGLE = Degrees.to(-1); - - // USE RADIANS FOR THE ARM - public static final double INTAKE_ANGLE_RAD = Units.degreesToRadians(0); - public static final double HANG_ANGLE_RAD = Units.degreesToRadians(90); - public static final double AMP_ANGLE_RAD = 1.28; - public static final double AMP_ANGLE_RAD_NEW_MOTOR = 1.456; - public static final double SPEAKER_ANGLE_RAD = -0.2; - public static final double PODIUM_ANGLE_RAD = -0.2 + Units.degreesToRadians(7.5); - // -0.427725 - public static final double GROUND_INTAKE_POS = -0.34537; - - public static final double HANG_ANGL_RAD = GROUND_INTAKE_POS + Units.degreesToRadians(30); - - public static final double SUBWOOFER_ANGLE_RAD = -0.22;// touching the base of the speaker - public static final double SAFE_ZONE_ANGLE_RAD = Units.degreesToRadians(36);// touching the white line - //public static final double PODIUM_ANGLE_RAD = Units.degreesToRadians(40);// touching the safe pad on the stage - - // 0.4 rad for shooting at podium - - // PID, Feedforward, Trapezoid - public static final double kP = 0.45;// 5.7938; // (2 * Math.PI); - public static final double kI = 0; - public static final double kD = 0 * 1000; - public static final double kS = 1.6 / 2; // 0.1498; - public static final double kG = 0.8067; // 0.3489; - public static final double kV = 5.1201 / (2 * Math.PI); - public static final double kA = 0.43308 / (2 * Math.PI); - public static final double IZONE_RAD = 0; - // fine for now, change it later before use - ("Incorect use of setIZone()" - // Issue #22) - // public static final double MAX_FF_VEL_RAD_P_S = 0.2; //rad/s - public static final double MAX_FF_VEL_RAD_P_S = Math.PI * .5; - public static final double MAX_FF_ACCEL_RAD_P_S = 53.728 / 4; // rad / s^2 - // ((.89*2)/(1.477/(61.875^2))/61.875)-20.84 - - public static final double MAX_FF_VEL_RAD_P_S_BABY = 0; - public static final double MAX_FF_ACCEL_RAD_P_S_BABY = 0; - //TODO: determine these values^ - - - public static final double SOFT_LIMIT_LOCATION_IN_RADIANS = 0; - public static final double CLIMB_POS = 1.701; //RADIANS - public static final double MIN_VOLTAGE = -0.5; // -((kS + kG + 1)/12); - public static final double MAX_VOLTAGE = 0.5; // (kS + kG + 1)/12; - public static final double MIN_VOLTAGE_BABY = MIN_VOLTAGE/12 *0.7; - public static final double MAX_VOLTAGE_BABY = MAX_VOLTAGE/12*0.7; - public static final double CLIMB_FINISH_POS = -0.38; - // if needed - public static final double COM_ARM_LENGTH_METERS = 0.381; - public static final double ARM_MASS_KG = 9.59302503; - - public static TrapezoidProfile.Constraints TRAP_CONSTRAINTS;// initalized by arm constructor - // other0; - - // public static final double MARGIN_OF_ERROR = Math.PI / 18; v^2 /a - - // Boundaries - public static final double ARM_TELEOP_MAX_GOAL_DIFF_FROM_CURRENT_RAD = 1.8345; // placeholder - public static final double POS_TOLERANCE_RAD = - Units.degreesToRadians(5); // placeholder //Whether or not this is the actual - // account - // idk TODO: test on actual encoder without a conversion - // factor - public static final double VEL_TOLERANCE_RAD_P_SEC = (POS_TOLERANCE_RAD / 0.02); // 20ms per robot loop - public static final double UPPER_ANGLE_LIMIT_RAD = 1.63; - public static final double LOWER_ANGLE_LIMIT_RAD = -0.5; - public static final double CLIMBER_UP_ANGLE_RAD = UPPER_ANGLE_LIMIT_RAD; - public static final double CLIMBER_DOWN_ANGLE_RAD = LOWER_ANGLE_LIMIT_RAD; - - public static final double ARM_DISCONT_RAD = (LOWER_ANGLE_LIMIT_RAD + UPPER_ANGLE_LIMIT_RAD) / 2 - Math.PI; - - public static final double DISCONNECTED_ENCODER_TIMEOUT_SEC = 0.5; - public static final double SMART_CURRENT_LIMIT_TIMEOUT = 0.8; - - - public static final int NEO_BUILTIN_ENCODER_CPR = 42; - } - - public static final class Drivetrainc { - - // #region Subsystem Constants - - public static final double wheelBase = CONFIG.isSwimShady() ? Units.inchesToMeters(19.75) - : Units.inchesToMeters(16.75); - public static final double trackWidth = CONFIG.isSwimShady() ? Units.inchesToMeters(28.75) - : Units.inchesToMeters(23.75); - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double swerveRadius = Math.sqrt(Math.pow(wheelBase / 2, 2) + Math.pow(trackWidth / 2, 2)); - // The gearing reduction from the drive motor controller to the wheels - // Gearing for the Swerve Modules is 6.75 : 1 - public static final double driveGearing = 6.75; - // Turn motor shaft to "module shaft" - public static final double turnGearing = 150 / 7; - - public static final double driveModifier = 1; - public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36 / 7.65 /* - * empirical - * correction - */; - public static final double mu = 1; /* 70/83.2; */ - - public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60; // radians/s - // Angular speed to translational speed --> v = omega * r / gearing - public static final double maxSpeed = NEOFreeSpeed * (wheelDiameterMeters / 2.0) / driveGearing; - public static final double maxForward = maxSpeed; // todo: use smart dashboard to figure this out - public static final double maxStrafe = maxSpeed; // todo: use smart dashboard to figure this out - // seconds it takes to go from 0 to 12 volts(aka MAX) - public static final double secsPer12Volts = 0.1; - - // maxRCW is the angular velocity of the robot. - // Calculated by looking at one of the motors and treating it as a point mass - // moving around in a circle. - // Tangential speed of this point mass is maxSpeed and the radius of the circle - // is sqrt((wheelBase/2)^2 + (trackWidth/2)^2) - // Angular velocity = Tangential speed / radius - public static final double maxRCW = maxSpeed / swerveRadius; - - public static final boolean[] reversed = { false, false, false, false }; - // public static final boolean[] reversed = {true, true, true, true}; - // Determine correct turnZero constants (FL, FR, BL, BR) - public static final double[] turnZeroDeg = RobotBase.isSimulation() ? new double[] {-90.0, -90.0, -90.0, -90.0 } - : (CONFIG.isSwimShady() ? new double[] { 85.7812, 85.0782, -96.9433, -162.9492 } - : new double[] { -48.6914, 63.3691, 94.1309, -6.7676 });/* real values here */ - - // kP, kI, and kD constants for turn motor controllers in the order of - // front-left, front-right, back-left, back-right. - // Determine correct turn PID constants - public static final double[] turnkP = { 51.078, 60.885, 60.946, 60.986 }; // {0.00374, 0.00374, 0.00374, - // 0.00374}; - public static final double[] turnkI = { 0, 0, 0, 0 }; - public static final double[] turnkD = { 0/* dont edit */, 0.5, 0.42, 1 }; // todo: use d - // public static final double[] turnkS = {0.2, 0.2, 0.2, 0.2}; - public static final double[] turnkS = { 0.13027, 0.17026, 0.2, 0.23262 }; - - // V = kS + kV * v + kA * a - // 12 = 0.2 + 0.00463 * v - // v = (12 - 0.2) / 0.00463 = 2548.596 degrees/s - public static final double[] turnkV = { 2.6532, 2.7597, 2.7445, 2.7698 }; - public static final double[] turnkA = { 0.17924, 0.17924, 0.17924, 0.17924 }; - - // kP is an average of the forward and backward kP values - // Forward: 1.72, 1.71, 1.92, 1.94 - // Backward: 1.92, 1.92, 2.11, 1.89 - // Order of modules: (FL, FR, BL, BR) - public static final double[] drivekP = CONFIG.isSwimShady() ? new double[] { 2.8, 2.8, 2.8, 2.8 } - : new double[] { 1.75, 1.75, 1.75, .75 }; // {1.82/100, 1.815/100, 2.015/100, - // 1.915/100}; - public static final double[] drivekI = { 0, 0, 0, 0 }; - public static final double[] drivekD = { 0, 0, 0, 0 }; - public static final boolean[] driveInversion = (CONFIG.isSwimShady() - ? new boolean[] { false, false, false, false } - : new boolean[] { true, false, true, false }); - public static final boolean[] turnInversion = { true, true, true, true }; - // kS - public static final double[] kForwardVolts = { 0.26744, 0.31897, 0.27967, 0.2461 }; - public static final double[] kBackwardVolts = kForwardVolts; - - public static final double[] kForwardVels = { 2.81, 2.9098, 2.8378, 2.7391 }; - public static final double[] kBackwardVels = kForwardVels; - public static final double[] kForwardAccels = { 1.1047 / 2, 0.79422 / 2, 0.77114 / 2, 1.1003 / 2 }; - public static final double[] kBackwardAccels = kForwardAccels; - - public static final double autoMaxSpeedMps = 0.35 * 4.4; // Meters / second - public static final double autoMaxAccelMps2 = mu * g; // Meters / seconds^2 - public static final double autoMaxVolt = 10.0; // For Drivetrain voltage constraint in RobotPath.java - // The maximum acceleration the robot can achieve is equal to the coefficient of - // static friction times the gravitational acceleration - // a = mu * 9.8 m/s^2 - public static final double autoCentripetalAccel = mu * g * 2; - - public static final boolean isGyroReversed = true; - - // PID values are listed in the order kP, kI, and kD - public static final double[] xPIDController = CONFIG.isSwimShady() ? new double[] { 4, 0.0, 0.0 } - : new double[] { 2, 0.0, 0.0 }; - public static final double[] yPIDController = xPIDController; - public static final double[] thetaPIDController = CONFIG.isSwimShady() ? new double[] { 0.10, 0.0, 0.001 } - : new double[] {0.05, 0.0, 0.00}; - - public static final SwerveConfig swerveConfig = new SwerveConfig(wheelDiameterMeters, driveGearing, mu, - autoCentripetalAccel, kForwardVolts, kForwardVels, kForwardAccels, kBackwardVolts, kBackwardVels, - kBackwardAccels, drivekP, drivekI, drivekD, turnkP, turnkI, turnkD, turnkS, turnkV, turnkA, turnZeroDeg, - driveInversion, reversed, driveModifier, turnInversion); - - // public static final Limelight.Transform limelightTransformForPoseEstimation = - // Transform.BOTPOSE_WPIBLUE; - - // #endregion - - // #region Ports - - public static final int driveFrontLeftPort = CONFIG.isSwimShady() ? 8 : 11; // - public static final int driveFrontRightPort = CONFIG.isSwimShady() ? 13 : 19; // - public static final int driveBackLeftPort = CONFIG.isSwimShady() ? 5 : 14; // - public static final int driveBackRightPort = CONFIG.isSwimShady() ? 11 : 17; // correct - - public static final int turnFrontLeftPort = CONFIG.isSwimShady() ? 7 : 12; // - public static final int turnFrontRightPort = CONFIG.isSwimShady() ? 14 : 20; // 20 - public static final int turnBackLeftPort = CONFIG.isSwimShady() ? 6 : 15; // - public static final int turnBackRightPort = CONFIG.isSwimShady() ? 12 : 16; // correct - - public static final int canCoderPortFL = CONFIG.isSwimShady() ? 4 : 0; - public static final int canCoderPortFR = CONFIG.isSwimShady() ? 2 : 3; - public static final int canCoderPortBL = CONFIG.isSwimShady() ? 3 : 2; - public static final int canCoderPortBR = CONFIG.isSwimShady() ? 1 : 1; - - // #endregion - - // #region Command Constants - - public static double kNormalDriveSpeed = 1; // Percent Multiplier - public static double kNormalDriveRotation = 0.5; // Percent Multiplier - public static double kSlowDriveSpeed = 0.4; // Percent Multiplier - public static double kSlowDriveRotation = 0.250; // Percent Multiplier - - //baby speed values, i just guessed the percent multiplier. TODO: find actual ones we wana use - public static double kBabyDriveSpeed = 0.3; - public static double kBabyDriveRotation = 0.2; - public static double kAlignMultiplier = 1D / 3D; - public static final double kAlignForward = 0.6; - - public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to - // orient the wheels to the correct angle. This - // should be very small to avoid actually moving the - // robot. - - public static final double[] positionTolerance = { Units.inchesToMeters(.5), Units.inchesToMeters(.5), 5 }; // Meters, - // Meters, - // Degrees - public static final double[] velocityTolerance = { Units.inchesToMeters(1), Units.inchesToMeters(1), 5 }; // Meters, - // Meters, - // Degrees/Second - - // #endregion - // #region Subsystem Constants - - // "swerveRadius" is the distance from the center of the robot to one of the - // modules - public static final double turnkP_avg = (turnkP[0] + turnkP[1] + turnkP[2] + turnkP[3]) / 4; - public static final double turnIzone = .1; - - public static final double driveIzone = .1; - - public static final class Autoc { - public static final ReplanningConfig replanningConfig = new ReplanningConfig( /* - * put in - * Constants.Drivetrain.Auto - */ - false, // replan at start of path if robot not at start of path? - false, // replan if total error surpasses total error/spike threshold? - 1.5, // total error threshold in meters that will cause the path to be replanned - .8 // error spike threshold, in meters, that will cause the path to be replanned - ); - public static final PathConstraints pathConstraints = new PathConstraints(1.54, 6.86, 2 * Math.PI, - 2 * Math.PI); // The constraints for this path. If using a differential drivetrain, the - // angular constraints have no effect. - } - } - - public static final class Limelightc { - public static final String INTAKE_LL_NAME = "limelight-intake"; - public static final String SHOOTER_LL_NAME = "limelight-shooter"; - - public static final int[] VALID_IDS = { 4, 7 }; - - public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field - public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field - public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high - // to represent unreliable heading - public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity - - public static final double ERROR_TOLERANCE_RAD = 0.1; - public static final double HORIZONTAL_FOV_DEG = 0; // unused - public static final double RESOLUTION_WIDTH_PIX = 640; // unused - public static final double MOUNT_ANGLE_DEG_SHOOTER = 55.446; - public static final double MOUNT_ANGLE_DEG_INTAKE = -29; - public static final double HEIGHT_FROM_GROUND_METERS_SHOOTER = Units.inchesToMeters(8.891); - public static final double HEIGHT_FROM_GROUND_METERS_INTAKE = - Units.inchesToMeters(13); - public static final double ARM_TO_OUTTAKE_OFFSET_DEG = 115; // unused - public static final double NOTE_HEIGHT = Units.inchesToMeters(2); // unused - public static final double MIN_MOVEMENT_METERSPSEC = 1.5; - public static final double MIN_MOVEMENT_RADSPSEC = 0.5; - public static final double HEIGHT_FROM_RESTING_ARM_TO_SPEAKER_METERS = Units.inchesToMeters(65.5675); - public static final double SIDEWAYS_OFFSET_TO_OUTTAKE_MOUTH = Units.inchesToMeters(10.911); - public static final double END_EFFECTOR_BASE_ANGLE_RADS = Units.degreesToRadians(75); - public static final double VERTICAL_OFFSET_FROM_ARM_PIVOT = Units.inchesToMeters(3.65); // unused - public static final class Apriltag { - public static final int RED_SPEAKER_CENTER_TAG_ID = 4; - public static final int BLUE_SPEAKER_CENTER_TAG_ID = 7; - public static final double SPEAKER_CENTER_HEIGHT_METERS = Units.inchesToMeters(60.125); - public static final double HEIGHT_FROM_BOTTOM_TO_SUBWOOFER = Units.inchesToMeters(26); - public static final double HEIGHT_FROM_BOTTOM_TO_ARM_RESTING = Units.inchesToMeters(21.875); - } - } - - public static final class OI { - public static final class Driver { - public static final int port = 0; - - public static final int slowDriveButton = Button.kLeftBumper.value; - public static final int resetFieldOrientationButton = Button.kRightBumper.value; - public static final int toggleFieldOrientedButton = Button.kStart.value; - - public static final int rotateFieldRelative0Deg = Button.kY.value; - public static final int rotateFieldRelative90Deg = Button.kB.value; - public static final int rotateFieldRelative180Deg = Button.kA.value; - public static final int rotateFieldRelative270Deg = Button.kX.value; - } - - public static final class Manipulator { - public static final int port = 1; - // NEW BINDINGS(easier for manipulator) - // Xbox left joy Y axis -> raw Intake control - // Xbox right joy Y axis -> raw Outtake control - // Xbox right trigger axis -> Intake pos + intake - // Xbox left trigger axis -> amp pos , eject into amp - // Xbox left bumper button -> CLOSE Speaker pos , Fire - // Xbox right bumper button -> SAFE Speaker pos , Fire - // Xbox X button -> goto Intake pos - // Xbox Y button -> Eject rpm - public static final Axis INTAKE_BUTTON = Axis.kLeftTrigger; - public static final Axis SHOOTER_BUTTON = Axis.kRightTrigger; - public static final int EJECT_BUTTON = Button.kLeftBumper.value; - public static final int OPPOSITE_EJECT = Button.kRightBumper.value; - public static final Axis INTAKE_AX = Axis.kRightTrigger; - public static final Axis AMP_AX = Axis.kLeftTrigger; - public static final int SPEAKER_POS = Button.kX.value; - public static final int ARM_TO_AMP_BUTTON = Button.kY.value; - public static final int PASS_TO_OUTTAKE_STICK = - Button.kLeftStick.value; - public static final int PASS_TO_INTAKE_STICK = - Button.kRightStick.value; - public static final int UP_D_PAD = 0; - public static final int DOWN_D_PAD = 180; - public static final int LEFT_D_PAD = 270; - - public static final int A_BUTTON = Button.kA.value; - public static final int RAMP_OUTTAKE = Button.kB.value; - } - - - public static final double JOY_THRESH = 0.01; - public static final double MIN_AXIS_TRIGGER_VALUE = 0.2;// woah, this is high. - - } -} diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 2c7c51d..088629c 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -317,17 +317,17 @@ public static final class Manipulator { public static final class Elevatorc { // ports public static final int masterPort = 20; - public static final int followerPort = 21; + public static final int followerPort = 21; // inverted public static final int elevatorTopLimitSwitchPort = 1; public static final int elevatorBottomLimitSwitchPort = 2; - public static final double GEAR_RATIO = 0; //TODO: CHANGE TO ACTUAL GEAR RATIO + public static final double GEAR_RATIO = 1/20; //TODO: CHANGE TO ACTUAL GEAR RATIO // Config // TODO figure these parts out public static final double MAX_ACCEL_RAD_P_S = 1; public static final IdleMode masterIdleMode = IdleMode.kBrake; public static final IdleMode followerIdleMode = IdleMode.kBrake; - public static final boolean masterInverted = true; //????? Only one of these should be true + public static final boolean masterInverted = false; public static final boolean followerInverted = true; public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)/20); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)/20); diff --git a/src/main/java/org/carlmontrobotics/RobotContainer 2.j b/src/main/java/org/carlmontrobotics/RobotContainer 2.j deleted file mode 100644 index ebddde9..0000000 --- a/src/main/java/org/carlmontrobotics/RobotContainer 2.j +++ /dev/null @@ -1,488 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package org.carlmontrobotics; - -// java -import java.util.ArrayList; -import java.util.List; - -// constants -import static org.carlmontrobotics.Constants.Armc.*; -import static org.carlmontrobotics.Constants.OI.Manipulator.*; -import static org.carlmontrobotics.Constants.Effectorc.*; -import static org.carlmontrobotics.Constants.Limelightc.*; - -// non static constants -import org.carlmontrobotics.Constants.OI; -import org.carlmontrobotics.Constants.OI.Driver; -import org.carlmontrobotics.Constants.OI.Manipulator; -import org.carlmontrobotics.Constants.Armc; -import org.carlmontrobotics.Constants.Drivetrainc.Autoc; -import org.carlmontrobotics.Constants.Effectorc; -// robotcode2024 imports -import org.carlmontrobotics.commands.*; -import org.carlmontrobotics.subsystems.*; - -import com.pathplanner.lib.auto.AutoBuilder; -// pathplanner -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.GoalEndState; -import com.pathplanner.lib.path.PathPlannerPath; - -// wpilib geometry classes -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; -// controllers -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.GenericHID; -import edu.wpi.first.wpilibj.XboxController.Axis; -import edu.wpi.first.wpilibj.XboxController.Button; - -// dashboards -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -// commands -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ConditionalCommand; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.PrintCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -// control bindings -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.POVButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; - -public class RobotContainer { - private static boolean babyMode = false; - - // 1. using GenericHID allows us to use different kinds of controllers - // 2. Use absolute paths from constants to reduce confusion - public final GenericHID driverController = new GenericHID(Driver.port); - public final GenericHID manipulatorController = new GenericHID(Manipulator.port); - private final IntakeShooter intakeShooter = new IntakeShooter(); - - // ignore warning, LED must be initialized - private final Led led = new Led(intakeShooter); - private final Arm arm = new Arm(); - private final Drivetrain drivetrain = new Drivetrain(); - private final Limelight limelight = new Limelight(drivetrain); - - /* These are assumed to be equal to the AUTO ames in pathplanner */ - /* These must be equal to the pathPlanner path names from the GUI! */ - // Order matters - but the first one is index 1 on the physical selector - index - // 0 is reserved for - // null command. - // the last auto is hard-coded to go straight. since we have __3__ Autos, port 4 - // is simple - // straight - private List autoCommands; - private SendableChooser autoSelector = new SendableChooser(); - - private boolean hasSetupAutos = false; - private final String[] autoNames = new String[] { - /* These are assumed to be equal to the AUTO ames in pathplanner */ - "Left-Auto Ruiner", "Center-Auto Ruiner", "Right-Auto Ruiner", - "Right Limelight 4 Piece", "Left Limelight 4 Piece", - "Center Limelight 4 Piece", - - "Left-Amp", - - "Center Limelight 1 Piece", "Left Limelight 1 Piece", - "Right Limelight 1 Piece", "Center Field Limelight", - "Center Field Limelight(No Preload)", "Center Forward", - "Right Forward", "Left Forward", - "Backup-Center", "Backup-Right", "Backup-Left", - "Preload1Center", "Preload1Right", "Preload1Left", - - }; - DigitalInput[] autoSelectors = new DigitalInput[Math.min(autoNames.length, 10)]; - - public RobotContainer() { - { - // Put any configuration overrides to the dashboard and the terminal - SmartDashboard.putData("CONFIG overrides", Config.CONFIG); - System.out.println(Config.CONFIG); - - SmartDashboard.putData("BuildConstants", BuildInfo.getInstance()); - - SmartDashboard.setDefaultBoolean("babymode", babyMode); - SmartDashboard.setPersistent("babymode"); - // safe auto setup... stuff in setupAutos() is not safe to run here - will break - // robot - registerAutoCommands(); - SmartDashboard.putData(autoSelector); - SmartDashboard.setPersistent("SendableChooser[0]"); - - autoSelector.addOption("Nothing", 0); - autoSelector.addOption("Raw Forward", 1); - autoSelector.addOption("PP Simple Forward", 2);// index corresponds to index in autoCommands[] - - int i = 3; - for (String n : autoNames) { - autoSelector.addOption(n, i); - i++; - } - - ShuffleboardTab autoSelectorTab = Shuffleboard.getTab("Auto Chooser Tab"); - autoSelectorTab.add(autoSelector).withSize(2, 1); - } - - setDefaultCommands(); - setBindingsDriver(); - // setBindingsManipulatorENDEFF(); - setBindingsManipulator(); - } - - private void setDefaultCommands() { - drivetrain.setDefaultCommand(new TeleopDrive(drivetrain, - () -> ProcessedAxisValue(driverController, Axis.kLeftY), - () -> ProcessedAxisValue(driverController, Axis.kLeftX), - () -> ProcessedAxisValue(driverController, Axis.kRightX), - () -> driverController.getRawButton(Driver.slowDriveButton))); - // TODO: Are we going to use default command for intakeshooter? - intakeShooter.setDefaultCommand(new TeleopEffector(intakeShooter, - () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY), - manipulatorController, driverController)); - // TODO - // intakeShooter.setDefaultCommand(new RampMaxRPMDriving(intakeShooter)); - - arm.setDefaultCommand( - Config.CONFIG.useSmartDashboardControl ? new TestArmToPos(arm) - : new TeleopArm(arm, - () -> ProcessedAxisValue(manipulatorController, - Axis.kLeftY))); - - } - - private void setBindingsDriver() { - new JoystickButton(driverController, Driver.resetFieldOrientationButton) - .onTrue(new InstantCommand(drivetrain::resetFieldOrientation)); - // axisTrigger(driverController, Axis.kRightTrigger) - // .whileTrue(new SequentialCommandGroup(new PrintCommand("Running Intake"), - // new AutoMATICALLYGetNote(drivetrain, intakeShooter, limelight))); - - new POVButton(driverController, 0) - .whileTrue(new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 1))); - - axisTrigger(driverController, Axis.kLeftTrigger) - // .onTrue(new AlignToApriltag(drivetrain, limelight)); - .onTrue(new InstantCommand(() -> drivetrain.setFieldOriented(false))) - .onFalse(new InstantCommand(() -> drivetrain.setFieldOriented(true))); - - axisTrigger(driverController, Manipulator.SHOOTER_BUTTON) - .whileTrue(new AlignToApriltag(drivetrain, limelight, 2.0)); - new JoystickButton(driverController, Driver.rotateFieldRelative0Deg).onTrue( - new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(0), drivetrain)); - new JoystickButton(driverController, Driver.rotateFieldRelative90Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(270), - drivetrain)); - new JoystickButton(driverController, Driver.rotateFieldRelative180Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(180), - drivetrain)); - new JoystickButton(driverController, Driver.rotateFieldRelative270Deg) - .onTrue(new RotateToFieldRelativeAngle(Rotation2d.fromDegrees(90), - drivetrain)); - } - - private void setBindingsManipulator() { - new JoystickButton(manipulatorController, EJECT_BUTTON) - .onTrue(new Eject(intakeShooter)); - - // new JoystickButton(manipulatorController, A_BUTTON) - // .onTrue(new RampMaxRPMDriving(intakeShooter)); - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - // new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), - // new PassToOuttake(intakeShooter))); - - axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON).whileTrue( - new ConditionalCommand(new SequentialCommandGroup(new AimArmSpeaker(arm, limelight), - new PassToOuttake(intakeShooter)), new InstantCommand(() -> { - }), () -> LimelightHelpers.getTV(SHOOTER_LL_NAME))); - - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - // .whileTrue(new PassToOuttake(intakeShooter)); - - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - // .whileTrue(new AimArmSpeaker(arm, limelight)); - - new JoystickButton(manipulatorController, RAMP_OUTTAKE) - .whileTrue(new RampMaxRPM(intakeShooter)); - new JoystickButton(manipulatorController, OPPOSITE_EJECT) - .whileTrue(new EjectOuttakeSide(intakeShooter)); - - /* - * axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - * .onTrue(Config.CONFIG.useSmartDashboardControl - * ? new TestRPM(intakeShooter) - * : new Outtake(intakeShooter, arm) - */ - - // axisTrigger(manipulatorController, Manipulator.SHOOTER_BUTTON) - // .onTrue(new PassToOuttake(intakeShooter)); - - axisTrigger(manipulatorController, Manipulator.INTAKE_BUTTON) - .whileTrue(new Intake(intakeShooter)); - new JoystickButton(manipulatorController, ARM_TO_AMP_BUTTON) - .onTrue(new ArmToPos(arm, AMP_ANGLE_RAD_NEW_MOTOR)); - new JoystickButton(manipulatorController, A_BUTTON) - .onTrue(new ArmToPos(arm, GROUND_INTAKE_POS)); - new JoystickButton(manipulatorController, PASS_TO_OUTTAKE_STICK) - .onTrue(new PassToOuttake(intakeShooter)); - new JoystickButton(manipulatorController, PASS_TO_INTAKE_STICK) - .onTrue(new PassToIntake(intakeShooter)); - new JoystickButton(manipulatorController, SPEAKER_POS) - .onTrue(new ArmToPos(arm, SPEAKER_ANGLE_RAD)); - new POVButton(manipulatorController, UP_D_PAD) - .onTrue(new ArmToPos(arm, CLIMB_POS)); - new POVButton(manipulatorController, DOWN_D_PAD).onTrue(new Climb(arm)); - new POVButton(manipulatorController, LEFT_D_PAD) - .onTrue(new ArmToPos(arm, PODIUM_ANGLE_RAD)); - - } - - /** - * Flips an axis' Y coordinates upside down, but only if the select axis is a - * joystick axis - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double getStickValue(GenericHID hid, Axis axis) { - return hid.getRawAxis(axis.value) - * (axis == Axis.kLeftY || axis == Axis.kRightY ? -1 : 1); - } - - /** - * Processes an input from the joystick into a value between -1 and 1, - * sinusoidally instead of - * linearly - * - * @param value The value to be processed. - * @return The processed value. - */ - private double inputProcessing(double value) { - double processedInput; - // processedInput = - // (((1-Math.cos(value*Math.PI))/2)*((1-Math.cos(value*Math.PI))/2))*(value/Math.abs(value)); - processedInput = Math.copySign(((1 - Math.cos(value * Math.PI)) / 2) - * ((1 - Math.cos(value * Math.PI)) / 2), value); - return processedInput; - } - - /** - * Combines both getStickValue and inputProcessing into a single function for - * processing joystick - * outputs - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double ProcessedAxisValue(GenericHID hid, Axis axis) { - return DeadzonedAxis(inputProcessing(getStickValue(hid, axis))); - } - - /** - * Returns zero if a axis input is inside the deadzone - * - * @param hid The controller/plane joystick the axis is on - * @param axis The processed axis - * @return The processed value. - */ - private double DeadzonedAxis(double axOut) { - return (Math.abs(axOut) <= OI.JOY_THRESH) ? 0.0 : axOut; - } - - /** - * Returns a new instance of Trigger based on the given Joystick and Axis - * objects. The Trigger is - * triggered when the absolute value of the stick value on the specified axis - * exceeds a minimum - * threshold value. - * - * @param stick The Joystick object to retrieve stick value from. - * @param axis The Axis object to retrieve value from the Joystick. - * @return A new instance of Trigger based on the given Joystick and Axis - * objects. * @throws - * NullPointerException if either stick or axis is null. - */ - private Trigger axisTrigger(GenericHID controller, Axis axis) { - return new Trigger(() -> Math - .abs(getStickValue(controller, axis)) > OI.MIN_AXIS_TRIGGER_VALUE); - } - - private void registerAutoCommands() { - //// AUTO-USABLE COMMANDS - NamedCommands.registerCommand("Intake", new Intake(intakeShooter)); - NamedCommands.registerCommand("Eject", new Eject(intakeShooter)); - - // NamedCommands.registerCommand("ArmToSpeaker", new MoveToPos(arm, - // Armc.SPEAKER_ANGLE_RAD, 0)); - NamedCommands.registerCommand("ArmToAmp", - new ArmToPos(arm, Armc.AMP_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToSubwoofer", - new ArmToPos(arm, Armc.SUBWOOFER_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToPodium", - new ArmToPos(arm, Armc.PODIUM_ANGLE_RAD)); - NamedCommands.registerCommand("ArmToGround", - new ArmToPos(arm, GROUND_INTAKE_POS)); - - NamedCommands.registerCommand("RampRPMAuton", - new RampRPMAuton(intakeShooter)); - - NamedCommands.registerCommand("SwitchRPMShoot", - new Outtake(intakeShooter, arm)); - - // NamedCommands.registerCommand("AutonRuinerShoot", new - // AutonRuinerShoot(intakeShooter)); - // NamedCommands.registerCommand("IntakeAutonRuiner", new - // IntakeAutonRuiner(intakeShooter)); - - NamedCommands.registerCommand("AutonRuinerShootAndIntake", - new AutonRuinerShootAndIntake(intakeShooter, arm)); - - NamedCommands.registerCommand("PassToOuttake", - new PassToOuttake(intakeShooter)); - NamedCommands.registerCommand("AimArmSpeakerMT2", - new AimArmSpeaker(arm, limelight)); - NamedCommands.registerCommand("AlignToAprilTagMegaTag2", - new AlignToApriltag(drivetrain, limelight, 0.0)); - NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( - new ParallelDeadlineGroup( - new WaitCommand(3.0), - new SequentialCommandGroup( - // TODO: Use Align To Drivetrain - // new AlignDrivetrain(drivetrain), - new ParallelCommandGroup( - new AlignToApriltag(drivetrain, limelight, 0.0), - new AimArmSpeaker(arm, limelight), - new RampRPMAuton(intakeShooter)), - new PassToOuttake(intakeShooter), - new ArmToPos(arm, GROUND_INTAKE_POS))))); - NamedCommands.registerCommand("ShootSubwoofer", - new SequentialCommandGroup(new ParallelCommandGroup( - new ArmToPos(arm, - Armc.SUBWOOFER_ANGLE_RAD), - new RampRPMAuton(intakeShooter)), - new PassToOuttake(intakeShooter), - new ArmToPos(arm, GROUND_INTAKE_POS))); - NamedCommands.registerCommand("Limelight Intake CCW", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 1))); - NamedCommands.registerCommand("Limelight Intake CW", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, -1))); - - NamedCommands.registerCommand("Limelight Intake Straight", - new ParallelCommandGroup(new Intake(intakeShooter), - new AutoMATICALLYGetNote(drivetrain, limelight, - intakeShooter, 0))); - - NamedCommands.registerCommand("StopIntake", - new InstantCommand(intakeShooter::stopIntake)); - NamedCommands.registerCommand("StopOutake", - new InstantCommand(intakeShooter::stopOuttake)); - NamedCommands.registerCommand("StopBoth", - new ParallelCommandGroup(new InstantCommand(intakeShooter::stopIntake), - new InstantCommand(intakeShooter::stopOuttake))); - } - - private void setupAutos() { - //// CREATING PATHS from files - if (!hasSetupAutos) { - autoCommands=new ArrayList();//clear old/nonexistent autos - - for (int i = 0; i < autoNames.length; i++) { - String name = autoNames[i]; - - autoCommands.add(new PathPlannerAuto(name)); - - /* - * // Charles' opinion: we shouldn't have it path find to the starting pose at the start of match - * new SequentialCommandGroup( - * AutoBuilder.pathfindToPose( - * PathPlannerAuto.getStaringPoseFromAutoFile(name), - * PathPlannerAuto.getPathGroupFromAutoFile(name).get(0). - * getPreviewStartingHolonomicPose(), - * Autoc.pathConstraints), - * new PathPlannerAuto(name)); - */ - } - hasSetupAutos = true; - - // NOTHING - autoCommands.add(0, new PrintCommand("Running NULL Auto!")); - // RAW FORWARD command - autoCommands.add(1, new SequentialCommandGroup( - new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), new WaitCommand(0.5), - new LastResortAuto(drivetrain))); - // dumb PP forward command - autoCommands.add(2, new PrintCommand("PPSimpleAuto not Configured!")); - } - // force regeneration each auto call - autoCommands.set(2, constructPPSimpleAuto());//overwrite this slot each time auto runs - } - - public Command constructPPSimpleAuto() { - /** - * PATHPLANNER SETTINGS Robot Width (m): .91 Robot Length(m): .94 Max Module Spd - * (m/s): 4.30 - * Default Constraints Max Vel: 1.54, Max Accel: 6.86 Max Angvel: 360, Max - * AngAccel: 360 - * (guesses!) - */ - // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER - Pose2d currPos = drivetrain.getPose(); - - //FIXME running red PP file autos seems to break something, so the robot drivetrain drives in the wrong direction. - //running blue PP autos is fine though - //Note: alliance detection and path generation work correctly! - //Solution: Redeploy after auto. - Pose2d endPos = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue) - ? currPos.transformBy(new Transform2d(1, 0, new Rotation2d(0))) - : currPos.transformBy(new Transform2d(-1, 0, new Rotation2d(0))); - - List bezierPoints = PathPlannerPath.bezierFromPoses(currPos, endPos); - - // Create the path using the bezier points created above, /* m/s, m/s^2, rad/s, rad/s^2 */ - PathPlannerPath path = new PathPlannerPath(bezierPoints, - Autoc.pathConstraints, new GoalEndState(0, currPos.getRotation())); - - path.preventFlipping = false;// don't flip, we do that manually already. - - return new SequentialCommandGroup( - new InstantCommand(()->drivetrain.drive(-.0001, 0, 0)),//align drivetrain wheels. - AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); - } - - public Command getAutonomousCommand() { - setupAutos(); - - Integer autoIndex = autoSelector.getSelected(); - - if (autoIndex != null && autoIndex != 0) { - new PrintCommand("Running selected auto: " + autoSelector.toString()); - return autoCommands.get(autoIndex.intValue()); - } - return new PrintCommand("No auto :("); - } - -} diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 8c7f011..6390ffc 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -15,7 +15,10 @@ import java.util.function.Consumer; import org.carlmontrobotics.Constants; -import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; +import static org.carlmontrobotics.Constants.Elevatorc.*; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; @@ -54,7 +57,7 @@ public class Elevator extends SubsystemBase { private SparkMaxConfig followerConfig; private RelativeEncoder followerEncoder; // Limit Switches - private DigitalInput topLimitSwitch; + // private DigitalInput topLimitSwitch; no upper limit switch private DigitalInput bottomLimitSwitch; //Vars private double heightGoal; @@ -85,24 +88,26 @@ public class Elevator extends SubsystemBase { public Elevator() { //motors - masterMotor = new SparkMax(Constants.Elevatorc.masterPort, MotorType.kBrushless); - masterConfig = new SparkMaxConfig(); + // masterMotor = new SparkMax(masterPort, MotorType.kBrushless); + masterMotor = MotorControllerFactory.createSparkMax(masterPort, MotorConfig.NEO); masterEncoder = masterMotor.getEncoder(); - followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); - followerConfig = new SparkMaxConfig(); + + // followerMotor = new SparkMax(Constants.Elevatorc.followerPort, MotorType.kBrushless); + followerMotor = MotorControllerFactory.createSparkMax(followerPort, MotorConfig.NEO); followerEncoder = followerMotor.getEncoder(); + configureMotors(); //Calibration - topLimitSwitch = new DigitalInput(Constants.Elevatorc.elevatorTopLimitSwitchPort); - bottomLimitSwitch = new DigitalInput(Constants.Elevatorc.elevatorBottomLimitSwitchPort); + // topLimitSwitch = new DigitalInput(elevatorTopLimitSwitchPort); + bottomLimitSwitch = new DigitalInput(elevatorBottomLimitSwitchPort); timer = new Timer(); timer.start(); //PID - pidElevatorController = new PIDController(Constants.Elevatorc.kP, Constants.Elevatorc.kI, Constants.Elevatorc.kD); + pidElevatorController = new PIDController(kP, kI, kD); //FeedForward - feedforwardElevatorController = new ElevatorFeedforward(Constants.Elevatorc.kS, Constants.Elevatorc.kG, Constants.Elevatorc.kV, Constants.Elevatorc.kA); + feedforwardElevatorController = new ElevatorFeedforward(kS, kG, kV, kA); sysIdRoutine = new SysIdRoutine( @@ -133,20 +138,20 @@ public Elevator() { private void configureMotors () { //Master Config masterConfig - .inverted(Constants.Elevatorc.masterInverted) - .idleMode(Constants.Elevatorc.masterIdleMode); + .inverted(masterInverted) + .idleMode(masterIdleMode); masterConfig.encoder - .positionConversionFactor(Constants.Elevatorc.masterPositionConversionFactor) - .velocityConversionFactor(Constants.Elevatorc.masterVelocityConversionFactor); + .positionConversionFactor(masterPositionConversionFactor) + .velocityConversionFactor(masterVelocityConversionFactor); masterConfig.closedLoop - .pid(Constants.Elevatorc.kP, Constants.Elevatorc.kI,Constants.Elevatorc.kD) + .pid(kP, kI, kD) .feedbackSensor(FeedbackSensor.kPrimaryEncoder); - masterMotor.configure(masterConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + masterMotor.configure(masterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); //I don't know if this is needed. Response: Not rly. Only the follow. //Follower Config followerConfig.apply(masterConfig); - followerConfig.follow(Constants.Elevatorc.masterPort, Constants.Elevatorc.followerInverted); - followerMotor.configure(followerConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + followerConfig.follow(masterPort, followerInverted); + followerMotor.configure(followerConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public void setGoal(double goal) { @@ -165,29 +170,29 @@ public double getCurrentHeight() { return masterEncoder.getPosition(); } - public boolean elevatorAtMax() { - return !topLimitSwitch.get(); - } + // public boolean elevatorAtMax() { + // return !topLimitSwitch.get(); + // } public boolean elevatorAtMin() { return !bottomLimitSwitch.get(); } public void updateEncoders() { - if (elevatorAtMax()) { - masterEncoder.setPosition(Constants.Elevatorc.maxElevatorHeightInches); - timer.reset(); - timer.start(); - } - else if (elevatorAtMin()) { - masterEncoder.setPosition(Constants.Elevatorc.minElevatorHeightInches); + // if (elevatorAtMax()) { + // masterEncoder.setPosition(maxElevatorHeightInches); + // timer.reset(); + // timer.start(); + // } + if (elevatorAtMin()) { + masterEncoder.setPosition(minElevatorHeightInches); timer.reset(); timer.start(); } } public void getToGoal() { - if(!elevatorAtMax() || heightGoal Date: Sun, 9 Mar 2025 15:58:20 -0700 Subject: [PATCH 143/153] fixed constants (hopefully) --- .../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 088629c..b45a990 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -320,7 +320,7 @@ public static final class Elevatorc { public static final int followerPort = 21; // inverted public static final int elevatorTopLimitSwitchPort = 1; public static final int elevatorBottomLimitSwitchPort = 2; - public static final double GEAR_RATIO = 1/20; //TODO: CHANGE TO ACTUAL GEAR RATIO + public static final double GEAR_RATIO = 1.0/20; //TODO: CHANGE TO ACTUAL GEAR RATIO // Config // TODO figure these parts out @@ -329,8 +329,8 @@ public static final class Elevatorc { public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = false; public static final boolean followerInverted = true; - public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)/20); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 - public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)/20); + public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 + public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)*1.0/60); public static final double maxElevatorHeightInches = 53.2; public static final double minElevatorHeightInches = 0; @@ -346,13 +346,13 @@ public static final class Elevatorc { //Positions public static final double downPos = 0; public static final double l1 = 0; - public static final double l2 = 6.5; //inches - public static final double l3 = 22.5; //inches - public static final double l4 = 52.64; - public static final double net = 53.2; + public static final double l2 = 6.5-1.236220; //inches + public static final double l3 = 22.5-1.236220; //inches + public static final double l4 = 52.64-1.236220; + public static final double net = 53.2-1.236220; public static final double processor = 0; - public static final double bottomAlgaeRemoval = 22.5; - public static final double uppperAlgaeRemoval = 38.35; + public static final double bottomAlgaeRemoval = 22.5-1.236220; + public static final double uppperAlgaeRemoval = 38.35-1.236220; //ScoreENUM public enum ElevatorPos { DOWN(downPos), From 59079c8e4a485938b428dbad52135eb921487745 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Sun, 9 Mar 2025 16:48:21 -0700 Subject: [PATCH 144/153] updated sysid code by adding buttons for shuffleboard for quasistatic and dynamic tests and attempted adding height and velocity checks for sysid tests --- .../java/org/carlmontrobotics/Constants.java | 8 ++--- .../carlmontrobotics/subsystems/Elevator.java | 35 ++++++++++++++++++- 2 files changed, 37 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index b45a990..588e759 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -126,14 +126,12 @@ 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, 1.75 }; // {2.2319, 2.2462, 2.4136, 3.6862}; // {1.82/100, 1.815/100, - // 2.015/100, - // 1.915/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.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 } - : new boolean[] { false, true, false, true }); + ? new boolean[] { false, false, false, 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, diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 6390ffc..4da16bf 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -11,10 +11,15 @@ import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import java.lang.reflect.Method; import java.util.Map; import java.util.function.Consumer; import org.carlmontrobotics.Constants; +import org.carlmontrobotics.Constants.Elevatorc.ElevatorPos; + +import static org.carlmontrobotics.Config.CONFIG; +import static org.carlmontrobotics.Constants.Drivetrainc.velocityTolerance; import static org.carlmontrobotics.Constants.Elevatorc.*; import org.carlmontrobotics.lib199.MotorConfig; @@ -32,6 +37,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; @@ -40,6 +46,8 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.RobotController; 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.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -86,6 +94,15 @@ public class Elevator extends SubsystemBase { Units.Seconds.of(5)//AH: maximum sysID test time ); + private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Elevator SysID"); + + private void sysIdSetup() { + sysIdTab.add("Quasistatic backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + sysIdTab.add("Quasistatic forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dynamic forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + sysIdTab.add("Dynamic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + } + public Elevator() { //motors // masterMotor = new SparkMax(masterPort, MotorType.kBrushless); @@ -133,6 +150,11 @@ public Elevator() { }, this) ); + + if (CONFIG.isSysIdTesting()) { + sysIdSetup(); + } + } private void configureMotors () { @@ -225,14 +247,25 @@ public boolean atGoalHeight() { } +//safetyMethod is used to check during sysid if the elevator height and voltage are at the safe threshold + private boolean safetyMethod(){ + if (maxElevatorHeightInches == masterEncoder.getPosition()){ + return true; + } + //FIX THIS if (maxVelocityMetersPerSecond == masterEncoder.getVelocity()){ + return true; + } + /** * Returns a command that will execute a quasistatic test in the given direction. * * @param direction The direction (forward or reverse) to run the test in */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysIdRoutine.quasistatic(direction); + return sysIdRoutine.quasistatic(direction);//.onlyWhile(safetyMethod(false)); + //use onlyWhile to decorate the command and therefore add safety limits (for height and voltage) + //TO-DO: fix safety method (add velocity) and also other bugs } /** From ba926b7da26c8e5ba483c36abd80be35f32a7fc8 Mon Sep 17 00:00:00 2001 From: Niosocket11 <107586975+Niosocket11@users.noreply.github.com> Date: Sun, 9 Mar 2025 21:48:25 -0700 Subject: [PATCH 145/153] Error fixing --- src/main/java/org/carlmontrobotics/Constants.java | 2 +- .../org/carlmontrobotics/subsystems/Elevator.java | 11 ++++++----- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 588e759..67eaa2e 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -316,7 +316,7 @@ public static final class Elevatorc { // ports public static final int masterPort = 20; public static final int followerPort = 21; // inverted - public static final int elevatorTopLimitSwitchPort = 1; + //public static final int elevatorTopLimitSwitchPort = 1; public static final int elevatorBottomLimitSwitchPort = 2; public static final double GEAR_RATIO = 1.0/20; //TODO: CHANGE TO ACTUAL GEAR RATIO diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 4da16bf..9ed17e1 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -4,12 +4,14 @@ package org.carlmontrobotics.subsystems; - import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.MetersPerSecond; import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; + +import edu.wpi.first.math.util.Units; import java.lang.reflect.Method; import java.util.Map; @@ -38,7 +40,6 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; -import edu.wpi.first.units.Units; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutVoltage; @@ -89,9 +90,9 @@ public class Elevator extends SubsystemBase { //AH: need a config to run a test private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( - Volts.of(1).per(Units.Seconds),//ramp rate, volts/sec + Volts.of(1).per(Seconds),//ramp rate, volts/sec Volts.of(1), //starting voltage, volts - Units.Seconds.of(5)//AH: maximum sysID test time + Seconds.of(5)//AH: maximum sysID test time ); private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Elevator SysID"); @@ -249,7 +250,7 @@ public boolean atGoalHeight() { //safetyMethod is used to check during sysid if the elevator height and voltage are at the safe threshold private boolean safetyMethod(){ - if (maxElevatorHeightInches == masterEncoder.getPosition()){ + if (Units.inchesToMeters(maxElevatorHeightInches) == masterEncoder.getPosition()){ return true; } //FIX THIS if (maxVelocityMetersPerSecond == masterEncoder.getVelocity()){ From 140192ca7c48ace2ddc47bf97d1856dc9d8e0fcd Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Mon, 10 Mar 2025 17:02:32 -0700 Subject: [PATCH 146/153] added safety limits for max height and velocity of the elevator for sysid tests --- .../java/org/carlmontrobotics/Constants.java | 4 +- .../carlmontrobotics/subsystems/Elevator.java | 50 +++++++++++++++---- 2 files changed, 43 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 67eaa2e..99a28bb 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -317,7 +317,7 @@ public static final class Elevatorc { public static final int masterPort = 20; public static final int followerPort = 21; // inverted //public static final int elevatorTopLimitSwitchPort = 1; - public static final int elevatorBottomLimitSwitchPort = 2; + public static final int elevatorBottomLimitSwitchPort = 0; public static final double GEAR_RATIO = 1.0/20; //TODO: CHANGE TO ACTUAL GEAR RATIO // Config @@ -331,7 +331,7 @@ public static final class Elevatorc { public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)*1.0/60); public static final double maxElevatorHeightInches = 53.2; public static final double minElevatorHeightInches = 0; - + //PID public static final double kP = 1; public static final double kI = 0; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 9ed17e1..c1b13db 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -15,6 +15,7 @@ import java.lang.reflect.Method; import java.util.Map; +import java.util.function.BooleanSupplier; import java.util.function.Consumer; import org.carlmontrobotics.Constants; @@ -68,6 +69,7 @@ public class Elevator extends SubsystemBase { // Limit Switches // private DigitalInput topLimitSwitch; no upper limit switch private DigitalInput bottomLimitSwitch; + private double maxVelocityMetersPerSecond = 10; //Vars private double heightGoal; private int elevatorState; @@ -75,9 +77,12 @@ public class Elevator extends SubsystemBase { private PIDController pidElevatorController; private ElevatorFeedforward feedforwardElevatorController; private Timer timer; - + private Timer encoderTimer; private final SysIdRoutine sysIdRoutine; - + private double lastMeasuredTime; + private double currTime; + private double lastElevPos; + private double lastElevVel; // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. private final MutVoltage[] m_appliedVoltage = new MutVoltage[2];//AH: its a holder, not a number. //Volts.mutable(0); @@ -105,6 +110,7 @@ private void sysIdSetup() { } public Elevator() { + encoderTimer = new Timer(); //motors // masterMotor = new SparkMax(masterPort, MotorType.kBrushless); masterMotor = MotorControllerFactory.createSparkMax(masterPort, MotorConfig.NEO); @@ -247,15 +253,33 @@ public boolean atGoalHeight() { } } + //private boolean isEncoderDisconnected() { + // double currentElevPos = getPos(); + // double currentRelativeElevVel = masterEncoder.getVelocity(); + + // if ((currentRelativeElevVel != 0)) { + // lastMeasuredTime = currTime; + // lastElevPos = currentElevPos; + // lastElevVel = currentRelativeElevVel; + // return false; + // } else { + // if(lastMeasuredTime > currTime+1 && lastElevPos == currentElevPos) { + // return true; + // } + // } + // // currTime - lastMeasuredTime < + // // DISCONNECTED_ENCODER_TIMEOUT_SEC; + + + // } //safetyMethod is used to check during sysid if the elevator height and voltage are at the safe threshold - private boolean safetyMethod(){ - if (Units.inchesToMeters(maxElevatorHeightInches) == masterEncoder.getPosition()){ - return true; - } - //FIX THIS if (maxVelocityMetersPerSecond == masterEncoder.getVelocity()){ + private boolean isUNSAFE(){ + if (Units.inchesToMeters(maxElevatorHeightInches) >= masterEncoder.getPosition() || maxVelocityMetersPerSecond >= masterEncoder.getVelocity() || Units.inchesToMeters(minElevatorHeightInches) <=masterEncoder.getPosition()){ return true; } + return false; + } /** @@ -264,7 +288,8 @@ private boolean safetyMethod(){ * @param direction The direction (forward or reverse) to run the test in */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return sysIdRoutine.quasistatic(direction);//.onlyWhile(safetyMethod(false)); + // BooleanSupplier bruh = Elevator::safetyMethod(); + return sysIdRoutine.quasistatic(direction).onlyWhile((BooleanSupplier)()->isUNSAFE()); //use onlyWhile to decorate the command and therefore add safety limits (for height and voltage) //TO-DO: fix safety method (add velocity) and also other bugs } @@ -275,7 +300,7 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { * @param direction The direction (forward or reverse) to run the test in */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysIdRoutine.dynamic(direction); + return sysIdRoutine.dynamic(direction).onlyWhile((BooleanSupplier)()->isUNSAFE()); } public double getEleVel() { return masterEncoder.getVelocity(); @@ -297,5 +322,12 @@ public void periodic() { SmartDashboard.putNumber("Since Calibrated", timer.get()); updateEncoders(); getToGoal(); + if(isUNSAFE() && masterMotor.getBusVoltage() > 0) { + masterMotor.set(0); + } + // if (isEncoderDisconnected()) { + // masterMotor.set(0); + + // } } } From 121fe9bb2dc9785423c5d8fc9819ba6f0b9639f0 Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Mon, 10 Mar 2025 17:14:15 -0700 Subject: [PATCH 147/153] adjusted stuff --- .../carlmontrobotics/subsystems/Elevator.java | 76 +++++++++++-------- 1 file changed, 43 insertions(+), 33 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index c1b13db..2840af3 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -76,7 +76,7 @@ public class Elevator extends SubsystemBase { //PID private PIDController pidElevatorController; private ElevatorFeedforward feedforwardElevatorController; - private Timer timer; + // private Timer timer; private Timer encoderTimer; private final SysIdRoutine sysIdRoutine; private double lastMeasuredTime; @@ -124,8 +124,8 @@ public Elevator() { //Calibration // topLimitSwitch = new DigitalInput(elevatorTopLimitSwitchPort); bottomLimitSwitch = new DigitalInput(elevatorBottomLimitSwitchPort); - timer = new Timer(); - timer.start(); + // timer = new Timer(); + // timer.start(); //PID @@ -196,7 +196,7 @@ public double getGoal() { } public double getCurrentHeight() { - return masterEncoder.getPosition(); + return masterEncoder.getPosition();//conversion done in config/constants } // public boolean elevatorAtMax() { @@ -204,23 +204,23 @@ public double getCurrentHeight() { // } public boolean elevatorAtMin() { - return !bottomLimitSwitch.get(); + return !bottomLimitSwitch.get();//limit switches are opposite } - public void updateEncoders() { - // if (elevatorAtMax()) { - // masterEncoder.setPosition(maxElevatorHeightInches); - // timer.reset(); - // timer.start(); - // } - if (elevatorAtMin()) { - masterEncoder.setPosition(minElevatorHeightInches); - timer.reset(); - timer.start(); - } - } + // public void updateEncoders() {//what the fuck?? + // // if (elevatorAtMax()) { + // // masterEncoder.setPosition(maxElevatorHeightInches); + // // timer.reset(); + // // timer.start(); + // // } + // if (elevatorAtMin()) { + // masterEncoder.setPosition(minElevatorHeightInches); + // timer.reset(); + // timer.start(); + // } + // } - public void getToGoal() { + public void goToGoal() { if(heightGoal= masterEncoder.getPosition() || maxVelocityMetersPerSecond >= masterEncoder.getVelocity() || Units.inchesToMeters(minElevatorHeightInches) <=masterEncoder.getPosition()){ - return true; + public boolean isUNSAFE(){ + if (Units.inchesToMeters(maxElevatorHeightInches) >= masterEncoder.getPosition() + || maxVelocityMetersPerSecond <= masterEncoder.getVelocity() + || Units.inchesToMeters(minElevatorHeightInches) <=masterEncoder.getPosition()){ + return false; } - return false; + return true; + } + + public boolean isSAFE() { + return !isUNSAFE(); } @@ -289,7 +295,7 @@ private boolean isUNSAFE(){ */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { // BooleanSupplier bruh = Elevator::safetyMethod(); - return sysIdRoutine.quasistatic(direction).onlyWhile((BooleanSupplier)()->isUNSAFE()); + return sysIdRoutine.quasistatic(direction).onlyWhile((BooleanSupplier)()->isSAFE()); //use onlyWhile to decorate the command and therefore add safety limits (for height and voltage) //TO-DO: fix safety method (add velocity) and also other bugs } @@ -300,7 +306,7 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { * @param direction The direction (forward or reverse) to run the test in */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysIdRoutine.dynamic(direction).onlyWhile((BooleanSupplier)()->isUNSAFE()); + return sysIdRoutine.dynamic(direction).onlyWhile((BooleanSupplier)()->isSAFE()); } public double getEleVel() { return masterEncoder.getVelocity(); @@ -316,14 +322,18 @@ public void periodic() { SmartDashboard.putString("ElevatorState", "🟢GO🟢"); } else { - SmartDashboard.putString("ElevatorState", "🟡CAUTION🟡"); - } + SmartDashboard.putString("ElevatorState", "🟡AT MIN🟡"); + }//add one for max height + //add one for if unsafe SmartDashboard.putNumber("Elevator Height", getCurrentHeight()); - SmartDashboard.putNumber("Since Calibrated", timer.get()); - updateEncoders(); - getToGoal(); + // SmartDashboard.putNumber("Since Calibrated", timer.get()); + // updateEncoders(); + goToGoal(); + if(isUNSAFE() && masterMotor.getBusVoltage() > 0) { masterMotor.set(0); + System.err.println("Bad Bad nightmare bad. Elevator unsafe"); + //hey tell them they're unsafe and a bad happened } // if (isEncoderDisconnected()) { // masterMotor.set(0); From 28abf558021c24f14d24e4e7236a5b717d8f7b3e Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Mon, 10 Mar 2025 18:05:29 -0700 Subject: [PATCH 148/153] fixed errors and violated robot container --- .../java/org/carlmontrobotics/Config.java | 2 +- .../java/org/carlmontrobotics/Constants.java | 20 +- .../org/carlmontrobotics/RobotContainer.java | 183 +++++++++--------- .../commands/TeleopDrive.java | 6 +- .../carlmontrobotics/subsystems/Elevator.java | 7 +- 5 files changed, 111 insertions(+), 107 deletions(-) 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 diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 99a28bb..85bcacb 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -274,12 +274,12 @@ public static final class OI { public static final class Driver { public static final int driverPort = 0; - public static final int EFFECTOR_TOP_MOTOR_ID = 31; - public static final int EFFECTOR_BOTTOM_MOTOR_ID = 33; - public static final int EFFECTOR_PINCHER_MOTOR_ID = 31; - public static final int EFFECTOR_ARM_MOTOR_ID = 34; - public static final int effectorDistanceSensorID = 5; - /* + // public static final int EFFECTOR_TOP_MOTOR_ID = 31; + // public static final int EFFECTOR_BOTTOM_MOTOR_ID = 33; + // public static final int EFFECTOR_PINCHER_MOTOR_ID = 31; + // public static final int EFFECTOR_ARM_MOTOR_ID = 34; + // public static final int effectorDistanceSensorID = 5; + /* * public static final int A = 1; * public static final int B = 2; * public static final int X = 3; @@ -383,10 +383,10 @@ public static final class AlgaeEffectorc { // EFFECTOR - public static final int UPPER_MOTOR_PORT = 1; - public static final int LOWER_MOTOR_PORT = 2; - public static final int PINCH_MOTOR_PORT = 3; - public static final int ARM_MOTOR_PORT = 4; + public static final int UPPER_MOTOR_PORT = 32; + public static final int LOWER_MOTOR_PORT =33; + public static final int PINCH_MOTOR_PORT = 31; + public static final int ARM_MOTOR_PORT = 34; public static final int aChannelEnc = 0; public static final int bChannelEnc = 1; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index 78483e6..c672745 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -55,7 +55,7 @@ public class RobotContainer { public final GenericHID driverController = new GenericHID(OI.Driver.driverPort); public final GenericHID manipulatorController = new GenericHID(Manipulator.manipulatorPort); - private final Drivetrain drivetrain = new Drivetrain(); + //private final Drivetrain drivetrain = new 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! */ @@ -65,43 +65,43 @@ public class RobotContainer { // 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 List autoCommands; + // private SendableChooser autoSelector = new SendableChooser(); - private boolean hasSetupAutos = false; - private final String[] autoNames = new String[] {}; - private final AlgaeEffector algaeEffector = new AlgaeEffector(); - private final Elevator elevator = new Elevator(); + // private boolean hasSetupAutos = false; + // private final String[] autoNames = new String[] {}; + // private final AlgaeEffector algaeEffector = new AlgaeEffector(); + private final Elevator elevator = new Elevator(); 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()); - - 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); + // SmartDashboard.putData("CONFIG overrides", Config.CONFIG); + // SmartDashboard.putData(drivetrain); + // 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(); @@ -110,11 +110,11 @@ 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(OI.Driver.slowDriveButton), elevator)); elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); } @@ -136,27 +136,27 @@ private void setBindingsDriver() { private void setBindingsManipulator() { // intake - new JoystickButton(manipulatorController, INTAKE_BUMPER) - .whileTrue(new SequentialCommandGroup( - new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), - new GroundIntakeAlgae(algaeEffector))); + // new JoystickButton(manipulatorController, INTAKE_BUMPER) + // .whileTrue(new SequentialCommandGroup( + // new ArmToPosition(algaeEffector, ARM_INTAKE_ANGLE), + // new GroundIntakeAlgae(algaeEffector))); // outake // dealgify - new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) - .whileFalse(new OuttakeAlgae(algaeEffector)); + // new JoystickButton(manipulatorController, OI.Manipulator.OuttakeBumper) + // .whileFalse(new OuttakeAlgae(algaeEffector)); - new JoystickButton(manipulatorController, XboxController.Button.kA.value) - .onTrue(new DealgaficationIntake(algaeEffector)); + // new JoystickButton(manipulatorController, XboxController.Button.kA.value) + // .onTrue(new DealgaficationIntake(algaeEffector)); - new JoystickButton(manipulatorController, XboxController.Button.kB.value) - .onTrue(new ShootAlgae(algaeEffector)); - new JoystickButton(manipulatorController, XboxController.Button.kY.value) - .onTrue(new InstantCommand(() -> { - algaeEffector.stopMotors(); - })); + // new JoystickButton(manipulatorController, XboxController.Button.kB.value) + // .onTrue(new ShootAlgae(algaeEffector)); + // new JoystickButton(manipulatorController, XboxController.Button.kY.value) + // .onTrue(new InstantCommand(() -> { + // algaeEffector.stopMotors(); + // })); } /** @@ -238,13 +238,13 @@ private void registerAutoCommands() { private void setupAutos() { //// CREATING PATHS from files - if (!hasSetupAutos) { - autoCommands = new ArrayList();// clear old/nonexistent autos + // if (!hasSetupAutos) { + // autoCommands = new ArrayList();// clear old/nonexistent autos - for (int i = 0; i < autoNames.length; i++) { - String name = autoNames[i]; + // for (int i = 0; i < autoNames.length; i++) { + // String name = autoNames[i]; - autoCommands.add(new PathPlannerAuto(name)); + // autoCommands.add(new PathPlannerAuto(name)); /* * // Charles' opinion: we shouldn't have it path find to the starting pose at @@ -257,21 +257,21 @@ private void setupAutos() { * 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 + //} + // 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() { @@ -283,40 +283,41 @@ public Command constructPPSimpleAuto() { * (guesses!) */ // default origin is on BLUE ALIANCE DRIVER RIGHT CORNER - Pose2d currPos = drivetrain.getPose(); + // 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))); + // 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.waypointsFromPoses(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, null, new GoalEndState(0, currPos.getRotation())); + // // 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, null, new GoalEndState(0, currPos.getRotation())); - path.preventFlipping = false;// don't flip, we do that manually already. + // 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))); + // return new SequentialCommandGroup( + // new InstantCommand(() -> drivetrain.drive(-.0001, 0, 0)), // align drivetrain wheels. + // AutoBuilder.followPath(path).beforeStarting(new WaitCommand(1))); + return new PrintCommand("I HAT EEEYTHI"); } public Command getAutonomousCommand() { - setupAutos(); + // setupAutos(); - Integer autoIndex = autoSelector.getSelected(); + // Integer autoIndex = autoSelector.getSelected(); - if (autoIndex != null && autoIndex != 0) { - new PrintCommand("Running selected auto: " + autoSelector.toString()); - return autoCommands.get(autoIndex.intValue()); - } + // 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 index b94a165..74877d0 100644 --- a/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/commands/TeleopDrive.java @@ -34,19 +34,21 @@ public class TeleopDrive extends Command { private boolean babyMode; Elevator elevator; AlgaeEffector arm; - private double height = elevator.getCurrentHeight(); + private double height; /** * Creates a new TeleopDrive. */ public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, - BooleanSupplier slow) { + BooleanSupplier slow, Elevator elevator) { addRequirements(this.drivetrain = drivetrain); this.fwd = fwd; this.str = str; this.rcw = rcw; this.slow = slow; + this.elevator = elevator; + this.height = elevator.getCurrentHeight(); } // Called when the command is initially scheduled. diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 2840af3..480d7a7 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -28,6 +28,7 @@ import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import com.playingwithfusion.CANVenom.BrakeCoastMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; import com.revrobotics.spark.SparkBase.PersistMode; @@ -60,11 +61,11 @@ public class Elevator extends SubsystemBase { /** Creates a new Elevator. */ //Master private SparkMax masterMotor; - private SparkMaxConfig masterConfig; + private SparkMaxConfig masterConfig = new SparkMaxConfig(); private RelativeEncoder masterEncoder; //Follower private SparkMax followerMotor; - private SparkMaxConfig followerConfig; + private SparkMaxConfig followerConfig = new SparkMaxConfig(); private RelativeEncoder followerEncoder; // Limit Switches // private DigitalInput topLimitSwitch; no upper limit switch @@ -123,7 +124,7 @@ public Elevator() { configureMotors(); //Calibration // topLimitSwitch = new DigitalInput(elevatorTopLimitSwitchPort); - bottomLimitSwitch = new DigitalInput(elevatorBottomLimitSwitchPort); + bottomLimitSwitch = new DigitalInput(elevatorBottomLimitSwitchPort); // timer = new Timer(); // timer.start(); From 605b2bf84584e8fc9d8af367720166bdea02f916 Mon Sep 17 00:00:00 2001 From: Brandon Date: Mon, 10 Mar 2025 19:26:56 -0700 Subject: [PATCH 149/153] Feedforward Values!!!!!! :)))))) --- .../java/org/carlmontrobotics/Constants.java | 14 +++--- .../org/carlmontrobotics/RobotContainer.java | 2 +- .../carlmontrobotics/subsystems/Elevator.java | 49 ++++++++++--------- 3 files changed, 35 insertions(+), 30 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 85bcacb..1f5f598 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -323,24 +323,24 @@ public static final class Elevatorc { // Config // TODO figure these parts out public static final double MAX_ACCEL_RAD_P_S = 1; - public static final IdleMode masterIdleMode = IdleMode.kBrake; + public static final IdleMode masterIdleMode = IdleMode.kBrake; //TODO: Change back to break public static final IdleMode followerIdleMode = IdleMode.kBrake; public static final boolean masterInverted = false; public static final boolean followerInverted = true; public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 public static final double masterVelocityConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)*1.0/60); - public static final double maxElevatorHeightInches = 53.2; + public static final double maxElevatorHeightInches = 52.5; public static final double minElevatorHeightInches = 0; //PID - public static final double kP = 1; + public static final double kP = 2.7859; public static final double kI = 0; public static final double kD = 0; //Feedforward - public static final double kS = 0; - public static final double kG = 0; - public static final double kV = 0; - public static final double kA = 0; + public static final double kS = 0.1447; + public static final double kG = 0.17398; + public static final double kV = 8.8598; + public static final double kA = 1.7037; //Positions public static final double downPos = 0; public static final double l1 = 0; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index c672745..b77e9b2 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -115,7 +115,7 @@ private void setDefaultCommands() { // () -> ProcessedAxisValue(driverController, Axis.kLeftX), // () -> ProcessedAxisValue(driverController, Axis.kRightX), // () -> driverController.getRawButton(OI.Driver.slowDriveButton), elevator)); - elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); + //elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); } private void setBindingsDriver() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 480d7a7..9399495 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -42,6 +42,8 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; import edu.wpi.first.units.measure.MutDistance; import edu.wpi.first.units.measure.MutLinearVelocity; import edu.wpi.first.units.measure.MutVoltage; @@ -70,7 +72,7 @@ public class Elevator extends SubsystemBase { // Limit Switches // private DigitalInput topLimitSwitch; no upper limit switch private DigitalInput bottomLimitSwitch; - private double maxVelocityMetersPerSecond = 10; + private double maxVelocityMetersPerSecond = 5; //Vars private double heightGoal; private int elevatorState; @@ -85,13 +87,13 @@ public class Elevator extends SubsystemBase { private double lastElevPos; private double lastElevVel; // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. - private final MutVoltage[] m_appliedVoltage = new MutVoltage[2];//AH: its a holder, not a number. + private final MutVoltage m_appliedVoltage = Volts.mutable(0);//AH: its a holder, not a number. //Volts.mutable(0); // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. - private final MutDistance[] m_distance = new MutDistance[2];//AH: 2 for 2 elevator motors + private final MutDistance m_distance = Meters.mutable(0);//AH: 2 for 2 elevator motors //Meters.mutable(0); // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. - private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[2];//AH: ITS A HOLDER :o + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0);//AH: ITS A HOLDER :o //MetersPerSecond.mutable(0); //AH: need a config to run a test @@ -111,6 +113,7 @@ private void sysIdSetup() { } public Elevator() { + encoderTimer = new Timer(); //motors // masterMotor = new SparkMax(masterPort, MotorType.kBrushless); @@ -144,18 +147,11 @@ public Elevator() { log -> { log.motor("Elevator-Mastr")//AH: you have 2 motors, must log both .voltage( - m_appliedVoltage[0] + m_appliedVoltage .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts))//AH: GET() IS NOT VOLTAGE - .linearPosition(m_distance[0].mut_replace(masterEncoder.getPosition(), Meters)) - .linearVelocity(m_velocity[0].mut_replace(masterEncoder.getVelocity(), MetersPerSecond));//AH: use metric units always - - log.motor("Elevator-Follwr")//AH: you have 2 motors, must log both - .voltage( - m_appliedVoltage[1] - .mut_replace(followerMotor.getBusVoltage() * followerMotor.getAppliedOutput(), Volts))//AH: GET() IS NOT VOLTAGE - .linearPosition(m_distance[1].mut_replace(followerEncoder.getPosition(), Meters)) - .linearVelocity(m_velocity[1].mut_replace(followerEncoder.getVelocity(), MetersPerSecond)); - }, + .linearPosition(m_distance.mut_replace(masterEncoder.getPosition(), Meters)) + .linearVelocity(m_velocity.mut_replace(masterEncoder.getVelocity(), MetersPerSecond));//AH: use metric units always + }, this) ); @@ -276,9 +272,9 @@ public boolean atGoalHeight() { //safetyMethod is used to check during sysid if the elevator height and voltage are at the safe threshold public boolean isUNSAFE(){ - if (Units.inchesToMeters(maxElevatorHeightInches) >= masterEncoder.getPosition() + if (1.33>= masterEncoder.getPosition() || maxVelocityMetersPerSecond <= masterEncoder.getVelocity() - || Units.inchesToMeters(minElevatorHeightInches) <=masterEncoder.getPosition()){ + || 0 <=masterEncoder.getPosition()){ return false; } return true; @@ -288,7 +284,13 @@ public boolean isSAFE() { return !isUNSAFE(); } - + public boolean isBruh() { + if(getCurrentHeight()>1.33 || getCurrentHeight() < 0) { + return false; + + } + return true; + } /** * Returns a command that will execute a quasistatic test in the given direction. * @@ -296,7 +298,7 @@ public boolean isSAFE() { */ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { // BooleanSupplier bruh = Elevator::safetyMethod(); - return sysIdRoutine.quasistatic(direction).onlyWhile((BooleanSupplier)()->isSAFE()); + return sysIdRoutine.quasistatic(direction).onlyWhile((BooleanSupplier)()->isBruh()); //use onlyWhile to decorate the command and therefore add safety limits (for height and voltage) //TO-DO: fix safety method (add velocity) and also other bugs } @@ -307,7 +309,7 @@ public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { * @param direction The direction (forward or reverse) to run the test in */ public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return sysIdRoutine.dynamic(direction).onlyWhile((BooleanSupplier)()->isSAFE()); + return sysIdRoutine.dynamic(direction).onlyWhile((BooleanSupplier)()->isBruh()); } public double getEleVel() { return masterEncoder.getVelocity(); @@ -316,9 +318,12 @@ public double getEleVel() { @Override public void periodic() { + // if (elevatorAtMax()){ // SmartDashboard.putString("ElevatorState", "🔴STOP🔴"); // } + //masterMotor.set(0); + SmartDashboard.putBoolean("SAFE?", isBruh()); if (elevatorAtMin()) { SmartDashboard.putString("ElevatorState", "🟢GO🟢"); } @@ -329,10 +334,10 @@ public void periodic() { SmartDashboard.putNumber("Elevator Height", getCurrentHeight()); // SmartDashboard.putNumber("Since Calibrated", timer.get()); // updateEncoders(); - goToGoal(); + // goToGoal(); if(isUNSAFE() && masterMotor.getBusVoltage() > 0) { - masterMotor.set(0); + //masterMotor.set(0); System.err.println("Bad Bad nightmare bad. Elevator unsafe"); //hey tell them they're unsafe and a bad happened } From dcc0e4efa57af55ba31cfb2979be48250c77121c Mon Sep 17 00:00:00 2001 From: Brandon Date: Mon, 10 Mar 2025 19:44:36 -0700 Subject: [PATCH 150/153] daniel look at this pls --- src/main/java/org/carlmontrobotics/Constants.java | 2 +- .../java/org/carlmontrobotics/RobotContainer.java | 2 +- .../org/carlmontrobotics/subsystems/Elevator.java | 11 +++++++++-- 3 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 1f5f598..24a8990 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -334,7 +334,7 @@ public static final class Elevatorc { //PID public static final double kP = 2.7859; - public static final double kI = 0; + public static final double kI = 0.4; public static final double kD = 0; //Feedforward public static final double kS = 0.1447; diff --git a/src/main/java/org/carlmontrobotics/RobotContainer.java b/src/main/java/org/carlmontrobotics/RobotContainer.java index b77e9b2..c672745 100644 --- a/src/main/java/org/carlmontrobotics/RobotContainer.java +++ b/src/main/java/org/carlmontrobotics/RobotContainer.java @@ -115,7 +115,7 @@ private void setDefaultCommands() { // () -> ProcessedAxisValue(driverController, Axis.kLeftX), // () -> ProcessedAxisValue(driverController, Axis.kRightX), // () -> driverController.getRawButton(OI.Driver.slowDriveButton), elevator)); - //elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); + elevator.setDefaultCommand(new TeleopElevator(elevator, () -> ProcessedAxisValue(manipulatorController, Axis.kLeftY))); } private void setBindingsDriver() { diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 9399495..0380bbb 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -28,6 +28,7 @@ import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import com.pathplanner.lib.path.GoalEndState; import com.playingwithfusion.CANVenom.BrakeCoastMode; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase.ControlType; @@ -94,6 +95,7 @@ public class Elevator extends SubsystemBase { //Meters.mutable(0); // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0);//AH: ITS A HOLDER :o + private double goalHeight; //MetersPerSecond.mutable(0); //AH: need a config to run a test @@ -115,6 +117,7 @@ private void sysIdSetup() { public Elevator() { encoderTimer = new Timer(); + SmartDashboard.putNumber("Goal", goalHeight); //motors // masterMotor = new SparkMax(masterPort, MotorType.kBrushless); masterMotor = MotorControllerFactory.createSparkMax(masterPort, MotorConfig.NEO); @@ -322,8 +325,12 @@ public void periodic() { // if (elevatorAtMax()){ // SmartDashboard.putString("ElevatorState", "🔴STOP🔴"); // } - //masterMotor.set(0); + // //masterMotor.set(0); + // goalHeight = SmartDashboard.getNumber("Goal", 0); + // System.out.println(goalHeight); + setGoal(.2); SmartDashboard.putBoolean("SAFE?", isBruh()); + if (elevatorAtMin()) { SmartDashboard.putString("ElevatorState", "🟢GO🟢"); } @@ -334,7 +341,7 @@ public void periodic() { SmartDashboard.putNumber("Elevator Height", getCurrentHeight()); // SmartDashboard.putNumber("Since Calibrated", timer.get()); // updateEncoders(); - // goToGoal(); + goToGoal(); if(isUNSAFE() && masterMotor.getBusVoltage() > 0) { //masterMotor.set(0); From 8f5837f81b498bfdedff4a5b12977a11822d14df Mon Sep 17 00:00:00 2001 From: Brandon Date: Mon, 10 Mar 2025 20:44:39 -0700 Subject: [PATCH 151/153] cooketh thy 199 - daniel --- build.gradle | 2 +- .../java/org/carlmontrobotics/Constants.java | 8 ++++---- .../carlmontrobotics/subsystems/Elevator.java | 18 +++++++++++------- 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/build.gradle b/build.gradle index fa2d511..2482ad8 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 24a8990..089b1a0 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -323,8 +323,8 @@ public static final class Elevatorc { // Config // TODO figure these parts out public static final double MAX_ACCEL_RAD_P_S = 1; - public static final IdleMode masterIdleMode = IdleMode.kBrake; //TODO: Change back to break - public static final IdleMode followerIdleMode = IdleMode.kBrake; + public static final IdleMode masterIdleMode = IdleMode.kCoast; //TODO: Change back to break + public static final IdleMode followerIdleMode = IdleMode.kCoast; public static final boolean masterInverted = false; public static final boolean followerInverted = true; public static final double masterPositionConversionFactor = Units.inchesToMeters(2*GEAR_RATIO*(Math.PI * 1.76)); // 2*(gear_ratio*(pi*sprocket_pitch_diameter)) aka 2*1/20*pi*1.76 @@ -338,8 +338,8 @@ public static final class Elevatorc { public static final double kD = 0; //Feedforward public static final double kS = 0.1447; - public static final double kG = 0.17398; - public static final double kV = 8.8598; + public static final double kG = 0.17398+0.05; + public static final double kV = 8.8598+2; public static final double kA = 1.7037; //Positions public static final double downPos = 0; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 0380bbb..6e8edf5 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -42,6 +42,7 @@ import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.estimator.KalmanFilterLatencyCompensator; import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint; import edu.wpi.first.units.measure.Distance; import edu.wpi.first.units.measure.LinearVelocity; @@ -221,11 +222,13 @@ public boolean elevatorAtMin() { // } public void goToGoal() { - if(heightGoal 0) { + if(isBruh() && masterMotor.getBusVoltage() > 0) { //masterMotor.set(0); System.err.println("Bad Bad nightmare bad. Elevator unsafe"); //hey tell them they're unsafe and a bad happened From 2bd4dc26206d29fa62a7e9cf101a49c2a3102fa8 Mon Sep 17 00:00:00 2001 From: Brandon Date: Tue, 11 Mar 2025 09:50:31 -0700 Subject: [PATCH 152/153] maybe goToGoal Fix --- .../org/carlmontrobotics/subsystems/Elevator.java | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 6e8edf5..9adc721 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -222,13 +222,12 @@ public boolean elevatorAtMin() { // } public void goToGoal() { - System.out.println("GOing to GOAL"); - System.out.println(heightGoal); - double voltage = pidElevatorController.calculate(masterEncoder.getVelocity()) + - feedforwardElevatorController.calculate(masterEncoder.getVelocity()); - System.out.println(voltage); - masterMotor.setVoltage(voltage); - + System.out.println("GOing to GOAL"); + System.out.println(heightGoal); + double vel = pidElevatorController.calculate(masterEncoder.getPosition(), heightGoal); + double feed = feedforwardElevatorController.calculate(vel); + masterMotor.setVoltage(vel + feed); + } public void setSpeed(double speed){ From 6b1bc8245b045a6066cc3625ba06f4ce746f1f0a Mon Sep 17 00:00:00 2001 From: DriverStationComputer <35879629+DriverStationComputer@users.noreply.github.com> Date: Tue, 11 Mar 2025 15:44:37 -0700 Subject: [PATCH 153/153] Good Constants, elevator moves to goal consistently with no oscillations --- src/main/java/org/carlmontrobotics/Constants.java | 14 +++++++------- .../org/carlmontrobotics/subsystems/Elevator.java | 4 ++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/Constants.java b/src/main/java/org/carlmontrobotics/Constants.java index 089b1a0..107f9a6 100644 --- a/src/main/java/org/carlmontrobotics/Constants.java +++ b/src/main/java/org/carlmontrobotics/Constants.java @@ -333,14 +333,14 @@ public static final class Elevatorc { public static final double minElevatorHeightInches = 0; //PID - public static final double kP = 2.7859; - public static final double kI = 0.4; - public static final double kD = 0; + public static final double kP = 40;//45.476; + public static final double kI = 0; + public static final double kD = 10.609; //Feedforward - public static final double kS = 0.1447; - public static final double kG = 0.17398+0.05; - public static final double kV = 8.8598+2; - public static final double kA = 1.7037; + public static final double kS = 0.12666; + public static final double kG = 0.177; + public static final double kV = 8.9921; + public static final double kA = 1.4586; //Positions public static final double downPos = 0; public static final double l1 = 0; diff --git a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java index 9adc721..2b779c6 100644 --- a/src/main/java/org/carlmontrobotics/subsystems/Elevator.java +++ b/src/main/java/org/carlmontrobotics/subsystems/Elevator.java @@ -225,7 +225,7 @@ public void goToGoal() { System.out.println("GOing to GOAL"); System.out.println(heightGoal); double vel = pidElevatorController.calculate(masterEncoder.getPosition(), heightGoal); - double feed = feedforwardElevatorController.calculate(vel); + double feed = feedforwardElevatorController.calculate(0); masterMotor.setVoltage(vel + feed); } @@ -330,7 +330,7 @@ public void periodic() { // //masterMotor.set(0); // goalHeight = SmartDashboard.getNumber("Goal", 0); // System.out.println(goalHeight); - setGoal(1); + setGoal(.75); SmartDashboard.putBoolean("SAFE?", isBruh()); if (elevatorAtMin()) {