tweaks to L1 and auto align good when PIs are on

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

@@ -163,6 +163,8 @@ public class RobotContainer {
 
 		driver.b().whileTrue(manipulator.runManipulator(() -> -0.35, true));
 
+		driver.x().whileTrue(manipulator.runManipulator(() -> -0.2, true));
+		
 		driver.start().whileTrue(drivetrain.resetToVision());
 	
 		driver.rightBumper().whileTrue(
@@ -224,18 +226,18 @@ public class RobotContainer {
 
 		operator.x().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL2AlgaePosition, ManipulatorPivotConstants.kL2AlgaePosition)
-				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
 				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.b().onTrue(
 			safeMoveManipulator(ElevatorConstants.kL3AlgaePosition, ManipulatorPivotConstants.kL3AlgaePosition)
-				.alongWith(manipulator.runManipulator(() -> 0.5, false))
+				.alongWith(manipulator.runManipulator(() -> 0.85, false))
 				.until(() -> driver.a().getAsBoolean())
 		);
 
 		operator.y().onTrue(moveWithAlgae(ElevatorConstants.kProcessorPosition, ManipulatorPivotConstants.kProcessorPosition)
-			.alongWith(manipulator.runManipulator(() -> 0.5, false))
+			.alongWith(manipulator.runManipulator(() -> 0.85, false))
 			.until(() -> driver.a().getAsBoolean())
 		);
 
@@ -421,30 +423,30 @@ public class RobotContainer {
 		//sensorTab.add("odometry", drivetrain::getPose);
 
 		apriltagTab.addDouble("Orange ID", () -> drivetrain.vision.getOrangeClosestTag())
-			.withSize(2,1).withPosition(1,1);
+			.withSize(1,1).withPosition(1,1);
 		apriltagTab.addDouble("Orange tx", () -> drivetrain.vision.getOrangeTX())
-			.withSize(2,1).withPosition(3,1);
+			.withSize(1,1).withPosition(2,1);
 		apriltagTab.addDouble("Orange ty", () -> drivetrain.vision.getOrangeTY())
-			.withSize(2,1).withPosition(5,1);
+			.withSize(1,1).withPosition(3,1);
 		apriltagTab.addDouble("Orange dist", () -> drivetrain.vision.getOrangeDist())
-			.withSize(2,1).withPosition(7,1);
+			.withSize(1,1).withPosition(4,1);
 		apriltagTab.addDouble("orange fps", () -> drivetrain.vision.getOrangeFPS())
-			.withSize(2,1).withPosition(9,1);
+			.withSize(1,1).withPosition(5,1);
 		apriltagTab.addBoolean("orange detected", () -> drivetrain.vision.getOrangeTagDetected())
-			.withSize(2,1).withPosition(11,1);
+			.withSize(1,1).withPosition(6,1);
 
 		apriltagTab.addDouble("Black ID", () -> drivetrain.vision.getBlackClosestTag())
-			.withSize(2,1).withPosition(1,2);
+			.withSize(1,1).withPosition(1,2);
 		apriltagTab.addDouble("Black tx", () -> drivetrain.vision.getBlackTX())
-			.withSize(2,1).withPosition(3,2);
+			.withSize(1,1).withPosition(2,2);
 		apriltagTab.addDouble("Black ty", () -> drivetrain.vision.getBlackTY())
-			.withSize(2,1).withPosition(5,2);
+			.withSize(1,1).withPosition(3,2);
 		apriltagTab.addDouble("Black dist", () -> drivetrain.vision.getBlackDist())
-			.withSize(2,1).withPosition(7,2);
+			.withSize(1,1).withPosition(4,2);
 		apriltagTab.addDouble("Black fps", () -> drivetrain.vision.getBlackFPS())
-			.withSize(2,1).withPosition(7,2);
+			.withSize(1,1).withPosition(5,2);
 		apriltagTab.addBoolean("Black detected", () -> drivetrain.vision.getBlackTagDetected())
-			.withSize(2,1).withPosition(9,2);
+			.withSize(1,1).withPosition(6,2);
 
 		apriltagTab.addDouble("Closest tag", () -> drivetrain.getClosestTag())
 			.withSize(2,1).withPosition(4,4);

src/main/java/frc/robot/constants/AutoConstants.java

@@ -19,10 +19,16 @@ public class AutoConstants {
     public static final double kPXYController = 3.5;
     public static final double kPThetaController = 5;
 
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
+
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
 
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
     // TODO This is a constant being managed like a static rewriteable variable
     public static RobotConfig kRobotConfig;
 

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -42,7 +42,7 @@ public class ElevatorConstants {
     public static final double kMaxAcceleration = 240;    // 400 inches per second^2 (also COOKING) calculated max is 600 in/s^2
 
     public static final double kCoralIntakePosition = 0;
-    public static final double kL1Position = 14;
+    public static final double kL1Position = 17;
     public static final double kL2Position = 11;
     public static final double kL3Position = 27;
     public static final double kL4Position = 50.5;

src/main/java/frc/robot/constants/ManipulatorPivotConstants.java

@@ -34,7 +34,7 @@ public class ManipulatorPivotConstants {
     public static final double kMaxAcceleration = Units.degreesToRadians(1000.0); // degrees per second^2 calculated max = 2100
     public static final double kMaxVelocity = Units.degreesToRadians(100.0); // degrees per second calculated max = 168
 
-    public static final double kEncoderOffset = 0.78-0.25;
+    public static final double kEncoderOffset = 0.04500000178813934;
 
     public static final double kStartingPosition = Units.degreesToRadians(90);
     public static final double kCoralIntakePosition = Units.degreesToRadians(175.0+90);

src/main/java/frc/robot/subsystems/Drivetrain.java

@@ -115,13 +115,13 @@ public class Drivetrain extends SubsystemBase {
             VecBuilder.fill(1, 1, Units.degreesToRadians(360))
         );
 
-        pidHeading = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
+        pidHeading = new ProfiledPIDController(AutoConstants.kAlignPThetaController, 0, 0, AutoConstants.kAlignThetaControllerConstraints);
         pidHeading.setTolerance(Units.degreesToRadians(3));
         pidHeading.enableContinuousInput(-Units.degreesToRadians(180), Units.degreesToRadians(180));
 
-        pidTranslationX = new PIDController(AutoConstants.kPXYController,0,0);
+        pidTranslationX = new PIDController(AutoConstants.kAlignPXYController,0,0);
         pidTranslationX.setTolerance(Units.inchesToMeters(0.5));
-        pidTranslationY = new PIDController(AutoConstants.kPXYController,0,0);
+        pidTranslationY = new PIDController(AutoConstants.kAlignPXYController,0,0);
         pidTranslationY.setTolerance(Units.inchesToMeters(0.5));
 
         driveController = new HolonomicDriveController(pidTranslationX, pidTranslationY, pidHeading);
@@ -326,6 +326,9 @@ public class Drivetrain extends SubsystemBase {
 
     public Command goToPose(DoubleSupplier xSetpoint, DoubleSupplier ySetpoint, Supplier<Rotation2d> headingSetpoint){
         return startRun(() -> {
+                pidTranslationX.reset();
+                pidTranslationY.reset();
+                pidHeading.reset(m_estimator.getEstimatedPosition().getRotation().getRadians(), gyro.getVelocityZ());
             },
             () -> {
                     ChassisSpeeds controlEffort = driveController.calculate(
@@ -459,7 +462,11 @@ public class Drivetrain extends SubsystemBase {
 
     public Command resetToVision(){
         return runOnce(() -> {
-            m_estimator.resetPose(orangePose2d);
+            if(vision.getOrangeTagDetected()){
+                m_estimator.resetPose(new Pose2d(orangePose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }else if(vision.getBlackTagDetected()){
+                m_estimator.resetPose(new Pose2d(blackPose2d.getTranslation(), m_estimator.getEstimatedPosition().getRotation()));
+            }
         });
     }
 }