Vision Interim Commit

Co-Authored-By: Aceius E. <144858100+AceiusRedshift@users.noreply.github.com>

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

@@ -146,7 +146,7 @@ public static class ArmConstants {
 
 		public static final boolean ARE_MOTORS_REVERSED = false;
 
-		public static final double ELEVATION_PID_P = 6.5;
+		public static final double ELEVATION_PID_P = 13.6;
 		public static final double ELEVATION_PID_I = 0.0;
 		public static final double ELEVATION_PID_D = 0.0;
 	}
@@ -179,9 +179,9 @@ public static class IntakeShooterConstants {
 		public static final double FLYWHEEL_SHOOTER_SPEED_AMP = -0.5;
 		public static final double FLYWHEEL_SHOOTER_SPEED_SPEAKER = -1.0;
 
-		public static final double INTAKE_GRABBER_SPEED_AMP = 1.0;
-		public static final double INTAKE_GRABBER_SPEED_SPEAKER = 1.0;
-		public static final double INTAKE_GRABBER_SPEED_INTAKE = -1.0;
+		public static final double INTAKE_GRABBER_SPEED_AMP = .75;
+		public static final double INTAKE_GRABBER_SPEED_SPEAKER = .75;
+		public static final double INTAKE_GRABBER_SPEED_INTAKE = -.75;
 	}
 
 	public static class RobotMovementConstants {

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

@@ -253,9 +253,16 @@ public void setUpDriveController() {
 					.onTrue(new AutoRotateTo(drivetrain, Rotation2d.fromDegrees(270), true));
 
 			driverXboxController.leftBumper()
-					.whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(300)));
+					.whileTrue(new AimAtAngle(drivetrain, inputs,
+							teamChooser.getSelected() == Alliance.Blue ? Rotation2d.fromDegrees(270)
+									: Rotation2d.fromDegrees(90)));
 			driverXboxController.rightBumper()
-					.whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(60)));
+					.whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(0)));
+
+			// driverXboxController.leftBumper()
+			// .whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(300)));
+			// driverXboxController.rightBumper()
+			// .whileTrue(new AimAtAngle(drivetrain, inputs, Rotation2d.fromDegrees(60)));
 
 			driverXboxController.b().onTrue(cancelCommand);
 		}

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

