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

Working on various auto lock on and some Auto stuff

Browse Source
This commit is contained in:
Bradley Bickford
2026-02-11 14:59:31 -05:00
parent 9549c5343d
commit f8429dc899
5 changed files with 220 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -5,6 +5,10 @@
package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -22,110 +26,125 @@ import frc.robot.utilities.Elastic;
import frc.robot.utilities.Utilities;
public class RobotContainer {
private PhotonVision vision;
private Drivetrain drivetrain;
private PhotonVision vision;
private Drivetrain drivetrain;
private CommandXboxController driver;
private CommandXboxController driver;
private SendableChooser<Command> autoChooser;
private SendableChooser<Command> autoChooser;
private Timer shiftTimer;
private Timer shiftTimer;
public RobotContainer() {
vision = new PhotonVision();
drivetrain = new Drivetrain();
public RobotContainer() {
vision = new PhotonVision();
drivetrain = new Drivetrain(null);
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
shiftTimer = new Timer();
shiftTimer.reset();
shiftTimer = new Timer();
shiftTimer.reset();
configureBindings();
}
configureBindings();
configureShiftDisplay();
private void configureBindings() {
drivetrain.setDefaultCommand(
drivetrain.drive(
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
() -> false
)
);
driver.start().and(driver.x()).whileTrue(drivetrain.runFrontLeft(1, 0));
driver.start().and(driver.y()).whileTrue(drivetrain.runFrontRight(1, 0));
driver.start().and(driver.a()).whileTrue(drivetrain.runRearLeft(1, 0));
driver.start().and(driver.b()).whileTrue(drivetrain.runRearRight(1, 0));
driver.start().negate().and(driver.x()).whileTrue(drivetrain.runFrontLeft(0, 45));
driver.start().negate().and(driver.y()).whileTrue(drivetrain.runFrontRight(0, 45));
driver.start().negate().and(driver.a()).whileTrue(drivetrain.runRearLeft(0, 45));
driver.start().negate().and(driver.b()).whileTrue(drivetrain.runRearRight(0, 45));
driver.rightBumper().whileTrue(drivetrain.setX());
//drivetrain.setDefaultCommand(drivetrain.disableOutputs());
configureShiftDisplay();
}
public Command getAutonomousCommand() {
if(AutoConstants.kAutoConfigOk) {
return autoChooser.getSelected();
} else {
return new PrintCommand("Robot Config loading failed, autonomous disabled");
if(AutoConstants.kAutoConfigOk) {
autoChooser = AutoBuilder.buildAutoChooser();
configureNamedCommands();
}
}
}
private void configureShiftDisplay() {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
private void configureBindings() {
drivetrain.setDefaultCommand(
drivetrain.drive(
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
() -> true
)
);
RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
shiftTimer.stop();
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
}));
driver.a().whileTrue(
drivetrain.lockRotationToHub(
driver::getLeftY,
driver::getLeftX,
false // TODO Should this be true by default?
)
);
}
RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
Elastic.selectTab(OIConstants.kTeleopTab);
shiftTimer.reset();
shiftTimer.start();
}));
private void configureNamedCommands() {
NamedCommands.registerCommand(
"Drivetrain Set X",
drivetrain.setX()
);
new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
}));
NamedCommands.registerCommand(
"Drivetrain Face Hub",
drivetrain.rotateToPose(
Utilities.getHubPose(),
false // TODO Should this be true by default?
)
);
}
new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
);
}));
public Command getAutonomousCommand() {
if(AutoConstants.kAutoConfigOk) {
return autoChooser.getSelected();
} else {
return new PrintCommand("Robot Config loading failed, autonomous disabled");
}
}
new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
);
}));
private void configureShiftDisplay() {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
);
}));
RobotModeTriggers.autonomous().onTrue(new InstantCommand(() -> {
shiftTimer.stop();
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
}));
new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
);
}));
RobotModeTriggers.teleop().onTrue(new InstantCommand(() -> {
Elastic.selectTab(OIConstants.kTeleopTab);
shiftTimer.reset();
shiftTimer.start();
}));
new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
}));
}
new Trigger(() -> shiftTimer.get() <= 10).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
}));
new Trigger(() -> shiftTimer.get() > 10 && shiftTimer.get() <= 35).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
);
}));
new Trigger(() -> shiftTimer.get() > 35 && shiftTimer.get() <= 60).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
);
}));
new Trigger(() -> shiftTimer.get() > 60 && shiftTimer.get() <= 85).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kRedDisplay : OIConstants.kBlueDisplay
);
}));
new Trigger(() -> shiftTimer.get() > 85 && shiftTimer.get() <= 110).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(
OIConstants.kCurrentActiveHub,
Utilities.whoHasFirstShift() == Alliance.Red ? OIConstants.kBlueDisplay : OIConstants.kRedDisplay
);
}));
new Trigger(() -> shiftTimer.get() > 110).onTrue(new InstantCommand(() -> {
SmartDashboard.putStringArray(OIConstants.kCurrentActiveHub, OIConstants.kRedBlueDisplay);
}));
}
}

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

