diff --git a/build.gradle b/build.gradle index 1945af5..9b32861 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" } java { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 0ddb192..2045a79 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -63,6 +63,7 @@ public static final class CanID { /** * Current limits - measure in Amps + * TODO: Would be nice to look at practice data, or match data, to corroborate these current limits */ public static final class CurrentLimit { public static final class SparkMax { @@ -73,7 +74,7 @@ public static final class SparkMax { } public static final class Neo { - public static final double FREE_SPEED_RPMS = 5676.0; + public static final double FREE_SPEED_RPMS = 5676.0; //TODO: Move this outside of the CurrentLimit class public static final int SMART = 60; public static final int SECONDARY = 80; } @@ -85,16 +86,16 @@ public static final class Neo { public static final class Kinematics { /* Robot mass in Kg. */ - public static final double MASS = Units.lbsToKilograms(106.0); //Note: this weight includes the battery (but no bumpers yet) + public static final double MASS = Units.lbsToKilograms(106.0); //Note: this weight includes the battery (but no bumpers yet) //TODO: update /* Robot frame width in meters */ - public static final double WIDTH = Units.inchesToMeters(28.125); + public static final double WIDTH = Units.inchesToMeters(28.125);//TODO: update /* Robot width in meters WITH bumpers on */ public static final double WIDTH_WITH_BUMPERS = Units.inchesToMeters(24.0); //TODO: update /* Robot frame length in meters */ - public static final double LENGTH = Units.inchesToMeters(28.125); + public static final double LENGTH = Units.inchesToMeters(28.125); //TODO: update /* Robot length in meters WITH bumpers on */ public static final double LENGTH_WITH_BUMPERS = Units.inchesToMeters(29.5); //TODO: update @@ -113,24 +114,29 @@ public static final class Kinematics { *

* Should be measured from center to center */ - public static final double DRIVETRAIN_TRACKWIDTH_METERS = Units.inchesToMeters(22.875); + public static final double DRIVETRAIN_TRACKWIDTH_METERS = Units.inchesToMeters(22.875); //TODO: update /** * The front-to-back distance between the drivetrain wheels *

