Autos

src/main/java/frc4388/robot/RobotContainer.java

@@ -10,7 +10,9 @@ package frc4388.robot;
 import java.io.File;
 
 import com.pathplanner.lib.commands.PathPlannerAuto;
+import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -353,16 +355,21 @@ public class RobotContainer {
 
         autoChooser.onChange((filename) -> {
             autoChooserUpdated = true;
-            if (filename.equals("Taxi")) {
-                autoCommand = new SequentialCommandGroup(
-                    new MoveForTimeCommand(m_robotSwerveDrive, 
-                        new Translation2d(0, -1), 
-                        new Translation2d(), 1000, true
-                ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
-            } else {
+            // if (filename.equals("Taxi")) {
+            //     autoCommand = new SequentialCommandGroup(
+            //         new MoveForTimeCommand(m_robotSwerveDrive, 
+            //             new Translation2d(0, -1), 
+            //             new Translation2d(), 1000, true
+            //     ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
+            // } else {
                 autoCommand = new PathPlannerAuto(filename);
-            }
+            // }
             System.out.println("Robot Auto Changed " + filename);
+
+            //----
+            PathPlannerAuto auto = new PathPlannerAuto(filename);
+            m_robotSwerveDrive.setInitalPose(auto.getStartingPose());
+            //-----
         });
         SmartDashboard.putData(autoChooser);
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.config.PIDConstants;
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
 import com.pathplanner.lib.util.PathPlannerLogging;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -21,6 +22,7 @@ import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants.AutoConstants;
 import frc4388.robot.subsystems.vision.Vision;
@@ -50,7 +52,8 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     public double lastOdomSpeed;
 
-    public Pose2d initalPose2d = new Pose2d(); // Mira aqui
+    public Pose2d initalPose2d = new Pose2d();
+    
 
 
     public double rotTarget = 0.0;
@@ -136,7 +139,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public void setOdoPose(Pose2d pose) {
         if (pose == null) return;
         initalPose2d = pose;
-        io.resetPose(pose);
+        io.resetPose(initalPose2d);
+    }
+
+    public void setInitalPose(Pose2d startingAutoPose){
+        initalPose2d = startingAutoPose;
     }
     // MIRA public void setOdoPose(Pose2d pose) {
     //     if (pose == null) return;

src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java

@@ -10,6 +10,7 @@ import com.ctre.phoenix6.swerve.SwerveDrivetrain;
 import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
 
@@ -40,6 +41,20 @@ public class SwerveReal implements SwerveIO {
         swerveDriveTrain.tareEverything();
     }
 
+    @Override
+    public void resetPose(Pose2d pose) {
+        if (pose == null) return;
+        try {
+            // Preferred: ask the drivetrain to reset its odometry directly
+            System.out.println("!"+pose);
+            swerveDriveTrain.resetPose(pose);
+        } catch (NoSuchMethodError | RuntimeException e) {
+            // Fallback: tare sensors then add a timed vision measurement so odometry is seeded
+            swerveDriveTrain.tareEverything();
+            swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime()));
+        }
+    }
+
     @Override
     public void setLimits(double limitInAmps) {
         for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {