Merge branch 'main' of https://github.com/Team334/R2024

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
 import frc.robot.commands.elevator.HoldElevator;
+import frc.robot.commands.shooter.AutoAim;
 import frc.robot.commands.shooter.HoldShooter;
 import frc.robot.commands.shooter.SpinShooter;
 import frc.robot.commands.swerve.BrakeSwerve;
@@ -25,6 +26,7 @@
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
+import frc.robot.utils.UtilFuncs;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -35,7 +37,7 @@
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
   private final VisionSubsystem _visionSubsystem = new VisionSubsystem();
-  private final SwerveDriveSubsystem _swerveDrive = new SwerveDriveSubsystem(_visionSubsystem);
+  private final SwerveDriveSubsystem _swerveSubsystem = new SwerveDriveSubsystem(_visionSubsystem);
   private final ShooterSubsystem _shooterSubsystem = new ShooterSubsystem();
   private final ElevatorSubsystem _elevatorSubsystem = new ElevatorSubsystem();
 
@@ -54,18 +56,27 @@ public class RobotContainer {
 
   /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
-    Command interruptSwerve = new BrakeSwerve(_swerveDrive, 3);
+    Command interruptSwerve = new BrakeSwerve(_swerveSubsystem, 3);
 
     NamedCommands.registerCommand("printHello", new PrintCommand("AUTON HELLO"));
     NamedCommands.registerCommand("waitCommand", new WaitCommand(3));
     NamedCommands.registerCommand("interruptSwerve", interruptSwerve);
 
-    _swerveDrive.setDefaultCommand(
-        new TeleopDrive(
-            _swerveDrive,
-            () -> -_driveFilterLeftY.calculate(_driveController.getLeftY()),
-            () -> -_driveFilterLeftX.calculate(_driveController.getLeftX()),
-            () -> -_driveFilterRightX.calculate(_driveController.getRightX())));
+    _swerveSubsystem.setDefaultCommand(
+      // new TeleopDrive(
+      //   _swerveDrive,
+      //   () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1),
+      //   () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1),
+      //   () -> UtilFuncs.ApplyDeadband(-_driveFilterRightX.calculate(_driveController.getRightX()), 0.1)
+      // )
+      new AutoAim(
+        _shooterSubsystem,
+        _visionSubsystem,
+        _swerveSubsystem,
+        () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftY.calculate(_driveController.getLeftY()), 0.1),
+        () -> UtilFuncs.ApplyDeadband(-_driveFilterLeftX.calculate(_driveController.getLeftX()), 0.1)
+      )
+    );
 
     // _elevatorSubsystem.setDefaultCommand(new HoldElevator(_elevatorSubsystem));
     // _shooterSubsystem.setDefaultCommand(new HoldShooter(_shooterSubsystem));
@@ -80,37 +91,37 @@ public RobotContainer() {
 
   // to configure button bindings
   private void configureBindings() {
-    _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveDrive));
-    _driveController.square().onTrue(new ResetPose(_swerveDrive));
+    _driveController.R1().onTrue(new ToggleSwerveOrient(_swerveSubsystem));
+    _driveController.square().onTrue(new ResetPose(_swerveSubsystem));
     _driveController.circle().whileTrue(new SpinShooter(_shooterSubsystem));
