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

auto getting there everything else gucci

Browse Source
This commit is contained in:
Tylr-J42
2026-03-21 16:08:26 -04:00
parent b8c376499b
commit cb1c7ba0e3
9 changed files with 291 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
src/main/deploy/pathplanner/autos/left to center.auto
View File

@@ -15,6 +15,24 @@
"data": {
"pathName": "left start to center"
}
},
{
"type": "path",
"data": {
"pathName": "over bump to pile"
}
},
{
"type": "path",
"data": {
"pathName": "back from center"
}
},
{
"type": "named",
"data": {
"name": "auto shoot"
}
}
]
}

63
src/main/deploy/pathplanner/paths/back from center.path Normal file
View File

@@ -0,0 +1,63 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.050842639593909,
"y": 5.715482233502538
},
"prevControl": null,
"nextControl": {
"x": 5.130030456852792,
"y": 5.7523147208121825
},
"isLocked": false,
"linkedName": "over bump"
},
{
"anchor": {
"x": 3.002954314720812,
"y": 5.310324873096447
},
"prevControl": {
"x": 4.052680203045686,
"y": 5.9825177664974625
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 0.10454545454545605,
"rotationDegrees": -115.0
},
{
"waypointRelativePos": 0.7931818181818296,
"rotationDegrees": -115.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -36.158185439808385
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -123.34070734647689
},
"useDefaultConstraints": true
}

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

@@ -8,111 +8,77 @@
},
"prevControl": null,
"nextControl": {
"x": 3.1594923857868027,
"y": 6.424507614213196
"x": 2.9108730964467004,
"y": 6.498172588832488
},
"isLocked": false,
"linkedName": "start left"
},
{
"anchor": {
"x": 2.634629441624366,
"y": 5.558944162436549
"x": 2.5885888324873094,
"y": 5.936477157360406
},
"prevControl": {
"x": 2.4136345177664977,
"y": 5.908852791878173
"x": 2.3675939086294413,
"y": 6.28638578680203
},
"nextControl": {
"x": 2.895419769065523,
"y": 5.146026143988051
"x": 2.8493791599284664,
"y": 5.523559138911907
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 4.522294416243654,
"y": 5.558944162436549
"x": 4.577543147208122,
"y": 5.715482233502538
},
"prevControl": {
"x": 3.5095162657293457,
"y": 5.533304209258972
"x": 3.565840773084476,
"y": 5.768729726877464
},
"nextControl": {
"x": 6.704619289340102,
"y": 5.614192893401015
"x": 5.102406091370558,
"y": 5.687857868020306
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.478101522849547,
"y": 7.244030456855094
"x": 6.050842639593909,
"y": 5.715482233502538
},
"prevControl": {
"x": 6.765542310662505,
"y": 7.465169522706244
},
"nextControl": {
"x": 8.012172588839395,
"y": 7.078284263961693
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.671472081218274,
"y": 4.7670456852791885
},
"prevControl": {
"x": 7.726720812182741,
"y": 7.639979695431472
"x": 5.525979695431473,
"y": 5.807563451776649
},
"nextControl": null,
"isLocked": false,
"linkedName": null
"linkedName": "over bump"
}
],
"rotationTargets": [
{
"waypointRelativePos": 1.5636363636363326,
"rotationDegrees": -45.0
},
{
"waypointRelativePos": 3.0318181818182266,
"rotationDegrees": -90.0
}
],
"constraintZones": [
{
"name": "Constraints Zone",
"minWaypointRelativePos": 2.6208291203235685,
"maxWaypointRelativePos": 4.0,
"constraints": {
"maxVelocity": 1.0,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
}
"waypointRelativePos": 1.62272727272727,
"rotationDegrees": -135.0
}
],
"constraintZones": [],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Intake Start",
"waypointRelativePos": 0,
"waypointRelativePos": 2.1,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.5,
"maxVelocity": 3.0,
"maxAcceleration": 2.0,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
@@ -120,7 +86,7 @@
},
"goalEndState": {
"velocity": 0,
"rotation": -89.09061955080092
"rotation": -123.34070734647689
},
"reversed": false,
"folder": null,
@@ -128,5 +94,5 @@
"velocity": 0,
"rotation": -90.0
},
"useDefaultConstraints": true
"useDefaultConstraints": false
}

112
src/main/deploy/pathplanner/paths/over bump to pile.path Normal file
View File