* Should be measured from center to center */ - public static final double DRIVETRAIN_WHEELBASE_METERS = Units.inchesToMeters(22.875); + public static final double DRIVETRAIN_WHEELBASE_METERS = Units.inchesToMeters(22.875); //TODO: update /** Distance from center of robot to center of swerve module **/ public static final double DRIVETRAIN_BASE_RADIUS = Math.hypot(DRIVETRAIN_TRACKWIDTH_METERS, DRIVETRAIN_WHEELBASE_METERS) / 2.0; //SDS Mk4i L3 gear ratios - equivalent to 6.12:1 overall ratio - public static final double DRIVE_GEAR_RATIO = (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0); //Note: (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0) == 1.0/6.12 = 0.1634 + public static final double DRIVE_GEAR_RATIO = (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0); //Note: (14.0 / 50.0) * (28.0 / 16.0) * (15.0 / 45.0) = 1.0/6.12 = 0.1634 public static final double STEER_GEAR_RATIO = 1.0 / (150.0 / 7.0); //150/7:1 gear ratio //TODO: Figure out why true is needed for correct wheel direction, when in theory it should be false + // Matt: I suspect it has to do with the location the motors are mounted. For + // example, if the drive motor is mounted on the "left" on the MK4I, but the + // "right" on the MK4, the inversion could manifset like this. Or, if one has an + // extra idler gear in it. I'll take a look at the module CAD and see if its + // clear. public static final boolean DRIVE_MOTOR_INVERTED = true; //should be false for Mk4i, true for Mk4 (tested on Eclipse robot) public static final boolean STEER_MOTOR_INVERTED = true; //should be true for Mk4i, false for Mk4 (tested on Eclipse robot) @@ -176,7 +182,7 @@ public static final class Kinematics { public static final double COLLISION_THRESHOLD_DELTA_G = 0.5; /* Pose estimate should not be reset until after this long after collision */ public static final long MICROSECONDS_SINCE_COLLISION_THRESHOLD = 250000; //0.25 seconds - + //TODO: Do we still need to tune these values? public static final Matrix WHEEL_ODOMETRY_POSE_STANDARD_DEVIATIONS = VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)); } @@ -187,6 +193,7 @@ public static final class Elevator { public static final double kElevatorKi = 0; public static final double kElevatorKd = 0; + // TODO: Elevator has changed slightly, confirm values are still accurate. //Feedforward values obtained via sysId: //https://www.reca.lc/linear?angle=%7B%22s%22%3A90%2C%22u%22%3A%22deg%22%7D¤tLimit=%7B%22s%22%3A38%2C%22u%22%3A%22A%22%7D&efficiency=90&limitAcceleration=0&limitDeceleration=0&limitVelocity=0&limitedAcceleration=%7B%22s%22%3A400%2C%22u%22%3A%22in%2Fs2%22%7D&limitedDeceleration=%7B%22s%22%3A50%2C%22u%22%3A%22in%2Fs2%22%7D&limitedVelocity=%7B%22s%22%3A10%2C%22u%22%3A%22in%2Fs%22%7D&load=%7B%22s%22%3A20%2C%22u%22%3A%22lbs%22%7D&motor=%7B%22quantity%22%3A2%2C%22name%22%3A%22NEO%22%7D&ratio=%7B%22magnitude%22%3A16%2C%22ratioType%22%3A%22Reduction%22%7D&spoolDiameter=%7B%22s%22%3A57.3%2C%22u%22%3A%22mm%22%7D&travelDistance=%7B%22s%22%3A27%2C%22u%22%3A%22in%22%7D public static final double kElevatorKs = 0.18006; // volts (V) @@ -194,21 +201,27 @@ public static final class Elevator { public static final double kElevatorKv = 5.3925; // volt per velocity (V/(m/s)) public static final double kElevatorKa = 0.46421; // volt per acceleration (V/(m/s²)) + // TODO: Elevator has changed slightly, confirm values are still accurate. public static final double kElevatorGearing = 16.0; //reduction public static final double kElevatorDrumRadius = Units.inchesToMeters(1.128); //57.3 mm diameter public static final double kElevatorDrumCircumference = kElevatorDrumRadius * 2.0 * Math.PI; public static final double kCarriageMass = Units.lbsToKilograms(20.0); + // TODO: Elevator has changed slightly, confirm values are still accurate. public static final double kMinElevatorHeightMeters = 0.0; // public static final double kMaxElevatorHeightMeters = Units.inchesToMeters(27.0); (27" * 2 for the cascade) public static final double kMaxElevatorHeightMeters = Units.inchesToMeters(54.0); // ~ 1.3716 meterss public static final double kTiltGearing = 75.0; //reduction + // TODO: something to keep in mind - if we continuously apply voltage into the + // hardstop, we may see higher rates of wear and tear. we may want to consider + // driving the elevator to, say, 1 inch above the hardstop, and then letting it + // drift down the last little bit. public static final double POSITION_BOTTOM = 0.0; - //TODO: test and confirm these values - these are just dummy values - public static final double POSITION_L1 = Units.inchesToMeters(6.0); - public static final double POSITION_L2 = Units.inchesToMeters(12.0); + //TODO: test and confirm these values - these are just dummy values + public static final double POSITION_L1 = Units.inchesToMeters(6.0); //Current plan is for L1 scoring to be from a totally bottomed elevator + public static final double POSITION_L2 = Units.inchesToMeters(12.0); //L2 scoring may be possible with a bottomed elevator, with some hardware tweaks. It might be worth a strategic discussion of whether or scoring L2 with a bottomed elevator should be a design target. public static final double POSITION_L3 = Units.inchesToMeters(18.0); public static final double POSITION_L4 = Units.inchesToMeters(24.0); @@ -221,10 +234,12 @@ public static final class Elevator { } public static final class PathPlanner { + //TODO: Do we still need to tune these values? public static final double rotation_P = 1.5; public static final double rotation_I = 0.0; public static final double rotation_D = 0.0; + //TODO: Do we still need to tune these values? public static final double translation_P = 1.0; public static final double translation_I = 0.0; public static final double translation_D = 0.0; @@ -234,6 +249,7 @@ public static final class Vision { public static final boolean VISION_ENABLED = true; public static final int APRIL_TAG_PIPELINE_INDEX = 0; public static final String ARDUCAM_MODEL = "OV9281"; + //TODO: Do we still need to tune these values? public static final double POSE_AMBIGUITY_CUTOFF = 0.2; //https://docs.photonvision.org/en/latest/docs/apriltag-pipelines/3D-tracking.html#ambiguity public static final double POSE_AMBIGUITY_SHIFTER = 0.2; public static final double POSE_AMBIGUITY_MULTIPLIER = 4.0; @@ -243,6 +259,7 @@ public static final class Vision { public static final int TAG_PRESENCE_WEIGHT = 10; public static final PoseStrategy POSE_STRATEGY = PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR; + //TODO: Do we still need to tune these values? /** * Standard deviations for vision measurements. Increase these numbers to trust your * vision measurements less. This matrix is in the form [x, y, theta]ᵀ, with units in meters and radians. @@ -259,6 +276,11 @@ public static final class Vision { * Unique camera names, usable in PhotonCamera instances */ public static final class CameraName { + /**TODO: current plan for cameras is as follows, in order of priority, based off what we have weight to allow for + * Priority 1: single camera, bottom of the elevator, fixed to the frame, in the middle + * Priority 2: two cameras, opposite the elevator, fixed to the swerve drive pods + * Priority 3: single camera, top of the elevator, tbd if it moves up and down with the elevator or not + */ //Note: these names are set in hardware via https://docs.arducam.com/UVC-Camera/Serial-Number-Tool-Guide/ public static final String FRONT_LEFT = "Arducam_OV9281_USB_Camera-2"; public static final String FRONT_RIGHT = "Arducam_OV9281_USB_Camera-3"; @@ -270,6 +292,7 @@ public static final class CameraName { * Mounting position of the cameras on the Robot */ public static final class CameraPose { + // TODO: These values will need to be updated once the cameras are mounted private static final double CAM_XY_FROM_CENTER_OF_ROBOT = Units.inchesToMeters(10.0625); private static final double CAM_Z_FROM_FLOOR = Units.inchesToMeters(8.5); private static final double CAM_PITCH_ANGLE = -Units.degreesToRadians(25.0); @@ -297,9 +320,10 @@ public static final class CameraPose { public static final class Game { //Note: field layout values can be obtained by examining the AprilTagFieldLayout .json file for the game - public static final double FIELD_LENGTH_METERS = 16.541; //x in field drawings (from 2024 game - update for 2025 if necessary) - public static final double FIELD_WIDTH_METERS = 8.211; //y in field drawings (from 2024 game - update for 2025 if necessary) + public static final double FIELD_LENGTH_METERS = 16.541; //x in field drawings (FIXME: from 2024 game - update for 2025 if necessary) + public static final double FIELD_WIDTH_METERS = 8.211; //y in field drawings (FIXME: from 2024 game - update for 2025 if necessary) //These are buffers to accomodate for margin of error + // TODO: Do we need to tune these? public static final double FIELD_POSE_XY_ERROR_MARGIN_METERS = Units.inchesToMeters(1.0); public static final double FIELD_POSE_THETA_ERROR_MARGIN_RADIANS = Units.degreesToRadians(2.0); } @@ -307,5 +331,6 @@ public static final class Game { public static final class Shuffleboard { public static final ShuffleboardTab COMPETITION_TAB = edu.wpi.first.wpilibj.shuffleboard.Shuffleboard.getTab("Competition"); public static final ShuffleboardTab DIAG_TAB = edu.wpi.first.wpilibj.shuffleboard.Shuffleboard.getTab("diag"); + public static final ShuffleboardTab PRACTICE_TAB = edu.wpi.first.wpilibj.shuffleboard.Shuffleboard.getTab("Practice"); } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0a78a6d..bfacf8e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,13 +5,16 @@ package frc.robot; import frc.robot.Constants.DriverStation; +import frc.robot.commands.DefaultClimbCommand; import frc.robot.commands.FieldOrientedDriveCommand; +import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Elevator; import frc.robot.subsystems.Rejector; import frc.robot.subsystems.VisionSystem; import frc.robot.util.Utilities; +import java.util.function.BooleanSupplier; import java.util.function.DoubleSupplier; import com.pathplanner.lib.auto.AutoBuilder; @@ -40,9 +43,19 @@ public class RobotContainer { private final Drivetrain drivetrain = new Drivetrain(); private final Elevator elevator = new Elevator(); // private final Rejector rejector = new Rejector(); + private final Climber climber = new Climber(); private final VisionSystem visionSystem; private final SendableChooser autoChooser; + + /* + * TODO: if possible, I think we should remove startingPositionChooser, and try + * to use vision info to provide this instead. Last year we used + * startingPositionChooser because the robot couldn't see april tags at the + * start of the auto, and because the base of the speaker provided an accurate + * and reliable hardstop. This year, I think its the opposite - we will be able + * to see apriltags, and we won't have a reliable hardstop. + */ private final SendableChooser startingPosisitonChooser = new SendableChooser<>(); /** The container for the robot. Contains subsystems, IO devices, and commands. */ @@ -98,6 +111,9 @@ private void setDefaultCommands() { elevator.setDefaultCommand(elevator.elevatorTiltXBoxControllerCommand(elevatorTiltSpeedSupplier)); // DoubleSupplier rejectorRotationSupplier = () -> -Utilities.modifyAxisGeneric(operatorController.getLeftX(), 1.0, 0.05); // rejector.setDefaultCommand(rejector.getRejectorOperatorCommand(rejectorRotationSupplier)); + + BooleanSupplier bzClimberSupplier = () -> operatorController.povUp().getAsBoolean(); + climber.setDefaultCommand(new DefaultClimbCommand(climber, bzClimberSupplier)); } /** @@ -112,12 +128,17 @@ public Command getAutonomousCommand() { public void configureAutos() { Constants.Shuffleboard.COMPETITION_TAB.add("auto machine", autoChooser).withPosition(0, 0).withSize(2, 1); + startingPosisitonChooser.addOption("1", 1); startingPosisitonChooser.addOption("2", 2); startingPosisitonChooser.addOption("3", 3); Constants.Shuffleboard.COMPETITION_TAB.add("where am I?", startingPosisitonChooser).withPosition(2, 0); } + // TODO: I forget what the use-case was, but a student had an idea for vibrating + // only the left or right side, to indicate one of two different things. At some + // point, we should test out how detectable this is by the driver(s) + //MJR: TODO: If we end up needing this, call it like something like this: // new InstantCommand(() -> rumbleControllers(true)).andThen(new WaitCommand(1.0)).finallyDo(() -> rumbleControllers(false)); public void rumbleControllers(boolean rumble) { diff --git a/src/main/java/frc/robot/commands/DefaultClimbCommand.java b/src/main/java/frc/robot/commands/DefaultClimbCommand.java new file mode 100644 index 0000000..6a2485f --- /dev/null +++ b/src/main/java/frc/robot/commands/DefaultClimbCommand.java @@ -0,0 +1,51 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.Climber; +import java.util.function.BooleanSupplier; + +/** + * This command is used to perform climbing with a joystick (speedSupplier is the joystick input value ) + */ + public class DefaultClimbCommand extends Command { + private final Climber climber; + + // DoubleSupplier objects need to be used, not double + private final BooleanSupplier readPress; + + public DefaultClimbCommand(Climber climber, BooleanSupplier speedSupplier) { + this.climber = climber; + this.readPress = speedSupplier; + + // Command requires the intake subsystem + addRequirements(climber); + } + + /** + * This method is run every 20 ms. + *

