From 048c7158eef96080d79f974ae0259288b0dd487d Mon Sep 17 00:00:00 2001 From: Bradley Bickford Date: Tue, 17 Mar 2026 19:01:02 -0400 Subject: [PATCH] Auto Align, Auto Hood Angle for 3000 RPM, Camera Poses Working --- src/main/java/frc/robot/RobotContainer.java | 50 ++++++++++++------- .../frc/robot/constants/AutoConstants.java | 2 - .../robot/constants/DrivetrainConstants.java | 3 +- .../frc/robot/constants/HoodConstants.java | 44 ++++++++-------- .../frc/robot/constants/PhotonConstants.java | 4 +- .../java/frc/robot/subsystems/Drivetrain.java | 13 +++-- .../frc/robot/subsystems/PhotonVision.java | 1 + 7 files changed, 70 insertions(+), 47 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d2f3bfe..8c84288 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -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()); + + Logger.recordOutput("Hood/DistanceToHub", distance); + + Optional currentSpeeds = shooter.getTargetSpeeds(); - if(HoodConstants.kUseInterpolatorForAngle) { - return HoodConstants.kDistanceToAngle.get(distance); + 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 { - // TODO The average actual speeds isn't really 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; } - }));*/ + })); } private void configureNamedCommands() { diff --git a/src/main/java/frc/robot/constants/AutoConstants.java b/src/main/java/frc/robot/constants/AutoConstants.java index 5e77fa9..2d80656 100644 --- a/src/main/java/frc/robot/constants/AutoConstants.java +++ b/src/main/java/frc/robot/constants/AutoConstants.java @@ -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; diff --git a/src/main/java/frc/robot/constants/DrivetrainConstants.java b/src/main/java/frc/robot/constants/DrivetrainConstants.java index 76ff74e..6b0d6d4 100644 --- a/src/main/java/frc/robot/constants/DrivetrainConstants.java +++ b/src/main/java/frc/robot/constants/DrivetrainConstants.java @@ -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 diff --git a/src/main/java/frc/robot/constants/HoodConstants.java b/src/main/java/frc/robot/constants/HoodConstants.java index 30a4ab6..17c713d 100644 --- a/src/main/java/frc/robot/constants/HoodConstants.java +++ b/src/main/java/frc/robot/constants/HoodConstants.java @@ -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 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))); } } diff --git a/src/main/java/frc/robot/constants/PhotonConstants.java b/src/main/java/frc/robot/constants/PhotonConstants.java index 21897c0..097a42d 100644 --- a/src/main/java/frc/robot/constants/PhotonConstants.java +++ b/src/main/java/frc/robot/constants/PhotonConstants.java @@ -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) ) ); diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index c2b2642..4d22935 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -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 ) diff --git a/src/main/java/frc/robot/subsystems/PhotonVision.java b/src/main/java/frc/robot/subsystems/PhotonVision.java index 9e316a7..25cc7d9 100644 --- a/src/main/java/frc/robot/subsystems/PhotonVision.java +++ b/src/main/java/frc/robot/subsystems/PhotonVision.java @@ -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;