@@ -0,0 +1,112 @@
{
"version": "2025.0",
"waypoints": [
{
"anchor": {
"x": 6.050842639593909,
"y": 5.715482233502538
},
"prevControl": null,
"nextControl": {
"x": 6.520456852791878,
"y": 6.406091370558377
},
"isLocked": false,
"linkedName": "over bump"
},
{
"anchor": {
"x": 7.017695431472081,
"y": 7.216406091370559
},
"prevControl": {
"x": 6.017695431472081,
"y": 7.216406091370559
},
"nextControl": {
"x": 8.017695431472081,
"y": 7.216406091370559
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 7.883258883248731,
"y": 4.831502538071066
},
"prevControl": {
"x": 7.941269035532995,
"y": 6.042370558375635
},
"nextControl": {
"x": 7.825248730964466,
"y": 3.620634517766498
},
"isLocked": false,
"linkedName": null
},
{
"anchor": {
"x": 6.050842639593909,
"y": 5.715482233502538
},
"prevControl": {
"x": 6.99789847715736,
"y": 5.183713197969544
},
"nextControl": null,
"isLocked": false,
"linkedName": null
}
],
"rotationTargets": [
{
"waypointRelativePos": 1.0045454545454569,
"rotationDegrees": -115.0
}
],
"constraintZones": [
{
"name": "Constraints Zone",
"minWaypointRelativePos": 1.1243680485338854,
"maxWaypointRelativePos": 2.0,
"constraints": {
"maxVelocity": 1.0,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
}
}
],
"pointTowardsZones": [],
"eventMarkers": [
{
"name": "Intake Start",
"waypointRelativePos": 0.7886754297269942,
"endWaypointRelativePos": null,
"command": null
}
],
"globalConstraints": {
"maxVelocity": 2.0,
"maxAcceleration": 1.5,
"maxAngularVelocity": 540.0,
"maxAngularAcceleration": 720.0,
"nominalVoltage": 12.0,
"unlimited": false
},
"goalEndState": {
"velocity": 0,
"rotation": -120.34324888423971
},
"reversed": false,
"folder": null,
"idealStartingState": {
"velocity": 0,
"rotation": -123.34070734647689
},
"useDefaultConstraints": true
}

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

@@ -84,7 +84,7 @@ public class RobotContainer {
resetOdometryToVisualPose = false;
//vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
vision.addPoseEstimateConsumer((vp) -> {
Logger.recordOutput(
"Vision/" + vp.cameraName() + "/Pose",
@@ -243,7 +243,7 @@ public class RobotContainer {
// Useful for testing PID and FF responses of the shooter
// You need to have graphs up of the logged data to make sure the response is correct
secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
//secondary.a().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
secondary.b().whileTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
// Useful for testing PID and FF responses of the intake pivot
@@ -276,6 +276,7 @@ public class RobotContainer {
driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
driver.y().whileTrue(drivetrain.zeroHeading());
driver.x().whileTrue(drivetrain.setX());
driver.leftTrigger().whileTrue(
drivetrain.lockRotationToHub(
@@ -320,10 +321,12 @@ public class RobotContainer {
//40 good for feeding
//secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
//30 degrees good for shooter far near outpost
secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
secondary.rightBumper().whileTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
//10 degrees good for shooting ~33in away from hub
hood.setDefaultCommand(hood.trackToAngle(() -> {
hood.setDefaultCommand(hood.trackToAnglePoseBased(drivetrain, shooter));
/*hood.setDefaultCommand(hood.trackToAngle(() -> {
Pose2d drivetrainPose = drivetrain.getPose();
Pose2d hubPose = Utilities.getHubPose();
@@ -345,7 +348,7 @@ public class RobotContainer {
} else {
return 0;
}
}));
}));*/
new Trigger(() -> MathUtil.isNear(
shooter.getTargetSpeeds().isEmpty() ? 0 : shooter.getTargetSpeeds().get().getSpeedRPM(),
@@ -399,7 +402,8 @@ public class RobotContainer {
// NamedCommands.registerCommand("Intake Start", intakeRoller.runIn());
new EventTrigger("Intake Start")
.onTrue(intakeRoller.runIn());
.onTrue(
intakeRoller.runIn());
NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop());
@@ -415,6 +419,8 @@ public class RobotContainer {
hood.trackToAngle(() -> Units.degreesToRadians(10)))
).withTimeout(3).andThen(spindexer.instantaneousStop()));
NamedCommands.registerCommand("auto shoot", hood.trackToAnglePoseBased(drivetrain, shooter));
}
public Command getAutonomousCommand() {

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

@@ -71,22 +71,22 @@ public class HoodConstants {
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 40)),
Double.valueOf(Units.degreesToRadians(10)));
Double.valueOf(Units.degreesToRadians(9.5)));
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 60)),
Double.valueOf(Units.degreesToRadians(13)));
Double.valueOf(Units.degreesToRadians(12.5)));
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 80)),
Double.valueOf(Units.degreesToRadians(17)));
Double.valueOf(Units.degreesToRadians(16.25)));
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 100)),
Double.valueOf(Units.degreesToRadians(21)));
Double.valueOf(Units.degreesToRadians(20.5)));
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 120)),
Double.valueOf(Units.degreesToRadians(24)));
Double.valueOf(Units.degreesToRadians(23.5)));
}
}

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

