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

Compare commits

base: Team_2648:auto_lock_on_with_set_x
Branches Tags
Team_2648:main Team_2648:auto_lock_on_with_set_x Team_2648:tuning Team_2648:photonvision_as_a_resource Team_2648:brads-tinkering
..
compare: Team_2648:main
Branches Tags
Team_2648:main Team_2648:auto_lock_on_with_set_x Team_2648:tuning Team_2648:photonvision_as_a_resource Team_2648:brads-tinkering

1 Commits
auto_lock_ ... main

Author SHA1 Message Date
Tylr-J42
013050ee19 outpost auto improved 2026-04-04 17:35:34 -04:00
10 changed files with 206 additions and 154 deletions
Expand all files Collapse all files

58
src/main/deploy/pathplanner/autos/Left Shoot Outpost.auto
View File

@@ -11,39 +11,41 @@
}
},
{
"type": "named",
"type": "parallel",
"data": {
"name": "spinup"
"commands": [
{
"type": "path",
"data": {
"pathName": "Start to Outpost"
}
}
]
}
},
{
"type": "path",
"type": "wait",
"data": {
"pathName": "start to score left"
"waitTime": 2.0
}
},
{
"type": "named",
"type": "parallel",
"data": {
"name": "shoot close"
}
},
{
"type": "path",
"data": {
"pathName": "Left to Outpost"
}
},
{
"type": "named",
"data": {
"name": "stop spindexer"
}
},
{
"type": "path",
"data": {
"pathName": "trough to shot"
"commands": [
{
"type": "path",
"data": {
"pathName": "trough to shot"
}
},
{
"type": "named",
"data": {
"name": "spinup"
}
}
]
}
},
{
@@ -55,7 +57,13 @@
{
"type": "named",
"data": {
"name": "shoot N jimmy"
"name": "aim"
}
},
{
"type": "named",
"data": {
"name": "auto shoot"
}
}
]

16
src/main/deploy/pathplanner/paths/Left to Outpost.path
View File

@@ -3,25 +3,25 @@
"waypoints": [
{
"anchor": {
"x": 3.2515736040609142,
"y": 4.914375634517767
"x": 1.925604060913706,
"y": 5.503695431472081
},
"prevControl": null,
"nextControl": {
"x": 2.4338749089244973,
"y": 5.333984175443034
"x": 1.107905365777289,
"y": 5.923303972397348
},
"isLocked": false,
"linkedName": "left close"
},
{
"anchor": {
"x": 0.77458883248731,
"y": 5.927269035532995
"x": 0.32339086294416286,
"y": 5.9825177664974625
},
"prevControl": {
"x": 2.3777086426889733,
"y": 6.110175322602984
"x": 1.9265106731458264,
"y": 6.165424053567452
},
"nextControl": null,
"isLocked": false,

38
src/main/deploy/pathplanner/paths/Start to outpost.path
View File

@@ -3,35 +3,35 @@
"waypoints": [
{
"anchor": {
"x": 3.6619856887266913,
"y": 2.2583184257605784
"x": 3.5922741116751276,
"y": 6.3692588832487305
},
"prevControl": null,
"nextControl": {
"x": 2.4937567084046877,
"y": 1.3172450805011864
"x": 2.956913705583757,
"y": 6.19430456852792
},
"isLocked": false,
"linkedName": null
"linkedName": "start left"
},
{
"anchor": {
"x": 0.7089624329216013,
"y": 0.668228980317108
"x": 0.32339086294416286,
"y": 5.9825177664974625
},
"prevControl": {
"x": 1.6987119856944104,
"y": 1.0414132379199703
"x": 1.3270761421319812,
"y": 5.964101522842641
},
"nextControl": null,
"isLocked": false,
"linkedName": "Outpost"
"linkedName": "trough"
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.7234468937875753,
"rotationDegrees": 180.0
"waypointRelativePos": 0.6910862504511138,
"rotationDegrees": -179.02889973265033
}
],
"constraintZones": [],
@@ -39,14 +39,14 @@
"eventMarkers": [
{
"name": "Intake Start",
"waypointRelativePos": 0.4500000000000002,
"waypointRelativePos": 0,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.5,
"maxVelocity": 0.75,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -54,13 +54,13 @@
},
"goalEndState": {
"velocity": 0,
"rotation": 180.0
"rotation": 178.80651057601818
},
"reversed": false,
"folder": "Right Outpost",
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": 180.0
"rotation": -90.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

75
src/main/deploy/pathplanner/paths/left start to center diagonal.path Normal file
View File

@@ -0,0 +1,75 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 3.5922741116751276,
"y": 6.3692588832487305
},
"prevControl": null,
"nextControl": {
"x": 3.9329746192893413,
"y": 6.3692588832487305
},
"isLocked": false,
"linkedName": "start left"
},
{
"anchor": {
"x": 4.577543147208122,
"y": 5.715482233502538
},
"prevControl": {
"x": 4.264467005076142,
"y": 5.715482233502538
},
"nextControl": {
"x": 5.120900364352579,
"y": 5.715482233502538
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.041634517766498,
"y": 6.3692588832487305
},
"prevControl": {
"x": 5.931137055837564,
"y": 5.65102538071066
},
"nextControl": null,
"isLocked": false,
"linkedName": "over bump"
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.8,
"rotationDegrees": -135.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 2.0,
"rotation": -129.95754893082906
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -90.0
},
"useDefaultConstraints": false
}