@@ -35,7 +35,8 @@ public class AlignAtTagWithX extends Command {
 
 	private final double xDistance;
 
-	// private final SlewRateLimiter rotateLimiter = new SlewRateLimiter(Units.degreesToRadians(25));
+	// private final SlewRateLimiter rotateLimiter = new
+	// SlewRateLimiter(Units.degreesToRadians(25));
 
 	/**
 	 * Create a new AlignAtTag command. Tries to constants Align at a tag while
@@ -120,21 +121,27 @@ public void execute() {
 			// If we see the tag update the rotation to the tag
 			rotationAfterLosingTag = (transform.getY() > 0 ? 1 : -1) * LOST_TAG_SPEED_RADIANS;
 
-			boolean innerPhase = (transform.getX() < INNER_MODE_BOX_X + xDistance) && (Math.abs(transform.getY()) < INNER_MODE_BOX_Y);
+			boolean innerPhase = (transform.getX() < INNER_MODE_BOX_X + xDistance)
+					&& (Math.abs(transform.getY()) < INNER_MODE_BOX_Y);
 
 			// Use X and Y controllers to drive to desired distance from tag
 			xSpeed = xController.calculate(transform.getX(), xDistance);
 			ySpeed = yController.calculate(transform.getY(), 0);
-			// SmartDashboard.putNumber("X speed a", xController.calculate(transform.getX()));
-			// SmartDashboard.putNumber("Y speed a", yController.calculate(transform.getY()));
+			// SmartDashboard.putNumber("X speed a",
+			// xController.calculate(transform.getX()));
+			// SmartDashboard.putNumber("Y speed a",
+			// yController.calculate(transform.getY()));
 
 			// Switch to determine when to switch to always facing given angle.
-			// If we are far enough away then drive by looking at tag so we don't lose the target.
+			// If we are far enough away then drive by looking at tag so we don't lose the
+			// target.
 			// If we are close enough then we just lock the angle
 			if (innerPhase) {
-				rotationSpeed = rotatePIDangle.calculate(drivetrain.getHeading().getRadians(), rotation.get().getRadians());
+				rotationSpeed = rotatePIDangle.calculate(drivetrain.getHeading().getRadians(),
+						rotation.get().getRadians());
 			} else {
-				rotationSpeed = rotatePIDtag.calculate(new Rotation2d(transform.getX(), transform.getY()).unaryMinus().getRotations());
+				rotationSpeed = rotatePIDtag
+						.calculate(new Rotation2d(transform.getX(), transform.getY()).unaryMinus().getRotations());
 			}
 		}
 

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

@@ -4,6 +4,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 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 frc.robot.Constants.RobotMovementConstants;
 import frc.robot.subsystems.SwerveDrivetrain;
@@ -86,6 +87,11 @@ public void execute() {
 		}
 
 		drivetrain.setDesiredState(new ChassisSpeeds(xSpeed, ySpeed, 0));
+
+		SmartDashboard.putString("AutoDriver PID Error",
+				"(" + yMovePID.getPositionError() + ", " + yMovePID.getPositionError() + ")"
+
+		);
 	}
 
 	@Override

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

@@ -11,6 +11,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
 import frc.robot.Constants.ArmConstants;
@@ -120,11 +121,13 @@ public static Command dropInAmp(SwerveDrivetrain drivetrain, Arm arm, IntakeShoo
 				|| vision.getTransformToTag(tagSupplier.getAsInt()) != null);
 
 		return Commands.sequence(
-				new AlignAtTagWithX(drivetrain, vision, tagSupplier, distanceToShootFrom, rotationSupplier)
-						.onlyIf(shouldUseVisionSupplier),
+				// new AlignAtTagWithX(drivetrain, vision, tagSupplier, distanceToShootFrom,
+				// rotationSupplier)
+				// .onlyIf(shouldUseVisionSupplier),
 				Commands.parallel(
-						new AutoDriveTo(drivetrain, new Translation2d(-distanceToAlignAt + distanceToShootFrom, 0))
-								.onlyIf(shouldUseVisionSupplier),
+						// new AutoDriveTo(drivetrain, new Translation2d(-distanceToAlignAt +
+						// distanceToShootFrom, 0))
+						// .onlyIf(shouldUseVisionSupplier),
 						new ArmRotateTo(arm, ArmConstants.ARM_AMP_SHOOTING_DEGREES),
 						new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_AMP),
 						new WaitCommand(minSpinUpTimeSeconds)),
@@ -166,29 +169,35 @@ public static Command shootInSpeaker(SwerveDrivetrain drivetrain, Arm arm, Intak
 		final BooleanSupplier shouldRunSupplier = () -> (!shouldUseVisionSupplier.getAsBoolean()
 				|| vision.getTransformToTag(tagSupplier.getAsInt()) != null);
 
-		return Commands.parallel(Commands.sequence(
-				new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER),
-				Commands.parallel(
-						Commands.sequence(
-								new AlignAtTagWithX(drivetrain, vision, tagSupplier, distanceFromTag + spacingFromPoint,
-										() -> new Rotation2d()),
-								new AutoDriveTo(drivetrain, new Translation2d(-spacingFromPoint, 0)))
-								.onlyIf(shouldUseVisionSupplier),
-						new WaitCommand(minSpinUpTimeSeconds),
-						new ArmRotateTo(arm,
-								ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES
-										+ SmartDashboard.getNumber("shootOffset", 0))),
-				new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER),
-				new WaitCommand(0.3),
-				new SpinFlywheelShooter(shooter, 0),
-				new SpinIntakeGrabbers(shooter, 0),
-				new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES)),
-				DriverConstants.ENABLE_RUMBLE ? new SetControllerRumbleFor(robotContainer.driverXboxRaw, 3, 1)
-						: Commands.sequence(),
-				DriverConstants.ENABLE_RUMBLE ? new SetControllerRumbleFor(robotContainer.operatorXboxRaw, 3, 1)
-						: Commands.sequence()
-		// if rumble isn't enabled pass an empty sequence instead
-		).onlyIf(shouldRunSupplier);
+		return Commands
+				.parallel(
+						new InstantCommand(() -> SmartDashboard.putBoolean("Got to Command", true)), Commands.sequence(
+								new SpinFlywheelShooter(shooter, IntakeShooterConstants.FLYWHEEL_SHOOTER_SPEED_SPEAKER),
+								Commands.parallel(
+										Commands.sequence(
+												new AlignAtTagWithX(drivetrain, vision, tagSupplier,
+														distanceFromTag + spacingFromPoint,
+														() -> new Rotation2d()),
+												Commands.race(
+														new AutoDriveTo(drivetrain,
+																new Translation2d(-spacingFromPoint, 0)),
+														new WaitCommand(4)))
+												.onlyIf(shouldUseVisionSupplier),
+										new WaitCommand(minSpinUpTimeSeconds),
+										new ArmRotateTo(arm,
+												ArmConstants.ARM_SPEAKER_SHOOTING_DEGREES
+														+ SmartDashboard.getNumber("shootOffset", 0))),
+								new SpinIntakeGrabbers(shooter, IntakeShooterConstants.INTAKE_GRABBER_SPEED_SPEAKER),
+								new WaitCommand(0.3),
+								new SpinFlywheelShooter(shooter, 0),
+								new SpinIntakeGrabbers(shooter, 0),
+								new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES)),
+						DriverConstants.ENABLE_RUMBLE ? new SetControllerRumbleFor(robotContainer.driverXboxRaw, 3, 1)
+								: Commands.sequence(),
+						DriverConstants.ENABLE_RUMBLE ? new SetControllerRumbleFor(robotContainer.operatorXboxRaw, 3, 1)
+								: Commands.sequence()
+				// if rumble isn't enabled pass an empty sequence instead
+				).onlyIf(shouldRunSupplier);
 	}
 
 	public static Command intakeFromFloorStart(Arm arm, IntakeShooter shooter) {
@@ -201,8 +210,8 @@ public static Command intakeFromFloorStart(Arm arm, IntakeShooter shooter) {
 	public static Command intakeFromFloorEnd(Arm arm, IntakeShooter shooter) {
 		return Commands.sequence(
 				new SpinFlywheelShooter(shooter, 0),
-				new SpinIntakeGrabbers(shooter, -1),
-				new WaitCommand(0.01),
+				new SpinIntakeGrabbers(shooter, -0.75),
+				new WaitCommand(0.0025),
 				new SpinIntakeGrabbers(shooter, 0),
 				new ArmRotateTo(arm, ArmConstants.ARM_STOW_2_DEGREES));
 	}

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

@@ -17,7 +17,7 @@ public class Vision extends SubsystemBase {
 
 	private final static boolean DEBUG_INFO = false;
 
-	private boolean visionEnabled = true;
+	private boolean visionEnabled = false;
 
 	final PhotonCamera camera;
 	final Transform3d robotToCamera;