@@ -5,8 +5,10 @@ import com.revrobotics.spark.FeedbackSensor;
import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.util.Units;
public class ShooterConstants {
public enum ShooterSpeeds {
kHubSpeed(3000.0),
@@ -41,16 +43,16 @@ public class ShooterConstants {
public static final boolean kLeftShooterMotorInverted = true;
public static final boolean kRightShooterMotorInverted = false;
public static final double kLeftP = 0.04;//0.01;//0.001;
public static final double kLeftP = 0.1;//0.01;//0.001;
public static final double kLeftI = 0;
public static final double kLeftD = 0;//1.8;
public static final double kLeftD = 0;//0.1;//1.8;
public static final double kLeftS = 0;
public static final double kLeftV = 0.00129;
public static final double kLeftA = 0;
public static final double kRightP = 0.04;//0.001;//0.001;
public static final double kRightP = 0.1;//0.001;//0.001;
public static final double kRightI = 0;
public static final double kRightD = 0;
public static final double kRightD = 0;//0.1;
public static final double kRightS = 0;
public static final double kRightV = 0.00125;
public static final double kRightA = 0;
@@ -78,12 +80,12 @@ public class ShooterConstants {
kLeftMotorConfig.absoluteEncoder
.positionConversionFactor(1)
.velocityConversionFactor(60)
.averageDepth(16); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
.averageDepth(8); // VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
kLeftMotorConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kLeftP, kLeftI, kLeftD, ClosedLoopSlot.kSlot0)
.outputRange(-1, 1)
//.allowedClosedLoopError(10.0, ClosedLoopSlot.kSlot0)
.allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
.feedForward
.sva(kLeftS, kLeftV, kLeftA, ClosedLoopSlot.kSlot0);
@@ -94,13 +96,13 @@ public class ShooterConstants {
kRightMotorConfig.absoluteEncoder
.positionConversionFactor(1)
.velocityConversionFactor(60)
.averageDepth(16)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
.averageDepth(8)// VERY IMPORTANT FOR RESPONSE OF FLYWHEEL DEFAULTS ARE DOGWATER
.inverted(true);
kRightMotorConfig.closedLoop
.feedbackSensor(FeedbackSensor.kAbsoluteEncoder)
.pid(kRightP, kRightI, kRightD)
.outputRange(-1, 1)
//.allowedClosedLoopError(10.0, ClosedLoopSlot.kSlot0)
.allowedClosedLoopError(25.0, ClosedLoopSlot.kSlot0)
.feedForward
.sva(kRightS, kRightV, kRightA, ClosedLoopSlot.kSlot0);
}

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

@@ -1,5 +1,6 @@
package frc.robot.subsystems;
import java.util.Optional;
import java.util.function.DoubleSupplier;
import org.littletonrobotics.junction.Logger;
@@ -13,6 +14,8 @@ import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
@@ -20,6 +23,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.button.Trigger;
import frc.robot.constants.HoodConstants;
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
import frc.robot.utilities.Utilities;
public class Hood extends SubsystemBase {
private SparkMax motor;
@@ -92,6 +97,32 @@ public class Hood extends SubsystemBase {
Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage());
}
public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) {
return trackToAngle(() -> {
Pose2d drivetrainPose = drivetrain.getPose();
Pose2d hubPose = Utilities.getHubPose();
double distance = drivetrainPose.getTranslation()
.getDistance(hubPose.getTranslation());
Logger.recordOutput("Hood/DistanceToHub", distance);
Optional<ShooterSpeeds> currentSpeeds = shooter.getTargetSpeeds();
if(currentSpeeds.isPresent()) {
InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get());
if(map != null) {
return MathUtil.clamp(map.get(distance), 0, 40);
} else {
return 0;
}
} else {
return 0;
}
});
}
public Command trackToAngle(DoubleSupplier degreeAngleSupplier) {
return run(() -> {
currentTargetDegrees = degreeAngleSupplier.getAsDouble();

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

@@ -9,11 +9,13 @@ import com.revrobotics.AbsoluteEncoder;
import com.revrobotics.PersistMode;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.ResetMode;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.SparkClosedLoopController;
import com.revrobotics.spark.SparkMax;
import com.revrobotics.spark.SparkBase.ControlType;
import com.revrobotics.spark.SparkLowLevel.MotorType;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -33,6 +35,8 @@ public class Shooter extends SubsystemBase {
private SparkClosedLoopController rightClosedLoopController;
private ShooterSpeeds targetSpeeds;
private SimpleMotorFeedforward shooterFFLeft;
private SimpleMotorFeedforward shooterFFRight;
public Shooter() {
leftMotor = new SparkMax(ShooterConstants.kLeftShooterMotorCANID, MotorType.kBrushless);
@@ -60,6 +64,9 @@ public class Shooter extends SubsystemBase {
targetSpeeds = null;
rightRelative = rightMotor.getEncoder();
shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA);
shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA);
}
@Override
@@ -116,12 +123,16 @@ public class Shooter extends SubsystemBase {
} else {
leftClosedLoopController.setSetpoint(
targetSpeeds.getSpeedRPM(),
ControlType.kVelocity
ControlType.kVelocity,
ClosedLoopSlot.kSlot0,
shooterFFLeft.calculate(targetSpeeds.getSpeedRPM())
);
rightClosedLoopController.setSetpoint(
targetSpeeds.getSpeedRPM(),
ControlType.kVelocity
ControlType.kVelocity,
ClosedLoopSlot.kSlot0,
shooterFFRight.calculate(targetSpeeds.getSpeedRPM())
);
}
});