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);
}