-    _driveController.cross().whileTrue(new BrakeSwerve(_swerveDrive));
+    _driveController.cross().whileTrue(new BrakeSwerve(_swerveSubsystem));
 
     // for testing raw percent output, is it straight?
     _driveController
         .L1()
         .onTrue(
             Commands.runOnce(
                 () -> {
-                  _swerveDrive.driveTest(0.1);
+                  _swerveSubsystem.driveTest(0.1);
                 },
-                _swerveDrive));
+                _swerveSubsystem));
 
     // for testing velocity output (forward at 0.3 m/s), is it straight?
     _driveController
         .triangle()
         .whileTrue(
             Commands.run(
                 () -> {
-                  _swerveDrive.driveChassis(new ChassisSpeeds(0.3, 0, 0));
+                  _swerveSubsystem.driveChassis(new ChassisSpeeds(0.3, 0, 0));
                 },
-                _swerveDrive));
+                _swerveSubsystem));
   }
 
   /**
    * @return The Command to schedule for auton.
    */
   public Command getAutonCommand() {
-    _swerveDrive.fieldOriented =
+    _swerveSubsystem.fieldOriented =
         false; // make sure swerve is robot-relative for pathplanner to work
 
     return _autonChooser.getSelected();

src/main/java/frc/robot/commands/shooter/AutoAim.java

@@ -3,8 +3,14 @@
 
 package frc.robot.commands.shooter;
 
+import java.util.function.DoubleSupplier;
+
+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.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.Constants;
 import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.SwerveDriveSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
@@ -14,13 +20,34 @@ public class AutoAim extends Command {
   private VisionSubsystem _vision;
   private SwerveDriveSubsystem _swerve;
 
+  private DoubleSupplier _xSpeed;
+  private DoubleSupplier _ySpeed;
+
+  private PIDController _headingController = new PIDController(
+    Constants.PID.PP_ROTATION.kP,
+    0,
+    0
+  );
+
   /** Creates a new AutoAim. */
-  public AutoAim(ShooterSubsystem shooter, VisionSubsystem vision, SwerveDriveSubsystem swerve) {
+  public AutoAim(
+    ShooterSubsystem shooter, 
+    VisionSubsystem vision, 
+    SwerveDriveSubsystem swerve,
+    DoubleSupplier xSpeed,
+    DoubleSupplier ySpeed
+  ) {
     // Use addRequirements() here to declare subsystem dependencies.
     _shooter = shooter;
     _vision = vision;
     _swerve = swerve;
-    addRequirements(_shooter, _vision); // not adding swerve as a requirement to prevent interrupting default command
+
+    _xSpeed = xSpeed;
+    _ySpeed = ySpeed;
+
+    _headingController.setTolerance(2);
+
+    addRequirements(_shooter, _vision, _swerve);
   }
 
   // Called when the command is initially scheduled.
@@ -30,17 +57,31 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    double shooterAngle = 0;
+    double desiredShooterAngle = 0;
+    double desiredSwerveHeading = 0;
+
+    double currentSwerveHeading = _swerve.getHeading().getDegrees();
 
     if (_vision.isApriltagVisible()) {
-      shooterAngle = _vision.shooterAnglesToSpeaker()[1]; // ty
+      desiredShooterAngle = _vision.anglesToSpeaker()[1]; // ty
+      desiredSwerveHeading = _vision.anglesToSpeaker()[0];
+
     } else {
-      shooterAngle = _swerve.shooterAngleToSpeaker();
+      desiredShooterAngle = _swerve.anglesToSpeaker()[1];
+      desiredSwerveHeading = _swerve.anglesToSpeaker()[0];
     }
 
-    SmartDashboard.putNumber("ANGLE", shooterAngle);
+    desiredSwerveHeading += currentSwerveHeading;
+
+    _shooter.setAngle(desiredShooterAngle);
 
-    _shooter.setAngle(shooterAngle);
+    _swerve.driveChassis(
+      new ChassisSpeeds(
+        _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
+        _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
+        _headingController.calculate(currentSwerveHeading, desiredSwerveHeading)
+      )
+    );
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/swerve/TeleopDrive.java

@@ -50,23 +50,14 @@ public void initialize() {}
   @Override
   public void execute() {
     // apply controller deadband
-    double xSpeed = UtilFuncs.ApplyDeadband(_xSpeed.getAsDouble(), 0.1);
-    double ySpeed = UtilFuncs.ApplyDeadband(_ySpeed.getAsDouble(), 0.1);
-    double rotationSpeed = UtilFuncs.ApplyDeadband(_rotationSpeed.getAsDouble(), 0.1);
-
-    SmartDashboard.putNumber(
-        "DESIRED X SPEED",
-        xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF);
-    SmartDashboard.putNumber(
-        "DESIRED Y SPEED",
-        ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF);
-
     // drive the swerve chassis subsystem
     _swerveDrive.driveChassis(
-        new ChassisSpeeds(
-            xSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
-            ySpeed * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
-            rotationSpeed * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED));
+      new ChassisSpeeds(
+        _xSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
+        _ySpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_SPEED * Constants.Speeds.SWERVE_DRIVE_COEFF,
+        _rotationSpeed.getAsDouble() * Constants.Speeds.SWERVE_DRIVE_MAX_ANGULAR_SPEED
+      )
+    );
   }
 
   // Called once the command ends or is interrupted.

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

@@ -252,10 +252,10 @@ public void periodic() {
 
     if (_visionSubsystem.isApriltagVisible()) {
       Optional<Pose2d> visionBotpose = _visionSubsystem.getBotpose();
-      if (visionBotpose.isPresent())
-        _estimator.addVisionMeasurement(
-            _visionSubsystem.getBotpose().get(), Timer.getFPGATimestamp());
-      if (visionBotpose.isPresent()) _field.setRobotPose(visionBotpose.get());
+      if (visionBotpose.isPresent()) {
+        _estimator.addVisionMeasurement(_visionSubsystem.getBotpose().get(), _visionSubsystem.getLatency());
+        _field.setRobotPose(_estimator.getEstimatedPosition());
+      }
     }
 
     SmartDashboard.putData("FIELD", _field);
@@ -290,6 +290,10 @@ private void setupOrchestra() {
     _orchestra.play();
   }
 
+  public void setChassisSpeeds(ChassisSpeeds chassisSpeeds) {
+
+  }
+
   /**
    * Set the chassis speed of the swerve drive.
    *
@@ -384,7 +388,7 @@ public Rotation2d getHeadingRaw() {
   }
 
   /** Get the shooter's angle to the speaker hole using the drive's pose estimator. */
-  public double shooterAngleToSpeaker() {
+  public double[] anglesToSpeaker() {
     int tagID = Constants.FIELD_CONSTANTS.SPEAKER_TAG;
     Pose3d tagPose = Constants.FIELD_CONSTANTS.APRILTAG_LAYOUT.getTagPose(tagID).get();
 
@@ -395,7 +399,16 @@ public double shooterAngleToSpeaker() {
 
     double zDifference = FieldConstants.SPEAKER_HEIGHT - Constants.Physical.SHOOTER_HEIGHT_STOWED;
 
-    double angle = Math.atan(zDifference / distanceToRobot);
-    return Math.toDegrees(angle);
+    double angleY = Math.atan(zDifference / distanceToRobot);
+
+
+    Rotation2d currentRotation = getHeading();
+    double rotateDifference = Math.atan((getPose().getY() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getY()) / (getPose().getX() - Constants.FIELD_CONSTANTS.SPEAKER_POSE.getX()));
+
+    double angleX = currentRotation.getDegrees() - rotateDifference;
+
+    double[] angles = {angleX, angleY};
+
+    return angles;
   }
 }

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.utils.LimelightHelper;
@@ -38,6 +39,16 @@ public void periodic() {
     // SmartDashboard.putData("Limelight Field", _field);
   }
 
+  /**
+   * Returns the latency from the last time data was sent from the limelight. This should be used in the pose estimator.
+   */
+  public double getLatency() {
+    double tl = _limelight.getEntry("tl").getDouble(0);
+    double cl = _limelight.getEntry("cl").getDouble(0);
+
+    return Timer.getFPGATimestamp() - (tl/1000.0) - (cl/1000.0);
+  }
+
   /**
    * Returns the "wpiblue" botpose of the robot from the limelight.
    * 
@@ -117,7 +128,7 @@ public double[] tagAngleOffsets(int ID) {
    * 
    * @return [tx, ty] or null.
    */
-  public double[] shooterAnglesToSpeaker() {
+  public double[] anglesToSpeaker() {
     return tagAngleOffsets(Constants.FIELD_CONSTANTS.SPEAKER_TAG);
   }
 }

src/main/java/frc/robot/utils/LimelightHelper.java

@@ -76,4 +76,6 @@ public JsonNode getTag(int ID) {
 
     return null;
   }
+
+
 }