2 note auto good

src/main/java/frc/robot/Constants.java

@@ -133,7 +133,7 @@ public static class ArmConstants {
 		public static final double ARM_STOW_DEGREES = -83.0;
 		public static final double ARM_STOW_2_DEGREES = -100.0;
 		public static final double ARM_AMP_SHOOTING_DEGREES = -27.8;
-		public static final double ARM_SPEAKER_SHOOTING_DEGREES = -97.822 - 5 + 4 - 6;
+		public static final double ARM_SPEAKER_SHOOTING_DEGREES = -97.822 - 5 + 4 - 6 -1;
 		public static final double ARM_INTAKE_DEGREES = -109 - 1.5;
 
 		public static final int LEFT_MOTOR_ID = 5;
@@ -180,7 +180,7 @@ public static class IntakeShooterConstants {
 	}
 
 	public static class RobotMovementConstants {
-		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(3);
+		public static final double POSITION_TOLERANCE_METERS = Units.inchesToMeters(5);
 		public static final double ANGLE_TOLERANCE_RADIANS = Units.degreesToRadians(1);
 
 		public static final double ROTATION_PID_P = 0.21;

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

@@ -11,8 +11,8 @@
 import frc.robot.commands.CancelCommands;
 import frc.robot.commands.ChassisRemoteControl;
 import frc.robot.commands.HangControl;
+import frc.robot.commands.AimAtAngle;
 import frc.robot.commands.ArmRotateTo;
-import frc.robot.commands.AutoRotateTo;
 import frc.robot.commands.SetLightstripColorFor;
 import frc.robot.commands.SpinFlywheelShooter;
 import frc.robot.subsystems.ChassisDriveInputs;
@@ -40,6 +40,7 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.net.PortForwarder;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
@@ -142,6 +143,11 @@ public RobotContainer() {
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 
 		SmartDashboard.putData("ArmUp", new ArmRotateTo(arm, ArmConstants.ARM_START_DEGREES));
+		SmartDashboard.putData("ZeroYaw", new InstantCommand(drivetrain::zeroYaw));
+
+		SmartDashboard.putData(drivetrain);
+		SmartDashboard.putData(arm);
+		SmartDashboard.putData(intakeShooter);
 
 		SmartDashboard.putString("Bot Name", Constants.currentBot.toString() + " - " + Constants.serialNumber);
 
@@ -224,8 +230,10 @@ public void setUpDriveController() {
 
 			xbox.y().onTrue(Commands.runOnce(inputs::toggleFieldRelative));
 
-			xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true));
-			xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true));
+			// xbox.rightBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(0), true));
+			// xbox.leftBumper().onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(-90), true));
+			xbox.rightBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(0)));
+			xbox.leftBumper().whileFalse(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(-90)));
 
 			if (vision != null)
 				xbox.x().onTrue(Commands.runOnce(vision::toggleUsing, vision));

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

