Between comps

build.gradle

@@ -74,6 +74,8 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    implementation 'org.apache.commons:commons-math3:3.6.1'
 }
 
 test {

src/main/java/frc/robot/Robot.java

@@ -4,31 +4,41 @@
 
 package frc.robot;
 
-import static frc.robot.RobotContainer.shooterState;
-import static frc.robot.config.VisionConfig.photonPoseEstimatorForward;
-import static frc.robot.config.VisionConfig.result;
-
 import edu.wpi.first.epilogue.Epilogue;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.networktables.StructPublisher;
 import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.commands.DrivetrainCommand;
 import frc.robot.commands.FlywheelCommand;
+import frc.robot.commands.KickerCommand;
 import frc.robot.config.TunerConstants;
-import frc.robot.config.VisionConfig;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
-import java.util.Optional;
-import org.photonvision.EstimatedRobotPose;
+import frc.robot.subsystems.Hopper;
+import frc.robot.subsystems.Intake;
 
 @Logged
 public class Robot extends TimedRobot {
   private Command m_autonomousCommand;
   private final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
-
+  // last time we injected a synthetic vision measurement (seconds, FPGA time)
+  private double m_lastSimVisionTime = 0.0;
+  // whether we've injected a one-time offset vision measurement for testing
+  private boolean m_injectedOffset = false;
+  private Pose2d robotPose = new Pose2d();
   private final RobotContainer m_robotContainer;
+  StructPublisher<Pose2d> publisher =
+      NetworkTableInstance.getDefault().getStructTopic("Robot Pose", Pose2d.struct).publish();
+  StructArrayPublisher<Pose2d> arrayPublisher =
+      NetworkTableInstance.getDefault().getStructArrayTopic("MyPoseArray", Pose2d.struct).publish();
 
   public Robot() {
     m_robotContainer = new RobotContainer();
@@ -40,21 +50,22 @@ public Robot() {
   @Override
   public void robotPeriodic() {
     CommandScheduler.getInstance().run();
-    Optional<EstimatedRobotPose> visionEst =
-        VisionConfig.photonPoseEstimatorForward.estimateCoprocMultiTagPose(result);
-    if (visionEst.isEmpty()) {
-      visionEst = VisionConfig.photonPoseEstimatorForward.estimateLowestAmbiguityPose(result);
-    } else {
-      drivetrain.addVisionMeasurement(
-          photonPoseEstimatorForward
-              .estimateAverageBestTargetsPose(result)
-              .get()
-              .estimatedPose
-              .toPose2d(),
-          visionEst.get().timestampSeconds);
+    SmartDashboard.putString("Flywheel State", FlywheelCommand.flywheelState);
+    SmartDashboard.putString("Drivetrain State", DrivetrainCommand.drivetrainState);
+    SmartDashboard.putString("Hopper State", Hopper.hopperState);
+    SmartDashboard.putString("Intake State", Intake.intakeState);
+    SmartDashboard.putString("Kicker State", KickerCommand.kickerState);
+    try {
+      var pose = drivetrain.getState().Pose;
+      SmartDashboard.putNumber("OdometryX", pose.getX());
+      SmartDashboard.putNumber("OdometryY", pose.getY());
+      SmartDashboard.putNumber("OdometryRotDeg", pose.getRotation().getDegrees());
+      SmartDashboard.putNumber("SimVisionLastInject", m_lastSimVisionTime);
+    } catch (Exception ignored) {
     }
-    SmartDashboard.putString("shoot-state", shooterState);
-    SmartDashboard.putString("shoot-on", FlywheelCommand.isOn);
+    robotPose = drivetrain.getState().Pose;
+    publisher.set(robotPose);
+    arrayPublisher.set(new Pose2d[] {robotPose});
   }
 
   @Override
@@ -109,5 +120,24 @@ public void testExit() {}
   public void simulationInit() {}
 
   @Override
-  public void simulationPeriodic() {}
+  public void simulationPeriodic() {
+    double now = Timer.getFPGATimestamp();
+    if (now - m_lastSimVisionTime >= 0.1) {
+      m_lastSimVisionTime = now;
+      try {
+        var truePose = drivetrain.getState().Pose;
+        // After 3 seconds, inject a single offset measurement (+1m X) to observe correction
+        if (!m_injectedOffset && now > 3.0) {
+          Pose2d offsetPose =
+              new Pose2d(truePose.getX() + 1.0, truePose.getY(), truePose.getRotation());
+          drivetrain.addVisionMeasurement(offsetPose, now);
+          m_injectedOffset = true;
+        } else {
+          drivetrain.addVisionMeasurement(truePose, now);
+        }
+      } catch (Exception e) {
+        DriverStation.reportError("Sim vision injection failed: " + e.getMessage(), false);
+      }
+    }
+  }
 }

src/main/java/frc/robot/RobotContainer.java

@@ -5,7 +5,6 @@
 package frc.robot;
 
 import com.ctre.phoenix6.hardware.Pigeon2;
-import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.FollowPathCommand;
@@ -28,11 +27,9 @@ public class RobotContainer {
   private Flywheel flywheel = new Flywheel();
   private CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
   private Hopper hopper = new Hopper();
-  private Climb climb = new Climb();
   private Intake intake = new Intake();
   private Kicker kicker = new Kicker();
   private CommandXboxController controller = new CommandXboxController(1);
-  private SwerveDriveState driveState = new SwerveDriveState();
   private final SendableChooser<Command> autoChooser;
   public static boolean isExtended = false;
   public static String shooterState = "none";
@@ -176,8 +173,6 @@ private void configureBindings() {
                 .alongWith(Commands.run(() -> shooterState = "TRENCH")));
     // Down Plus = Zero hopper
     controller.povDown().onTrue(Commands.runOnce(() -> hopper.zero(), hopper));
-    // Up Plus = Defense Hopper
-    controller.povUp().toggleOnTrue(Commands.run(() -> hopper.retractDirectional(0.8), hopper));
   }
 
   public Command getAutonomousCommand() {

src/main/java/frc/robot/commands/DrivetrainCommand.java

@@ -5,11 +5,15 @@
 import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 import edu.wpi.first.epilogue.Logged;
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.config.TunerConstants;
 import frc.robot.helpers.ApplyModuleStates;
+import frc.robot.helpers.ShotCalculator;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import java.util.function.DoubleSupplier;
 
@@ -18,14 +22,16 @@ public class DrivetrainCommand extends Command {
   public static enum Position {
     TELEOP,
     STILL_SHOT,
-    SOTM
+    SOTM,
+    AUTO_ALIGN_HUB
   }
 
   private CommandSwerveDrivetrain subsystem;
   private DrivetrainCommand.Position pose;
   private DoubleSupplier leftY;
   private DoubleSupplier leftX;
   private DoubleSupplier rightX;
+  public static String drivetrainState = "Stopped";
 
   private final double MaxSpeed =
       TunerConstants.kSpeedAt12Volts.in(MetersPerSecond); // kSpeedAt12Volts desired top speed
@@ -48,6 +54,8 @@ public static enum Position {
   };
   private final ApplyModuleStates applyRequest = new ApplyModuleStates();
   private final SwerveRequest.SwerveDriveBrake brakeRequest = new SwerveRequest.SwerveDriveBrake();
+  private final ProfiledPIDController autoAlignPidController;
+  private Rotation2d m_targetAngle = Rotation2d.kZero;
 
   public DrivetrainCommand(
       CommandSwerveDrivetrain subsystem,
@@ -60,10 +68,38 @@ public DrivetrainCommand(
     this.leftX = leftX;
     this.leftY = leftY;
     this.rightX = rightX;
+    double alignP = 1;
+    double alignI = 0;
+    double alignD = 0;
+    this.autoAlignPidController =
+        new ProfiledPIDController(
+            alignP,
+            alignI,
+            alignD,
+            new TrapezoidProfile.Constraints(MaxAngularRate, MaxAngularRate / 0.2));
+    this.autoAlignPidController.enableContinuousInput(
+        -Math.PI, Math.PI); // Swerve angles are continuous (-180 to 180 deg)
+    // Don't compute hub-facing target in the constructor (DriverStation alliance may be
+    // unavailable during initialization). Initialize to current heading; execute() will
+    // recompute the actual hub-facing target each loop.
+    this.m_targetAngle = subsystem.getState().Pose.getRotation();
 
     addRequirements(subsystem);
   }
 
+  public double getAutoAlignRotationalOutput() {
+    Rotation2d currentAngle = subsystem.getState().Pose.getRotation();
+    double current = currentAngle.getRadians();
+    double target = m_targetAngle.getRadians();
+    double output = autoAlignPidController.calculate(current, target);
+    if (output > MaxAngularRate) {
+      output = MaxAngularRate;
+    } else if (output < -MaxAngularRate) {
+      output = -MaxAngularRate;
+    }
+    return output;
+  }
+
   @Override
   public void execute() {
     switch (pose) {
@@ -78,20 +114,48 @@ public void execute() {
                 .withRotationalRate(
                     -rightX.getAsDouble()
                         * MaxAngularRate)); // Drive counterclockwise with negative X
+        drivetrainState = "Teleop Drive";
         break;
 
       // (Needs check) Mode for when shots are from a still position
       case STILL_SHOT:
         // make X with wheels, set wheels to brake mode
         applyRequest.ModuleStates = states;
         subsystem.setControl(applyRequest);
-        System.out.println("Drivetrain: Still Shot Configuration");
+        drivetrainState = "Still Shot";
         break;
 
       // (Incomplete) Mode for moving shots
       case SOTM:
         // shoot on the move, reference Mechanical Advantage build log
-        System.out.println("Drivetrain: SOTM Drive");
+        drivetrainState = "Shoot on the Move Drive";
+        break;
+
+      case AUTO_ALIGN_HUB:
+        // Recompute the desired heading toward the hub each loop
+        m_targetAngle =
+            ShotCalculator.getRotationTowardsHub(
+                ShotCalculator.calculateHubPosition(), subsystem.getState().Pose.getTranslation());
+        double currentRad = subsystem.getState().Pose.getRotation().getRadians();
+        double targetRad = m_targetAngle.getRadians();
+        // Normalize error to [-pi, pi]
+        double error =
+            Math.atan2(Math.sin(targetRad - currentRad), Math.cos(targetRad - currentRad));
+        double absError = Math.abs(error);
+        double angleTolerance = Math.toRadians(1.0); // stop within 1 degree
+
+        double rotOutput = getAutoAlignRotationalOutput();
+
+        if (absError < angleTolerance) {
+          // Aligned: stop rotating (and hold position)
+          subsystem.setControl(drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(0.0));
+        } else {
+          // Not aligned: apply rotational output
+          System.out.println("new rotational output: " + rotOutput);
+          subsystem.setControl(
+              drive.withVelocityX(0.0).withVelocityY(0.0).withRotationalRate(rotOutput));
+        }
+        drivetrainState = "Auto Align";
         break;
 
       default:
@@ -104,6 +168,22 @@ public void end(boolean interrupted) {}
 
   @Override
   public boolean isFinished() {
-    return false;
+    if (pose != Position.AUTO_ALIGN_HUB) {
+      return false;
+    }
+
+    Translation2d hubPos = ShotCalculator.calculateHubPosition();
+
+    Translation2d currentTrans = subsystem.getState().Pose.getTranslation();
+    if (currentTrans == null) {
+      return false;
+    }
+
+    Rotation2d target = ShotCalculator.getRotationTowardsHub(hubPos, currentTrans);
+    double current = subsystem.getState().Pose.getRotation().getRadians();
+    double targetRad = target.getRadians();
+    double err = Math.atan2(Math.sin(targetRad - current), Math.cos(targetRad - current));
+    double angleTolerance = Math.toRadians(1.0); // 1 degree
+    return Math.abs(err) < angleTolerance;
   }
 }