Team_2648/2026-Robot-Code
5
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

A bunch of changes, mostly related to shooting balls at the hub dynamically, still need a means of doing this based on a single apriltag, in the event the robot pose is unreliable

Browse Source
This commit is contained in:
Bradley Bickford
2026-02-12 16:02:08 -05:00
parent f8429dc899
commit 91a5281202
12 changed files with 181 additions and 135 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
src/main/java/frc/robot/RobotContainer.java
View File

@@ -9,6 +9,8 @@ import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Transform2d;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -19,15 +21,23 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.AutoConstants;
import frc.robot.constants.CompetitionConstants;
import frc.robot.constants.HoodConstants;
import frc.robot.constants.OIConstants;
import frc.robot.constants.ShooterConstants;
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
import frc.robot.subsystems.Drivetrain;
import frc.robot.subsystems.Hood;
import frc.robot.subsystems.PhotonVision;
import frc.robot.subsystems.Shooter;
import frc.robot.utilities.Elastic;
import frc.robot.utilities.Utilities;
public class RobotContainer {
private PhotonVision vision;
private Drivetrain drivetrain;
private Hood hood;
private Shooter shooter;
private CommandXboxController driver;
@@ -38,6 +48,8 @@ public class RobotContainer {
public RobotContainer() {
vision = new PhotonVision();
drivetrain = new Drivetrain(null);
hood = new Hood();
shooter = new Shooter();
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
@@ -72,6 +84,33 @@ public class RobotContainer {
false // TODO Should this be true by default?
)
);
shooter.setDefaultCommand(
shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)
);
hood.setDefaultCommand(hood.trackToAngle(() -> {
Pose2d drivetrainPose = drivetrain.getPose();
Pose2d hubPose = Utilities.getHubPose();
double distance = drivetrainPose.getTranslation()
.plus(CompetitionConstants.KRobotToShooter.getTranslation().toTranslation2d())
.getDistance(hubPose.getTranslation());
if(HoodConstants.kUseInterpolatorForAngle) {
return HoodConstants.kDistanceToAngle.get(distance);
} else {
// TODO The average actual speeds isn't <i>really</i> the exit velocity of the ball
// on a hooded shooter, based on documentation, it's more like 30-50% depending on
// hood material, surface friction, etc.
return Utilities.shotAngle(
shooter.getAverageActualSpeeds(),
distance,
CompetitionConstants.kHubGoalHeightMeters - ShooterConstants.kShooterHeightMeters,
false
);
}
}));
}
private void configureNamedCommands() {

6
src/main/java/frc/robot/constants/CompetitionConstants.java
View File

@@ -4,6 +4,7 @@ import edu.wpi.first.apriltag.AprilTagFieldLayout;
import edu.wpi.first.apriltag.AprilTagFields;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.math.util.Units;
public class CompetitionConstants {
@@ -14,6 +15,11 @@ public class CompetitionConstants {
AprilTagFields.kDefaultField
);
public static final double kHubGoalHeightMeters = Units.inchesToMeters(72);
// TODO Real Values
public static final Transform3d KRobotToShooter = new Transform3d();
public static final Pose2d kBlueHubLocation = new Pose2d(
Units.inchesToMeters(182.11),
Units.inchesToMeters(158.84),

7
src/main/java/frc/robot/constants/HoodConstants.java
View File

@@ -4,6 +4,8 @@ import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
public class HoodConstants {
// TODO Real Values
public static final int kMotorCANID = 0;
@@ -15,13 +17,18 @@ public class HoodConstants {
public static final double kV = 0;
public static final double kA = 0;
public static final double kStartupAngle = 0;
public static final double kMaxManualSpeedMultiplier = .5;
public static final int kCurrentLimit = 15;
public static final boolean kInverted = false;
public static final boolean kUseInterpolatorForAngle = false;
public static final IdleMode kIdleMode = IdleMode.kBrake;
// TODO This needs to be filled in from some source
public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap();
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
public static final SparkMaxConfig kConfig = new SparkMaxConfig();

3
src/main/java/frc/robot/constants/IntakePivotConstants.java
View File

@@ -21,9 +21,10 @@ public class IntakePivotConstants {
}
public static final int kLeftMotorCANID = 0;
public static final int kRightMotorCANID = 1;
public static final int kRightMotorCANID = 0;
public static final double kConversionFactor = 0;
public static final double kMaxManualSpeedMultiplier = .5;
public static final double kP = 0;
public static final double kI = 0;

106
src/main/java/frc/robot/constants/ShooterConstants.java
View File

@@ -8,23 +8,17 @@ import edu.wpi.first.math.util.Units;
public class ShooterConstants {
public enum ShooterSpeeds {
kHubSpeed(0, 0),
kFeedSpeed(0, 0);
kHubSpeed(0),
kFeedSpeed(0);
private double frontRollerMPS;
private double rearRollerMPS;
private double speedMPS;
private ShooterSpeeds(double frontRollerMPS, double rearRollerMPS) {
this.frontRollerMPS = frontRollerMPS;
this.rearRollerMPS = rearRollerMPS;
private ShooterSpeeds(double speedMPS) {
this.speedMPS = speedMPS;
}
public double getFrontRollerMPS() {
return frontRollerMPS;
}
public double getRearRollerMPS() {
return rearRollerMPS;
public double getSpeedMPS() {
return speedMPS;
}
}
@@ -33,29 +27,31 @@ public class ShooterConstants {
public static final double kWheelDiameter = Units.inchesToMeters(6);
// TODO Real values
public static final int kFrontShooterMotor1CANID = 0;
public static final int kFrontShooterMotor2CANID = 0;
public static final int kRearShooterMotor1CANID = 0;
public static final int kRearShooterMotor2CANID = 0;
public static final int kLeftShooterMotor1CANID = 0;
public static final int kLeftShooterMotor2CANID = 0;
public static final int kRightShooterMotor1CANID = 0;
public static final int kRightShooterMotor2CANID = 0;
public static final boolean kFrontShooterMotor1Inverted = false;
public static final boolean kFrontShooterMotor2Inverted = false;
public static final boolean kRearShooterMotor1Inverted = false;
public static final boolean kRearShooterMotor2Inverted = false;
public static final boolean kLeftShooterMotor1Inverted = false;
public static final boolean kLeftShooterMotor2Inverted = false;
public static final boolean kRightShooterMotor1Inverted = false;
public static final boolean kRightShooterMotor2Inverted = false;
public static final double kFrontP = 0;
public static final double kFrontI = 0;
public static final double kFrontD = 0;
public static final double kFrontS = 0;
public static final double kFrontV = 0;
public static final double kFrontA = 0;
public static final double kLeftP = 0;
public static final double kLeftI = 0;
public static final double kLeftD = 0;
public static final double kLeftS = 0;
public static final double kLeftV = 0;
public static final double kLeftA = 0;
public static final double kRearP = 0;
public static final double kRearI = 0;
public static final double kRearD = 0;
public static final double kRearS = 0;
public static final double kRearV = 0;
public static final double kRearA = 0;
public static final double kRightP = 0;
public static final double kRightI = 0;
public static final double kRightD = 0;
public static final double kRightS = 0;
public static final double kRightV = 0;
public static final double kRightA = 0;
public static final double kShooterHeightMeters = 0;
// TODO Is this value sane?
public static final int kCurrentLimit = 30;
@@ -64,51 +60,51 @@ public class ShooterConstants {
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
public static final SparkMaxConfig kFrontMotor1Config = new SparkMaxConfig();
public static final SparkMaxConfig kFrontMotor2Config = new SparkMaxConfig();
public static final SparkMaxConfig kRearMotor1Config = new SparkMaxConfig();
public static final SparkMaxConfig kRearMotor2Config = new SparkMaxConfig();
public static final SparkMaxConfig kLeftMotor1Config = new SparkMaxConfig();
public static final SparkMaxConfig kLeftMotor2Config = new SparkMaxConfig();
public static final SparkMaxConfig kRightMotor1Config = new SparkMaxConfig();
public static final SparkMaxConfig kRightMotor2Config = new SparkMaxConfig();
static {
kFrontMotor1Config
kLeftMotor1Config
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kFrontShooterMotor1Inverted);
kFrontMotor1Config.absoluteEncoder
.inverted(kLeftShooterMotor1Inverted);
kLeftMotor1Config.absoluteEncoder
.positionConversionFactor(kWheelDiameter * Math.PI)
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
kFrontMotor1Config.closedLoop
kLeftMotor1Config.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kFrontP, kFrontI, kFrontD)
.pid(kLeftP, kLeftI, kLeftD)
.outputRange(-1, 1)
.feedForward
.sva(kFrontS, kFrontV, kFrontA);
.sva(kLeftS, kLeftV, kLeftA);
kFrontMotor2Config
kLeftMotor2Config
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kFrontShooterMotor2Inverted)
.follow(kFrontShooterMotor1CANID);
.inverted(kLeftShooterMotor2Inverted)
.follow(kLeftShooterMotor1CANID);
kRearMotor1Config
kRightMotor1Config
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kRearShooterMotor1Inverted);
kRearMotor1Config.absoluteEncoder
.inverted(kRightShooterMotor1Inverted);
kRightMotor1Config.absoluteEncoder
.positionConversionFactor(kWheelDiameter * Math.PI)
.velocityConversionFactor(kWheelDiameter * Math.PI / 60);
kRearMotor1Config.closedLoop
kRightMotor1Config.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kRearP, kRearI, kRearD)
.pid(kRightP, kRightI, kRightD)
.outputRange(-1, 1)
.feedForward
.sva(kRearS, kRearV, kRearA);
.sva(kRightS, kRightV, kRightA);
kRearMotor2Config
kRightMotor2Config
.idleMode(kShooterIdleMode)
.smartCurrentLimit(kCurrentLimit)
.inverted(kRearShooterMotor2Inverted)
.follow(kRearShooterMotor1CANID);
.inverted(kRightShooterMotor2Inverted)
.follow(kRightShooterMotor1CANID);
}
}

27
src/main/java/frc/robot/interfaces/IVisualPoseProvider.java
View File

@@ -1,27 +0,0 @@
package frc.robot.interfaces;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d;
/**
* An interface which ensures a class' ability to provide visual pose information
* in a consistent way
*/
public interface IVisualPoseProvider {
/**
* A record that can contain the two elements necessary for a WPILIB
* pose estimator to use the information from a vision system as part of a full
* robot pose estimation
*/
public record VisualPose(Pose2d visualPose, double timestamp) {}
/**
* Return a VisualPose or null if an empty Optional if none is available.
* Implementation should provide an empty response if it's unable to provide
* a reliable pose, or any pose at all.
*
* @return An Optional containing a VisualPose, or empty if no VisualPose can reliably be provided
*/
public Optional<VisualPose> getVisualPose();
}

6
src/main/java/frc/robot/subsystems/Drivetrain.java
View File

@@ -1,7 +1,6 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.OptionalDouble;
import java.util.function.BooleanSupplier;
import java.util.function.DoubleSupplier;
@@ -30,8 +29,8 @@ import frc.robot.constants.AutoConstants;
import frc.robot.constants.CompetitionConstants;
import frc.robot.constants.DrivetrainConstants;
import frc.robot.constants.OIConstants;
import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
import frc.robot.utilities.SwerveModule;
import frc.robot.utilities.VisualPose;
public class Drivetrain extends SubsystemBase {
private SwerveModule frontLeft;
@@ -45,9 +44,6 @@ public class Drivetrain extends SubsystemBase {
private PIDController yawRotationController;
private PhotonVision camera1;
private PhotonVision camera2;
public Drivetrain(Pose2d startupPose) {
frontLeft = new SwerveModule(
"FrontLeft",

10
src/main/java/frc/robot/subsystems/Hood.java
View File

@@ -48,12 +48,18 @@ public class Hood extends SubsystemBase {
});
}
public Command stop() {
public Command manualSpeed(DoubleSupplier speed) {
currentTargetRadians = 0;
return run(() -> {
motor.disable();
motor.set(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier);
});
}
public Command stop() {
return manualSpeed(() -> 0);
}
public double getTargetRadians() {
return currentTargetRadians;
}

11
src/main/java/frc/robot/subsystems/IntakePivot.java
View File

@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
@@ -69,8 +70,16 @@ public class IntakePivot extends SubsystemBase {
});
}
public Command manualSpeed(DoubleSupplier speed) {
currentTargetPosition = null;
return run(() -> {
leftMotor.set(speed.getAsDouble() * IntakePivotConstants.kMaxManualSpeedMultiplier);
});
}
public Command stop() {
return maintainPosition(null);
return manualSpeed(() -> 0);
}
public Optional<IntakePivotPosition> getCurrentTargetPosition() {

3
src/main/java/frc/robot/subsystems/PhotonVision.java
View File

@@ -24,9 +24,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.CompetitionConstants;
import frc.robot.constants.PhotonConstants;
import frc.robot.interfaces.IAprilTagProvider;
import frc.robot.interfaces.IVisualPoseProvider;
import frc.robot.interfaces.IVisualPoseProvider.VisualPose;
import frc.robot.utilities.PhotonVisionConfig;
import frc.robot.utilities.VisualPose;
public class PhotonVision extends SubsystemBase {
private PhotonCamera[] cameras;

93
src/main/java/frc/robot/subsystems/Shooter.java
View File

@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
@@ -18,54 +19,54 @@ import frc.robot.constants.ShooterConstants;
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
public class Shooter extends SubsystemBase {
private SparkMax frontMotor1;
private SparkMax frontMotor2;
private SparkMax rearMotor1;
private SparkMax rearMotor2;
private SparkMax leftMotor1;
private SparkMax leftMotor2;
private SparkMax rightMotor1;
private SparkMax rightMotor2;
private AbsoluteEncoder frontEncoder;
private AbsoluteEncoder rearEncoder;
private AbsoluteEncoder leftEncoder;
private AbsoluteEncoder rightEncoder;
private SparkClosedLoopController frontClosedLoopController;
private SparkClosedLoopController rearClosedLoopController;
private SparkClosedLoopController leftClosedLoopController;
private SparkClosedLoopController rightClosedLoopController;
private ShooterSpeeds targetSpeeds;
public Shooter() {
frontMotor1 = new SparkMax(ShooterConstants.kFrontShooterMotor1CANID, MotorType.kBrushless);
frontMotor2 = new SparkMax(ShooterConstants.kFrontShooterMotor2CANID, MotorType.kBrushless);
rearMotor1 = new SparkMax(ShooterConstants.kRearShooterMotor1CANID, MotorType.kBrushless);
rearMotor2 = new SparkMax(ShooterConstants.kRearShooterMotor2CANID, MotorType.kBrushless);
leftMotor1 = new SparkMax(ShooterConstants.kLeftShooterMotor1CANID, MotorType.kBrushless);
leftMotor2 = new SparkMax(ShooterConstants.kLeftShooterMotor2CANID, MotorType.kBrushless);
rightMotor1 = new SparkMax(ShooterConstants.kRightShooterMotor1CANID, MotorType.kBrushless);
rightMotor2 = new SparkMax(ShooterConstants.kRightShooterMotor2CANID, MotorType.kBrushless);
frontMotor1.configure(
ShooterConstants.kFrontMotor1Config,
leftMotor1.configure(
ShooterConstants.kLeftMotor1Config,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
rearMotor1.configure(
ShooterConstants.kRearMotor1Config,
rightMotor1.configure(
ShooterConstants.kRightMotor1Config,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
frontMotor2.configure(
ShooterConstants.kFrontMotor2Config,
leftMotor2.configure(
ShooterConstants.kLeftMotor2Config,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
rearMotor2.configure(
ShooterConstants.kRearMotor2Config,
rightMotor2.configure(
ShooterConstants.kRightMotor2Config,
ResetMode.kResetSafeParameters,
PersistMode.kPersistParameters
);
frontEncoder = frontMotor1.getAbsoluteEncoder();
rearEncoder = rearMotor1.getAbsoluteEncoder();
leftEncoder = leftMotor1.getAbsoluteEncoder();
rightEncoder = rightMotor1.getAbsoluteEncoder();
frontClosedLoopController = frontMotor1.getClosedLoopController();
rearClosedLoopController = rearMotor1.getClosedLoopController();
leftClosedLoopController = leftMotor1.getClosedLoopController();
rightClosedLoopController = rightMotor1.getClosedLoopController();
// TODO Set this to the initial startup speed
targetSpeeds = null;
@@ -74,21 +75,16 @@ public class Shooter extends SubsystemBase {
@Override
public void periodic() {
Logger.recordOutput(
"Shooter/FrontRollers/TargetMPS",
targetSpeeds == null ? 0 : targetSpeeds.getFrontRollerMPS()
"Shooter/TargetMPS",
targetSpeeds == null ? 0 : targetSpeeds.getSpeedMPS()
);
Logger.recordOutput(
"Shooter/RearRollers/TargetMPS",
targetSpeeds == null ? 0 : targetSpeeds.getRearRollerMPS()
);
Logger.recordOutput("Shooter/FrontRollers/CurrentMPS", frontEncoder.getVelocity());
Logger.recordOutput("Shooter/RearRollers/CurrentMPS", rearEncoder.getVelocity());
Logger.recordOutput("Shooter/LeftRollers/CurrentMPS", leftEncoder.getVelocity());
Logger.recordOutput("Shooter/RightRollers/CurrentMPS", rightEncoder.getVelocity());
// TODO How does the SparkMAX controller determine "at setpoint"? Is there any tolerance?
Logger.recordOutput("Shooter/FrontRollers/AtSetpoint", frontClosedLoopController.isAtSetpoint());
Logger.recordOutput("Shooter/RearRollers/AtSetpoint", rearClosedLoopController.isAtSetpoint());
Logger.recordOutput("Shooter/LeftRollers/AtSetpoint", leftClosedLoopController.isAtSetpoint());
Logger.recordOutput("Shooter/RightRollers/AtSetpoint", rightClosedLoopController.isAtSetpoint());
}
public Command maintainSpeed(ShooterSpeeds speeds) {
@@ -96,24 +92,37 @@ public class Shooter extends SubsystemBase {
return run(() -> {
if(targetSpeeds == null) {
frontMotor1.disable();
rearMotor1.disable();
leftMotor1.disable();
rightMotor1.disable();
} else {
frontClosedLoopController.setSetpoint(
targetSpeeds.getFrontRollerMPS(),
leftClosedLoopController.setSetpoint(
targetSpeeds.getSpeedMPS(),
ControlType.kVelocity
);
rearClosedLoopController.setSetpoint(
targetSpeeds.getRearRollerMPS(),
rightClosedLoopController.setSetpoint(
targetSpeeds.getSpeedMPS(),
ControlType.kVelocity
);
}
});
}
public Command manualSpeed(DoubleSupplier speed) {
targetSpeeds = null;
return run(() -> {
leftMotor1.set(speed.getAsDouble());
rightMotor1.set(speed.getAsDouble());
});
}
public Command stop() {
return maintainSpeed(null);
return manualSpeed(() -> 0);
}
public double getAverageActualSpeeds() {
return (leftEncoder.getVelocity() + rightEncoder.getVelocity()) / 2;
}
public Optional<ShooterSpeeds> getTargetSpeeds() {

5
src/main/java/frc/robot/utilities/VisualPose.java Normal file
View File

@@ -0,0 +1,5 @@
package frc.robot.utilities;
import edu.wpi.first.math.geometry.Pose2d;
public record VisualPose(Pose2d visualPose, double timestamp) {}