@@ -10,6 +10,7 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
import com.pathplanner.lib.path.PathConstraints;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
// TODO This is all hold over from 2025, does any of it need to change?
public class AutoConstants {
@@ -23,6 +24,8 @@ public class AutoConstants {
public static final double kPXYController = 3.5;
public static final double kPThetaController = 5;
public static final double kYawPIDTolerance = Units.degreesToRadians(2);
public static final double kAlignPXYController = 2;
public static final double kAlignPThetaController = 5;

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

@@ -2,6 +2,9 @@ package frc.robot.constants;
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.util.Units;
public class CompetitionConstants {
// THIS SHOULD BE FALSE DURING COMPETITION PLAY
@@ -10,4 +13,17 @@ public class CompetitionConstants {
public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(
AprilTagFields.kDefaultField
);
public static final Pose2d kBlueHubLocation = new Pose2d(
Units.inchesToMeters(182.11),
Units.inchesToMeters(158.84),
Rotation2d.fromDegrees(0)
);
public static final Pose2d kRedHubLocation = new Pose2d(
Units.inchesToMeters(182.11 + 143.5 * 2),
Units.inchesToMeters(158.84),
Rotation2d.fromDegrees(0)
);
}

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

@@ -22,10 +22,12 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
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;
@@ -41,10 +43,12 @@ public class Drivetrain extends SubsystemBase {
private SwerveDrivePoseEstimator estimator;
private PIDController yawRotationController;
private PhotonVision camera1;
private PhotonVision camera2;
public Drivetrain() {
public Drivetrain(Pose2d startupPose) {
frontLeft = new SwerveModule(
"FrontLeft",
DrivetrainConstants.kFrontLeftDrivingCANID,
@@ -79,6 +83,14 @@ public class Drivetrain extends SubsystemBase {
gyro = new AHRS(NavXComType.kMXP_SPI);
yawRotationController = new PIDController(
AutoConstants.kPThetaController,
0,
0
);
yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
yawRotationController.setTolerance(AutoConstants.kYawPIDTolerance);
// TODO 2025 used non-standard deviations for encoder/gyro inputs and vision, will need to be tuned for 2026 in the future
estimator = new SwerveDrivePoseEstimator(
DrivetrainConstants.kDriveKinematics,
@@ -89,7 +101,7 @@ public class Drivetrain extends SubsystemBase {
rearLeft.getPosition(),
rearRight.getPosition()
},
new Pose2d()
startupPose != null ? startupPose : new Pose2d()
);
if(AutoConstants.kAutoConfigOk) {
@@ -131,7 +143,7 @@ public class Drivetrain extends SubsystemBase {
Logger.recordOutput("Drivetrain/Pose", getPose());
Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue());
Logger.recordOutput("Drivetrain/Heading", getHeading());
Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees());
}
public Command runFrontLeft(double staticSpeed, double staticAngleDegrees) {
@@ -172,6 +184,66 @@ public class Drivetrain extends SubsystemBase {
});
}
public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
return runOnce(yawRotationController::reset).andThen(
drive(
() -> 0,
() -> 0,
() -> {
Rotation2d targetRotation = new Rotation2d(
targetPose.getX() - getPose().getX(),
targetPose.getY() - getPose().getY()
);
if(rotate180) {
targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
}
return yawRotationController.calculate(
getHeading().getRadians(),
targetRotation.getRadians()
);
},
() -> true
))
.until(yawRotationController::atSetpoint);
}
public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
return runOnce(yawRotationController::reset).andThen(
drive(
xSpeed,
ySpeed,
() -> {
Optional<Alliance> alliance = DriverStation.getAlliance();
if(alliance.isEmpty()) {
return 0;
}
Pose2d faceTowards = alliance.get() == Alliance.Blue ?
CompetitionConstants.kBlueHubLocation :
CompetitionConstants.kRedHubLocation;
Rotation2d targetRotation = new Rotation2d(
faceTowards.getX() - getPose().getX(),
faceTowards.getY() - getPose().getY()
);
if(rotate180) {
targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
}
return yawRotationController.calculate(
getHeading().getRadians(),
targetRotation.getRadians()
);
},
() -> true
)
);
}
// TODO check both cameras
/*public Command driveAprilTagLock(DoubleSupplier xSpeed, DoubleSupplier ySpeed, double deadband, int tagID) {
if (camera1 == null) {
@@ -314,7 +386,11 @@ public class Drivetrain extends SubsystemBase {
return gyro.getAngle() * (DrivetrainConstants.kGyroReversed ? -1 : 1);
}
public double getHeading() {
public Rotation2d getHeading() {
return estimator.getEstimatedPosition().getRotation();
}
public double getHeadingDegrees() {
return estimator.getEstimatedPosition().getRotation().getDegrees();
}
}

14
src/main/java/frc/robot/utilities/Utilities.java
View File

@@ -1,7 +1,11 @@
package frc.robot.utilities;
import java.util.Optional;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import frc.robot.constants.CompetitionConstants;
public class Utilities {
public static final double kG = -9.81;
@@ -23,6 +27,16 @@ public class Utilities {
return null;
}
public static Pose2d getHubPose() {
Optional<Alliance> alliance = DriverStation.getAlliance();
if(alliance.isEmpty() || alliance.get() == Alliance.Blue) {
return CompetitionConstants.kBlueHubLocation;
} else {
return CompetitionConstants.kRedHubLocation;
}
}
/**
* A ChatGPT possible hallucination related to calcuating whether a shot is possible
* for a given speed and change in X and Y position