+ * Send the input speeds to the appropriate intake/outtake method + */ + @Override + public void execute() { + // dont always command zero so other commands can use the intake + if (!DriverStation.isAutonomous()){ + if (readPress.getAsBoolean()) { + climber.climb(1.0); + } + else { + climber.climb(0.0); + + } + } + } + + /** When the intake method is interupted, set all velocities to zero. */ + @Override + public void end(boolean interrupted) { + climber.stop(); + } + } + + diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java new file mode 100644 index 0000000..419d822 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -0,0 +1,40 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +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.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +public class Climber extends SubsystemBase { + private final SparkMax climberMotor; + + // this is motor. "climber motor" is must had for all coding + public Climber() { + climberMotor = new SparkMax(19, MotorType.kBrushed); + //inverted is when you go back + //brake mode is so you don't fall down(climber) + SparkMaxConfig climbermotorConfig = new SparkMaxConfig(); + climbermotorConfig.inverted(false).idleMode(IdleMode.kBrake); + climbermotorConfig.smartCurrentLimit(45).secondaryCurrentLimit(65); //TODO: confirm these current limits are good, and convert to finals in the Constants Class + climberMotor.configure(climbermotorConfig, ResetMode.kResetSafeParameters, PersistMode.kNoPersistParameters); + } + + /** + * Sets the speeeeeeeeeeeeeeeed(BTW double is a noninteger ) + * @param speed how fast you want to go + */ + public void climb(double speed) { + climberMotor.set(speed); + } + + /** + * Stops the climber + */ + public void stop() { + climberMotor.set(0.); + } + +} diff --git a/src/main/java/frc/robot/util/Utilities.java b/src/main/java/frc/robot/util/Utilities.java index 355e3fe..7655508 100644 --- a/src/main/java/frc/robot/util/Utilities.java +++ b/src/main/java/frc/robot/util/Utilities.java @@ -15,6 +15,8 @@ public class Utilities { * * @param value The value you want to modify * @return The filtered value + * + * FIXME: Matt: I recommend we remove this, as it is arguably obselete by the generic version below. */ public static double modifyAxis(double value) { // Deadband @@ -46,6 +48,8 @@ public static double modifyAxisGeneric(double value, double power, double deadba } /* * I didnt write this and i dont remember what it does + * FIXME: Matt: I recommend we remove this if we don't remember what it does. + * For what its worth, we're not currently using it. */ public static double reboundValue(double value, double anchor){ double lowerBound = anchor - 180; @@ -61,6 +65,7 @@ public static double reboundValue(double value, double anchor){ public static void verifySparkMaxStatus(REVLibError revResult, int canID, String deviceName, String operation) { if (revResult != REVLibError.kOk) { + //FIXME: Matt - I know nothing about resource leaks, but it sounds bad? new Alert(deviceName + " SparkMax with CAN ID: " + canID + " failed " + operation + "! Result = " + revResult.toString(), AlertType.kError).set(true); System.out.println("Error configuring drive motor: " + revResult); }