diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 9c293117..d5a97d01 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -13,6 +13,8 @@ package frc.robot; +import edu.wpi.first.math.util.Units; + /** * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean * constants. This class should not be used for any other purpose. All constants should be declared @@ -27,9 +29,10 @@ public final class Constants { public static final double SHOOTER_DELAY = 1; public static final double INTAKE_SHOOTER_VOLTS = -8.0; public static final double INTAKE_HOPPER_VOLTS = -4.0; - public static final double TICKS_PER_REVOLUTION = 1440; public static final Mode currentMode = Mode.REAL; + public static final double FIELD_LENGTH = Units.inchesToMeters(651.223); + public static final double SPEAKER_Y = Units.inchesToMeters(219.277); public static enum Mode { /** Running on a real robot. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b06be806..b4cd7d16 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -145,6 +145,8 @@ private void configureButtonBindings() { .andThen(new LaunchNote(shooter, hopper)) .handleInterrupt(() -> shooter.stop())); controller.b().whileTrue(new IntakeNote(shooter, hopper).handleInterrupt(() -> shooter.stop())); + controller.x().whileTrue(new TurnToSpeaker(drive)); + controller.y().whileTrue(AutoBuilder.buildAuto("Reset_Odometry")); } /** diff --git a/src/main/java/frc/robot/commands/TurnToSpeaker.java b/src/main/java/frc/robot/commands/TurnToSpeaker.java new file mode 100644 index 00000000..d7dcdc13 --- /dev/null +++ b/src/main/java/frc/robot/commands/TurnToSpeaker.java @@ -0,0 +1,102 @@ +package frc.robot.commands; + +import static frc.robot.Constants.Mode.SIM; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants; +import frc.robot.subsystems.drive.Drive; +import org.littletonrobotics.junction.Logger; + +public class TurnToSpeaker extends Command { + private final Drive drive; + private final PIDController controller; + private static double kP = 0; + private static double kI = 0; + private static double kD = 0; + private static double minVelocity = 0; + private static double toleranceDegrees = 0; + private DriverStation.Alliance alliance = null; + + /** Creates a new TurnToAngle. Turns to the specified rotation. */ + public TurnToSpeaker(Drive drive) { + addRequirements(drive); + this.drive = drive; + + switch (Constants.currentMode) { + case REAL: + kP = 0.018; + kI = 0.0; + kD = 0.0; + minVelocity = 0.0; + toleranceDegrees = 1.0; + break; + default: // for SIM + kP = 0.1; + kI = 0.0; + kD = 0.001; + minVelocity = 0.0; + toleranceDegrees = 1.5; + break; + } + + controller = new PIDController(kP, kI, kD, 0.02); + controller.setTolerance(toleranceDegrees); + controller.enableContinuousInput(-180, 180); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + controller.reset(); + + if (DriverStation.getAlliance().isPresent()) { + this.alliance = DriverStation.getAlliance().get(); + } + + if (alliance == DriverStation.Alliance.Red) { + controller.setSetpoint( + Math.toDegrees( + Math.atan2( + Constants.SPEAKER_Y - drive.getPose().getY(), + Constants.FIELD_LENGTH - drive.getPose().getX())) + - (Constants.currentMode == SIM ? Drive.YAW_DISPLACEMENT : 0)); + } else { + controller.setSetpoint( + Math.toDegrees( + Math.atan2(Constants.SPEAKER_Y - drive.getPose().getY(), -drive.getPose().getX()) + - (Constants.currentMode == SIM ? Drive.YAW_DISPLACEMENT : 0))); + } + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Logger.recordOutput("TurnActive", true); + Logger.recordOutput("Turn X Goal", controller.getSetpoint()); + // Update output speeds + + double angularSpeed = controller.calculate(drive.getGyroYawDegrees()); + + if (Math.abs(angularSpeed) < minVelocity) { + angularSpeed = Math.copySign(minVelocity, angularSpeed); + } + + Logger.recordOutput("Angular Speed", angularSpeed); + drive.driveVelocity( + -angularSpeed * Drive.TRACK_WIDTH / 2.0, angularSpeed * Drive.TRACK_WIDTH / 2.0); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + Logger.recordOutput("TurnActive", false); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return controller.atSetpoint(); + } +}