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

Auto Align, Auto Hood Angle for 3000 RPM, Camera Poses Working

Browse Source
This commit is contained in:
Bradley Bickford
2026-03-17 19:01:02 -04:00
parent 47606ade0f
commit 048c7158ee
7 changed files with 70 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -6,6 +6,7 @@ package frc.robot;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import java.util.Optional;
import java.util.OptionalDouble;
import org.littletonrobotics.junction.Logger;
@@ -13,8 +14,12 @@ import org.littletonrobotics.junction.Logger;
import com.pathplanner.lib.auto.AutoBuilder;
import com.pathplanner.lib.auto.NamedCommands;
import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -59,6 +64,8 @@ public class RobotContainer {
private Timer shiftTimer;
private boolean resetOdometryToVisualPose;
public RobotContainer() {
vision = new PhotonVision();
drivetrain = new Drivetrain(null);
@@ -70,6 +77,7 @@ public class RobotContainer {
//climber = new Climber();
configureNamedCommands();
resetOdometryToVisualPose = false;
vision.addPoseEstimateConsumer(drivetrain::consumeVisualPose);
vision.addPoseEstimateConsumer((vp) -> {
@@ -78,6 +86,12 @@ public class RobotContainer {
vp.visualPose()
);
});
vision.addPoseEstimateConsumer((vp) -> {
if(resetOdometryToVisualPose) {
drivetrain.resetOdometry(vp.visualPose());
resetOdometryToVisualPose = false;
}
});
driver = new CommandXboxController(OIConstants.kDriverControllerPort);
@@ -93,7 +107,6 @@ public class RobotContainer {
if(AutoConstants.kAutoConfigOk) {
autoChooser = AutoBuilder.buildAutoChooser();
SmartDashboard.putData("Auto Chooser", autoChooser);
}
}
@@ -255,6 +268,7 @@ public class RobotContainer {
intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY()));
driver.a().onTrue(new InstantCommand(() -> resetOdometryToVisualPose = true));
driver.y().whileTrue(drivetrain.zeroHeading());
driver.leftTrigger().whileTrue(
@@ -285,36 +299,38 @@ public class RobotContainer {
secondary.a().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed));
secondary.x().toggleOnTrue(shooter.maintainSpeed(ShooterSpeeds.kFeedSpeed));
secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
//hood.setDefaultCommand(hood.trackToAngle(() -> Units.degreesToRadians(MathUtil.clamp(hoodAngle, 0, 40))));
//secondary.y().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(40)));
//40 good for feeding
secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
//secondary.b().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(30)));
//30 degrees good for shooter far near outpost
secondary.rightBumper().onTrue(hood.trackToAngle(() -> Units.degreesToRadians(10)));
//10 degrees good for shooting ~33in away from hub
/*
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);
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 {
// 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
);
return 0;
}
}));*/
} else {
return 0;
}
}));
}
private void configureNamedCommands() {

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

@@ -24,8 +24,6 @@ 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;

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

@@ -38,9 +38,10 @@ public class DrivetrainConstants {
public static final boolean kGyroReversed = true;
// TODO Hold over from 2025, adjust?
public static final double kHeadingP = .1;
public static final double kHeadingP = .65;
public static final double kXTranslationP = .5;
public static final double kYTranslationP = .5;
public static final double kYawPIDTolerance = Units.degreesToRadians(1);
// TODO How much do we trust gyro and encoders vs vision estimates.
// NOTE: Bigger values indicate LESS trust. Generally all three values for a given matrix should be the same

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

@@ -5,6 +5,7 @@ import java.io.File;
import java.io.FileReader;
import java.io.IOException;
import java.nio.file.Path;
import java.util.Map;
import com.revrobotics.spark.ClosedLoopSlot;
import com.revrobotics.spark.FeedbackSensor;
@@ -12,7 +13,9 @@ import com.revrobotics.spark.config.SparkMaxConfig;
import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Filesystem;
import frc.robot.constants.ShooterConstants.ShooterSpeeds;
public class HoodConstants {
// TODO Real Values
@@ -43,9 +46,9 @@ public class HoodConstants {
public static final IdleMode kIdleMode = IdleMode.kBrake;
// TODO This needs to be filled in from some source
public static final InterpolatingDoubleTreeMap kDistanceToAngle = new InterpolatingDoubleTreeMap();
public static final Map<ShooterSpeeds, InterpolatingDoubleTreeMap> kHoodInterpolators = Map.of(
ShooterSpeeds.kHubSpeed, 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();
@@ -66,27 +69,24 @@ public class HoodConstants {
.feedForward
.sva(kS, kV, kA);
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 40)),
Double.valueOf(Units.degreesToRadians(10)));
File interpolatorFile = Path.of(
Filesystem.getDeployDirectory().getAbsolutePath().toString(),
"interpolatorData.csv"
).toFile();
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 60)),
Double.valueOf(Units.degreesToRadians(13)));
if(interpolatorFile.exists()) {
try (BufferedReader reader = new BufferedReader(new FileReader(interpolatorFile))) {
reader.lines().forEach((s) -> {
if(s.trim() != "") { //Empty or whitespace line protection
String[] lineSplit = s.split(",");
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 80)),
Double.valueOf(Units.degreesToRadians(17)));
kDistanceToAngle.put(
Double.valueOf(lineSplit[0].replace("\"", "")),
Double.valueOf(lineSplit[1].replace("\"", ""))
);
}
});
} catch (IOException e) {
// This condition is never reached because of the if exists line above
}
}
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 100)),
Double.valueOf(Units.degreesToRadians(21)));
kHoodInterpolators.get(ShooterSpeeds.kHubSpeed).put(
Double.valueOf(Units.inchesToMeters(22.2 + 120)),
Double.valueOf(Units.degreesToRadians(24)));
}
}