34
src/main/deploy/pathplanner/paths/left start to center.path
View File

@@ -3,45 +3,29 @@
"waypoints": [
{
"anchor": {
"x": 3.573857868020305,
"x": 3.5922741116751276,
"y": 6.3692588832487305
},
"prevControl": null,
"nextControl": {
"x": 3.0213705583756347,
"y": 6.461340101522843
"x": 3.085827411167513,
"y": 6.277177664974619
},
"isLocked": false,
"linkedName": "start left"
},
{
"anchor": {
"x": 3.0766192893401025,
"y": 5.936477157360406
},
"prevControl": {
"x": 2.8556243654822344,
"y": 6.28638578680203
},
"nextControl": {
"x": 3.3374096167812595,
"y": 5.523559138911907
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.577543147208122,
"y": 5.715482233502538
},
"prevControl": {
"x": 3.5645859798769006,
"y": 5.732650999050524
"x": 3.2884060913705584,
"y": 5.697065989847716
},
"nextControl": {
"x": 5.120822335025381,
"y": 5.706274111675127
"x": 5.120844928223563,
"y": 5.723243687517044
},
"isLocked": false,
"linkedName": null
@@ -62,7 +46,7 @@
],
"rotationTargets": [
{
"waypointRelativePos": 1.62272727272727,
"waypointRelativePos": 0.8,
"rotationDegrees": -135.0
}
],
@@ -71,7 +55,7 @@
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxAcceleration": 3.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,

12
src/main/deploy/pathplanner/paths/start to score left.path
View File

@@ -3,12 +3,12 @@
"waypoints": [
{
"anchor": {
"x": 3.573857868020305,
"x": 3.5922741116751276,
"y": 6.3692588832487305
},
"prevControl": null,
"nextControl": {
"x": 2.7991959463121194,
"x": 2.817612189966942,
"y": 6.260219737341258
},
"isLocked": false,
@@ -16,12 +16,12 @@
},
{
"anchor": {
"x": 3.2515736040609142,
"y": 4.914375634517767
"x": 1.925604060913706,
"y": 5.503695431472081
},
"prevControl": {
"x": 3.44523908448796,
"y": 5.430816915656558
"x": 2.119269541340752,
"y": 6.020136712610872
},
"nextControl": null,
"isLocked": false,

29
src/main/deploy/pathplanner/paths/trough to shot.path
View File

@@ -3,38 +3,43 @@
"waypoints": [
{
"anchor": {
"x": 0.77458883248731,
"y": 5.927269035532995
"x": 0.32339086294416286,
"y": 5.9825177664974625
},
"prevControl": null,
"nextControl": {
"x": 1.7745888324873098,
"y": 5.927269035532995
"x": 0.6364670050761427,
"y": 6.03776649746193
},
"isLocked": false,
"linkedName": "trough"
},
{
"anchor": {
"x": 3.2515736040609142,
"y": 4.914375634517767
"x": 1.925604060913706,
"y": 5.503695431472081
},
"prevControl": {
"x": 2.6254213197969554,
"y": 5.420822335025381
"x": 1.409949238578681,
"y": 5.844395939086294
},
"nextControl": null,
"isLocked": false,
"linkedName": "left close"
}
],
"rotationTargets": [],
"rotationTargets": [
{
"waypointRelativePos": 0.7098520389751093,
"rotationDegrees": 176.84552599629856
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.5,
"maxVelocity": 4.0,
"maxAcceleration": 2.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -50,5 +55,5 @@
"velocity": 0,
"rotation": 178.80651057601818
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

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

@@ -148,8 +148,7 @@ public class RobotContainer {
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
() -> true,
() -> false
() -> true
)
);
@@ -277,8 +276,7 @@ public class RobotContainer {
driver::getLeftY,
driver::getLeftX,
driver::getRightX,
() -> true,
() -> false
() -> true
)
);
shooter.setDefaultCommand(shooter.stop());
@@ -297,8 +295,7 @@ public class RobotContainer {
drivetrain.lockRotationToHub(
driver::getLeftY,
driver::getLeftX,
false,
true
false
)
);
@@ -450,7 +447,7 @@ public class RobotContainer {
.alongWith(
shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
intakePivot.jimmy(0.5),
drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true))
drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false))
.withTimeout(0.5));
NamedCommands.registerCommand("auto shoot",
@@ -459,7 +456,7 @@ public class RobotContainer {
shooter.maintainSpeed(ShooterSpeeds.kHubSpeed),
spindexer.spinToShooter(),
intakePivot.jimmy(0.5),
drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false, true)));
drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false)));
}
public Command getAutonomousCommand() {

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

@@ -199,7 +199,7 @@ public class Drivetrain extends SubsystemBase {
* @return A complete Command structure that performs the specified action
*/
public Command rotateToPose(Pose2d targetPose, boolean rotate180) {
return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180, false)
return lockRotationToSuppliedPose(() -> targetPose, () -> 0, () -> 0, rotate180)
.until(yawRotationController::atSetpoint);
}
@@ -218,16 +218,14 @@ public class Drivetrain extends SubsystemBase {
* @param ySpeed The Y (left/right) translational speed of the robot
* @param rotate180 When false, the front of the robot faces the hub, when true, the back
* of the robot faces the hub
* @param lockXWhenStopped When not being commanded to drive, the drivetrain wheels will make an X shape if this is true
* @return A complete Command structure that performs the specified action
*/
public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180, boolean lockXWhenStopped) {
public Command lockRotationToHub(DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
return lockRotationToSuppliedPose(
Utilities::getHubPose,
xSpeed,
ySpeed,
rotate180,
lockXWhenStopped
rotate180
);
}
@@ -245,10 +243,9 @@ public class Drivetrain extends SubsystemBase {
* @param ySpeed The Y (left/right) translational speed of the robot
* @param rotate180 When false, the front of the robot faces the supplied pose, when true, the back
* of the robot faces the supplied pose
* @param lockXWhenStopped When not being commanded to drive, the drivetrain wheels will make an X shape if this is true
* @return A complete Command structure that performs the specified action
*/
public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180, boolean lockXWhenStopped) {
public Command lockRotationToSuppliedPose(Supplier<Pose2d> poseSupplier, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean rotate180) {
return runOnce(yawRotationController::reset).andThen(
drive(
xSpeed,
@@ -277,8 +274,7 @@ public class Drivetrain extends SubsystemBase {
return outputPower;
},
() -> true,
() -> lockXWhenStopped
() -> true
)
);
}
@@ -292,17 +288,15 @@ public class Drivetrain extends SubsystemBase {
* @param yaw The "yaw" of the tag source relative to the center of the image frame
* @param xSpeed The X (forward/backward) translational speed of the robot
* @param ySpeed The Y (left/right) translational speed of the robot
* @param lockXWhenStopped When not being commanded to drive, the drivetrain wheels will make an X shape if this is true
* @return A complete Command structure that performs the specified action
*/
public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed, boolean lockXWhenStopped) {
public Command lockToYaw(DoubleSupplier yaw, DoubleSupplier xSpeed, DoubleSupplier ySpeed) {
return runOnce(yawRotationController::reset).andThen(
drive(
xSpeed,
ySpeed,
() -> yawRotationController.calculate(yaw.getAsDouble(), 0),
() -> true,
() -> lockXWhenStopped
() -> true
)
);
}
@@ -316,14 +310,14 @@ public class Drivetrain extends SubsystemBase {
}
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative, BooleanSupplier lockXWhenStopped) {
public Command drive(DoubleSupplier xSpeed, DoubleSupplier ySpeed, DoubleSupplier rotation, BooleanSupplier fieldRelative) {
// TODO Specific Alliance code?
return run(() -> {
drive(
-MathUtil.applyDeadband(xSpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(ySpeed.getAsDouble(), OIConstants.kDriveDeadband),
-MathUtil.applyDeadband(rotation.getAsDouble(), OIConstants.kDriveDeadband),
fieldRelative.getAsBoolean(),
lockXWhenStopped.getAsBoolean()
fieldRelative.getAsBoolean()
);
});
}
@@ -375,41 +369,30 @@ public class Drivetrain extends SubsystemBase {
);
}
public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative, boolean lockXWhenStopped) {
if(lockXWhenStopped &&
MathUtil.applyDeadband(xSpeed, OIConstants.kDriveDeadband) == 0 &&
MathUtil.applyDeadband(ySpeed, OIConstants.kDriveDeadband) == 0 &&
MathUtil.applyDeadband(rotation, OIConstants.kDriveDeadband) == 0) {
public void drive(double xSpeed, double ySpeed, double rotation, boolean fieldRelative) {
double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
double xSpeedDelivered = 0;
double ySpeedDelivered = 0;
frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(-45)));
rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(45)));
} else {
double p = Math.sqrt(Math.pow(xSpeed, 2) + Math.pow(ySpeed, 2));
double xSpeedDelivered = 0;
double ySpeedDelivered = 0;
if(p != 0){
xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
}else{
xSpeedDelivered = 0;
ySpeedDelivered = 0;
}
double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative ?
ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered,
//estimator.getEstimatedPosition().getRotation()) :
Rotation2d.fromDegrees(getGyroValue())) :
new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)
);
setModuleStates(swerveModuleStates);
if(p != 0){
xSpeedDelivered = xSpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
ySpeedDelivered = ySpeed * (Math.pow(p, OIConstants.kJoystickExponential) / p) * DrivetrainConstants.kMaxSpeedMetersPerSecond;
}else{
xSpeedDelivered = 0;
ySpeedDelivered = 0;
}
double rotationDelivered = rotation * DrivetrainConstants.kMaxAngularSpeed;
SwerveModuleState[] swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
fieldRelative ?
ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered,
//estimator.getEstimatedPosition().getRotation()) :
Rotation2d.fromDegrees(getGyroValue())) :
new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotationDelivered)
);
setModuleStates(swerveModuleStates);
}
public void driveWithChassisSpeeds(ChassisSpeeds speeds) {

4
src/main/java/frc/robot/subsystems/IntakeRoller.java
View File

@@ -32,8 +32,8 @@ public class IntakeRoller extends SubsystemBase {
public Command runIn() {
return run(() -> {
leftMotor.set(IntakeRollerConstants.kSpeed*0.8);
rightMotor.set(IntakeRollerConstants.kSpeed*0.8);
leftMotor.set(IntakeRollerConstants.kSpeed*0.9);
rightMotor.set(IntakeRollerConstants.kSpeed*0.9);
});
}