auto align wackiness

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

@@ -11,9 +11,7 @@ import java.io.File;
 
 import com.pathplanner.lib.auto.NamedCommands;
 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;
@@ -29,7 +27,6 @@ import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
-import frc4388.robot.commands.MoveForTimeCommand;
 import frc4388.robot.commands.alignment.AutoAlign;
 import frc4388.robot.constants.Constants;
 import frc4388.robot.constants.Constants.OIConstants;
@@ -41,7 +38,6 @@ import frc4388.robot.subsystems.intake.Intake;
 import frc4388.robot.subsystems.intake.Intake.IntakeMode;
 import frc4388.robot.subsystems.shooter.Shooter;
 import frc4388.robot.subsystems.shooter.ShooterConstants;
-import frc4388.robot.subsystems.shooter.Shooter.ShooterMode;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.DeferredBlock;
@@ -124,8 +120,8 @@ public class RobotContainer {
     );
 
     private Command RobotShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> 
+        // TEST NEW AUTO ALIGN
-                new AutoAlign(m_robotSwerveDrive, m_vision, true)),
+        //new AutoAlign(m_robotSwerveDrive, m_vision),
         new InstantCommand(()-> m_robotShooter.setShooterShoot(), m_robotShooter),
         new InstantCommand(()-> m_robotShooter.setShooterReady(), m_robotShooter),
         new WaitCommand(3),

src/main/java/frc4388/robot/commands/alignment/AutoAlign.java

@@ -14,101 +14,59 @@ import frc4388.utility.structs.Gains;
 
 public class AutoAlign extends Command {
 
-    private PID xPID = new PID(AutoConstants.XY_GAINS, 0);
+    private PID rotPID = new PID(AutoConstants.ROT_GAINS, 0);
-    private PID yPID = new PID(AutoConstants.XY_GAINS, 0);
     private Pose2d targetpos;
 
     SwerveDrive swerveDrive;
     Vision vision;
-    boolean waitVelocityZero;
 
-    public AutoAlign(SwerveDrive swerveDrive, Vision vision, boolean waitVelocityZero) {
+    public AutoAlign(SwerveDrive swerveDrive, Vision vision) {
         this.swerveDrive = swerveDrive;
         this.vision = vision;
-        this.waitVelocityZero = waitVelocityZero && false;
         addRequirements(swerveDrive);
     }
 
-    public static double tagRelativeXError = -1;
-    private static void setTagRelativeXError(double val){
-        tagRelativeXError = val;
-    }
-
     @Override
     public void initialize() {
-        xPID.initialize();
+        rotPID.initialize();
-        yPID.initialize();
         this.targetpos = new Pose2d(FieldPositions.HUB_POSITION, new Rotation2d(0));
     }
     
-    double xerr;
+
-    double yerr;
     double roterr;
 
-    double xoutput;
-    double youtput;
     double rotoutput;
 
     @Override
     public void execute() {
-        xerr = TimesNegativeOne.invert(targetpos.getX() - vision.getPose2d().getX(), TimesNegativeOne.XAxis);
-        yerr = TimesNegativeOne.invert(targetpos.getY() - vision.getPose2d().getY(), !TimesNegativeOne.YAxis);
-        
-        roterr = ((targetpos.getRotation().getDegrees() - vision.getPose2d().getRotation().getDegrees()));
-
-        if(roterr > 180){
-            roterr -= 360;
-        }else if(roterr < -180){
-            roterr += 360;
-        }
 
 
+        Pose2d robotPose = vision.getPose2d();
+        if (robotPose == null) return;
+
+        double dx = targetpos.getX() - robotPose.getX();
+        double dy = targetpos.getY() - robotPose.getY();
+
+        Rotation2d desiredHeading = new Rotation2d(Math.atan2(dy, dx));
+
+        roterr = desiredHeading.getDegrees() - robotPose.getRotation().getDegrees();
+        if (roterr > 180) roterr -= 360;
+        if (roterr < -180) roterr += 360;
 
-        SmartDashboard.putNumber("PID X Error", xerr);
-        SmartDashboard.putNumber("PID Y Error", yerr);
         SmartDashboard.putNumber("PID Rot Error", roterr);
 
-        xoutput = xPID.update(xerr);
+        swerveDrive.driveRelativeAngle(new Translation2d(0.0, 0.0), desiredHeading);
-        youtput = yPID.update(yerr);
+     }
-
-        xoutput *= Math.abs(xerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
-        youtput *= Math.abs(yerr) < AutoConstants.XY_TOLERANCE ? 0 : 1;
-        
-
-
-        Translation2d leftStick = new Translation2d(
-            Math.max(Math.min(-youtput, 1), -1),
-            Math.max(Math.min(-xoutput, 1), -1)
-        );
-
-        
-
-        setTagRelativeXError(
-            new Translation2d(xerr, yerr).getAngle()
-            .rotateBy(targetpos.getRotation())
-            .getCos());
-
-        swerveDrive.driveRelativeAngle(leftStick, targetpos.getRotation());
-    }
 
     @Override
     public final boolean isFinished() {
-        boolean finished = (
+        boolean finished = Math.abs(roterr) < AutoConstants.ROT_TOLERANCE;
-            (Math.abs(xerr) < AutoConstants.XY_TOLERANCE || Math.abs(xoutput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
+        if (finished) {
-            (Math.abs(yerr) < AutoConstants.XY_TOLERANCE || Math.abs(youtput) <= AutoConstants.MIN_XY_PID_OUTPUT) && 
-            (Math.abs(roterr) < AutoConstants.ROT_TOLERANCE) &&
-            (!waitVelocityZero || swerveDrive.lastOdomSpeed < AutoConstants.VELOCITY_THRESHHOLD)
-        );
-
-        if(finished)
             swerveDrive.softStop();
-
+        }
         return finished;
     }
 
-
-
-
     private class PID {
         protected Gains  gains;
         private   double output    = 0;

src/main/java/frc4388/robot/constants/Constants.java

@@ -47,7 +47,7 @@ public final class Constants {
 
     public static final class AutoConstants {
         // public static final Gains XY_GAINS = new Gains(5,0.6,0.0);
-        public static final Gains XY_GAINS = new Gains(8,0,0.0);
+        public static final Gains ROT_GAINS = new Gains(8,0,0.0);
         // public static final ConfigurableDouble P_XY_GAINS = new ConfigurableDouble("P_XY_GAINS", XY_GAINS.kP);
         // public static final ConfigurableDouble I_XY_GAINS = new ConfigurableDouble("I_XY_GAINS", XY_GAINS.kI);
         // public static final ConfigurableDouble D_XY_GAINS = new ConfigurableDouble("D_XY_GAINS", XY_GAINS.kD);
@@ -61,7 +61,7 @@ public final class Constants {
         public static final Trim Y_OFFSET_TRIM =        new Trim("Y Offset Trim",        Double.MAX_VALUE, -Double.MAX_VALUE, 0.5, 0);
          
         public static final double XY_TOLERANCE = 0.07; // Meters
-        public static final double ROT_TOLERANCE = 5; // Degrees
+        public static final double ROT_TOLERANCE = 10; // Degrees
 
         public static final double MIN_XY_PID_OUTPUT = 0.0;
         public static final double MIN_ROT_PID_OUTPUT = 0.0;