4
src/main/java/frc/robot/constants/PhotonConstants.java
View File

@@ -18,7 +18,7 @@ public class PhotonConstants {
Units.inchesToMeters(28.5),
new Rotation3d(
Units.degreesToRadians(0),
Units.degreesToRadians(24.0),
Units.degreesToRadians(-24.0),
Units.degreesToRadians(30.0)
)
);
@@ -28,7 +28,7 @@ public class PhotonConstants {
Units.inchesToMeters(28.5),
new Rotation3d(
Units.degreesToRadians(0.0),
Units.degreesToRadians(24.0),
Units.degreesToRadians(-24.0),
Units.degreesToRadians(-10.0)
)
);

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

@@ -82,12 +82,12 @@ public class Drivetrain extends SubsystemBase {
gyro = new AHRS(NavXComType.kMXP_SPI);
yawRotationController = new PIDController(
AutoConstants.kPThetaController,
DrivetrainConstants.kHeadingP,
0,
0
);
yawRotationController.enableContinuousInput(-Math.PI, Math.PI);
yawRotationController.setTolerance(AutoConstants.kYawPIDTolerance);
yawRotationController.setTolerance(DrivetrainConstants.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(
@@ -262,10 +262,17 @@ public class Drivetrain extends SubsystemBase {
targetRotation = targetRotation.rotateBy(Rotation2d.k180deg);
}
return yawRotationController.calculate(
Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians());
Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians());
double outputPower = -yawRotationController.calculate(
getHeading().getRadians(),
targetRotation.getRadians()
);
Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower);
return outputPower;
},
() -> true
)

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

@@ -13,6 +13,7 @@ import org.photonvision.PhotonPoseEstimator.PoseStrategy;
import org.photonvision.targeting.PhotonPipelineResult;
import org.photonvision.targeting.PhotonTrackedTarget;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Pose3d;
import edu.wpi.first.math.geometry.Transform3d;
import edu.wpi.first.wpilibj2.command.SubsystemBase;