diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 73a650e..5e7dfc7 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/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 { diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b4fa630..b5e193d 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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); } diff --git a/src/main/java/frc/robot/commands/AlignAtTagWithX.java b/src/main/java/frc/robot/commands/AlignAtTagWithX.java index 808be99..c1cacda 100644 --- a/src/main/java/frc/robot/commands/AlignAtTagWithX.java +++ b/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()); } } diff --git a/src/main/java/frc/robot/commands/AutoDriveTo.java b/src/main/java/frc/robot/commands/AutoDriveTo.java index b93abbe..19370e2 100644 --- a/src/main/java/frc/robot/commands/AutoDriveTo.java +++ b/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 diff --git a/src/main/java/frc/robot/commands/Autos.java b/src/main/java/frc/robot/commands/Autos.java index 64bf2a4..6918862 100644 --- a/src/main/java/frc/robot/commands/Autos.java +++ b/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)); } diff --git a/src/main/java/frc/robot/subsystems/Vision.java b/src/main/java/frc/robot/subsystems/Vision.java index 526ceaa..6a185f9 100644 --- a/src/main/java/frc/robot/subsystems/Vision.java +++ b/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;