From d5f8f87c25985af542071d6bbf6d4f3c227f3524 Mon Sep 17 00:00:00 2001 From: mcarr424 Date: Mon, 17 Feb 2025 16:24:14 -0500 Subject: [PATCH] Climber code & generic comments Contains Gabby's code for running the climber. Will likely require some tweaking of things like current limits once we can test it for real, but for the time being, it appears to be behaving correctly. Also includes some arbitrary thoughts and comments of different sections of code. --- build.gradle | 2 +- src/main/java/frc/robot/Constants.java | 51 ++++++++++++++----- src/main/java/frc/robot/RobotContainer.java | 21 ++++++++ .../robot/commands/DefaultClimbCommand.java | 51 +++++++++++++++++++ .../java/frc/robot/subsystems/Climber.java | 40 +++++++++++++++ src/main/java/frc/robot/util/Utilities.java | 5 ++ 6 files changed, 156 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc/robot/commands/DefaultClimbCommand.java create mode 100644 src/main/java/frc/robot/subsystems/Climber.java 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); }