@@ -0,0 +1,82 @@
+package frc.robot.commands;
+
+import frc.robot.Constants.RobotMovementConstants;
+import frc.robot.subsystems.ChassisDriveInputs;
+import frc.robot.subsystems.SwerveDrivetrain;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/** Command to automatically aim at a angle */
+public class AimAtAngle extends Command {
+	private final SwerveDrivetrain drivetrain;
+	private final ChassisDriveInputs chassisDriveInputs;
+
+	private final PIDController rotatePID;
+
+	/**
+	 * Create a new AimAtAngle command. Tries to constants face in angle while still
+	 * allowing driver to control robot.
+	 * 
+	 * @param drivetrain          the drivetrain of the robot
+	 */
+	public AimAtAngle(SwerveDrivetrain drivetrain, ChassisDriveInputs chassisDriveInputs, Rotation2d direction) {
+		this.drivetrain = drivetrain;
+
+		this.chassisDriveInputs = chassisDriveInputs;
+
+		rotatePID = new PIDController(
+				RobotMovementConstants.ROTATION_PID_P,
+				RobotMovementConstants.ROTATION_PID_I,
+				RobotMovementConstants.ROTATION_PID_D);
+		rotatePID.enableContinuousInput(-Math.PI, Math.PI);
+		rotatePID.setTolerance(RobotMovementConstants.ANGLE_TOLERANCE_RADIANS);
+		rotatePID.setSetpoint(direction.getRadians());
+
+		addRequirements(drivetrain, chassisDriveInputs);
+	}
+
+
+	@Override
+	public void initialize() {
+		drivetrain.toDefaultStates();
+	}
+
+	@Override
+	public void execute() {
+
+		double rotationSpeed = 0;
+		if (!rotatePID.atSetpoint()) {
+			rotationSpeed = rotatePID.calculate(drivetrain.getHeading().getRadians());
+		}
+
+		double xSpeed = 0;
+		double ySpeed = 0;
+		boolean fieldRelative = false;
+		if (chassisDriveInputs != null) {
+			xSpeed = chassisDriveInputs.getX();
+			ySpeed = chassisDriveInputs.getY();
+			fieldRelative = chassisDriveInputs.isFieldRelative();
+		}
+
+		ChassisSpeeds desiredSpeeds = new ChassisSpeeds(xSpeed, ySpeed, 0);
+		if (fieldRelative) desiredSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(desiredSpeeds, drivetrain.getHeading());
+		desiredSpeeds.omegaRadiansPerSecond = rotationSpeed;
+
+		drivetrain.setDesiredState(desiredSpeeds, false);
+
+		drivetrain.updateSmartDashboard();
+	}
+
+	@Override
+	public boolean isFinished() {
+		return (chassisDriveInputs == null) && (rotatePID.atSetpoint());
+	}
+
+	@Override
+	public void end(boolean interrupted) {
+		drivetrain.stop();
+	}
+}

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

@@ -7,7 +7,6 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.RobotMovementConstants;
 
 public class AutoDriveTo extends Command {
@@ -84,8 +83,8 @@ public void execute() {
 	@Override
 	public boolean isFinished() {
 		boolean isEnded = xMovePID.atSetpoint() && yMovePID.atSetpoint();
-		SmartDashboard.putBoolean("xEndpoint", xMovePID.atSetpoint());
-		SmartDashboard.putBoolean("yEndpoint", yMovePID.atSetpoint());
+		// SmartDashboard.putBoolean("xEndpoint", xMovePID.atSetpoint());
+		// SmartDashboard.putBoolean("yEndpoint", yMovePID.atSetpoint());
 		return isEnded;
 	}
 

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

@@ -84,7 +84,6 @@ public static Command shoot2StartingAuto(SwerveDrivetrain drivetrain, Arm arm, I
 								new AutoDriveTo(drivetrain, new Translation2d(driveDistanceForNote1, 0))),
 						new WaitCommand(0.25),
 						intakeFromFloorEnd(arm, shooter),
-						new AutoRotateTo(drivetrain, new Rotation2d(0)),
 						Commands.race(
 								Commands.waitSeconds(3),
 								new AutoDriveTo(drivetrain, new Translation2d(-driveDistanceForNote1, 0))),
@@ -146,14 +145,14 @@ public static Command shootInSpeaker(SwerveDrivetrain drivetrain, Arm arm, Intak
 		}
 
 		return Commands.sequence(
-			Commands.parallel(
-				new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER, 0.4),
-				new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES)
-			),
-			new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER),
-			new WaitCommand(0.2),
-			new SpinFlywheelShooter(shooter, 0),
-			new SpinIntakeGrabbers(shooter, 0));
+				Commands.parallel(
+						new SpinFlywheelShooterForTime(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER,
+								1.5),
+						new ArmRotateTo(arm, ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES)),
+				new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER),
+				new WaitCommand(0.2),
+				new SpinFlywheelShooter(shooter, 0),
+				new SpinIntakeGrabbers(shooter, 0));
 	}
 
 	public static Command intakeFromFloorStart(Arm arm, IntakeShooter shooter) {

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

@@ -413,6 +413,13 @@ public Rotation3d getHeading3d() {
 		return gyro.getRotation3d();
 	}
 
+	/**
+	 * Reset gyro yaw
+	 */
+	public void zeroYaw() {
+		gyro.zeroYaw();
+	}
+
 	/**
 	 * Set amount to add to gyro position for field relative drive and
 	 * SmartDashboard display