Hi zane - rotate to speaker and on-the-fly fixes

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

@@ -0,0 +1,40 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.subsystems.ShooterSubsystem;
+
+public class AdvancedSpinUpCommand extends Command {
+    private final ShooterSubsystem shooterSubsystem;
+
+    private final Pose2d robotPose;
+
+    /**
+     * Command to spin up the shooter
+     * @param shooterSubsystem Instance of {@link ShooterSubsystem}
+     * @param robotPose Instance of {@link Pose2d}
+     */
+    public AdvancedSpinUpCommand(ShooterSubsystem shooterSubsystem, Pose2d robotPose) {
+        this.shooterSubsystem = shooterSubsystem;
+        this.robotPose = robotPose;
+
+        addRequirements(shooterSubsystem);
+    }
+
+    @Override
+    public void execute() {
+
+        if (robotPose.getX() < 9) {
+            shooterSubsystem.setSpeed(ShooterConstants.shooterRPM);
+        } else {
+            shooterSubsystem.setVoltage(0,0);
+        }
+
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        shooterSubsystem.setVoltage(0,0);
+    }
+}

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

@@ -14,7 +14,7 @@ public class AmpCommand extends Command {
 
 
     /**
-     * Moves the indexer to the amp
+     * Moves the indexer to the amp and scores
      * @param indexerSubsystem The instance of {@link IndexerSubsystem}
      * @param controller An instance of {@link XboxController}
      */

src/main/java/frc/robot/commands/SpinUpCommand.java → src/main/java/frc/robot/commands/BasicSpinUpCommand.java

@@ -4,14 +4,14 @@
 import frc.robot.constants.ShooterConstants;
 import frc.robot.subsystems.ShooterSubsystem;
 
-public class SpinUpCommand extends Command {
+public class BasicSpinUpCommand extends Command {
     private final ShooterSubsystem shooterSubsystem;
 
     /**
      * Command to spin up the shooter
      * @param shooterSubsystem Instance of {@link ShooterSubsystem}
      */
-    public SpinUpCommand(ShooterSubsystem shooterSubsystem) {
+    public BasicSpinUpCommand(ShooterSubsystem shooterSubsystem) {
         this.shooterSubsystem = shooterSubsystem;
 
         addRequirements(shooterSubsystem);

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

@@ -17,14 +17,22 @@ public class RotateToNoteAndDriveCommand extends Command {
     private final double visionRotation;
 
     private final ProfiledPIDController rotationPID = new ProfiledPIDController(
-            VisionConstants.rotateToNoteP,
-            VisionConstants.rotateToNoteI,
-            VisionConstants.rotateToNoteD,
-            VisionConstants.rotateToNoteConstraints
+            VisionConstants.rotateToP,
+            VisionConstants.rotateToI,
+            VisionConstants.rotateToD,
+            VisionConstants.rotateToConstraints
     );
 
     private final XboxController controller;
 
+    /**
+     * Rotates the robot to face the speaker, while still allowing the driver to control forward and backward movement
+     * @param swerveSubsystem The instance of {@link SwerveSubsystem}
+     * @param controller The instance of {@link XboxController}
+     * @param forward The desired forward percentage of the robot
+     * @param sideways The desired sideways percentage of the robot
+     * @param radians The desired rotation speed of the robot
+     */
     public RotateToNoteAndDriveCommand(SwerveSubsystem swerveSubsystem, XboxController controller, DoubleSupplier forward, DoubleSupplier sideways, DoubleSupplier radians) {
         this.swerveSubsystem = swerveSubsystem;
         this.forward = forward;
@@ -50,6 +58,7 @@ public void execute() {
                     true,
                     true
             );
+            controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0);
         } else {
             swerveSubsystem.drive(
                     forward.getAsDouble(),

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

@@ -0,0 +1,84 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ShooterConstants;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.IndexerSubsystem;
+import frc.robot.subsystems.SwerveSubsystem;
+
+import java.util.function.DoubleSupplier;
+
+public class RotateToSpeakerAndDriveCommand extends Command {
+    private final SwerveSubsystem swerveSubsystem;
+    private final DoubleSupplier forward;
+    private final DoubleSupplier sideways;
+    private final DoubleSupplier radians;
+    private final double odometryRotation;
+
+    private final ProfiledPIDController rotationPID = new ProfiledPIDController(
+            VisionConstants.rotateToP,
+            VisionConstants.rotateToI,
+            VisionConstants.rotateToD,
+            VisionConstants.rotateToConstraints
+    );
+
+    /**
+     * Rotates the robot to face the speaker, while still allowing the driver to control forward and backward movement
+     * @param swerveSubsystem The instance of {@link SwerveSubsystem}
+     * @param forward The desired forward percentage of the robot
+     * @param sideways The desired sideways percentage of the robot
+     * @param radians The desired rotation speed of the robot
+     */
+    public RotateToSpeakerAndDriveCommand(SwerveSubsystem swerveSubsystem, DoubleSupplier forward, DoubleSupplier sideways, DoubleSupplier radians) {
+        this.swerveSubsystem = swerveSubsystem;
+        this.forward = forward;
+        this.sideways = sideways;
+        this.radians = radians;
+
+        //Rotation tolerance in radians
+        rotationPID.setTolerance(0.05);
+
+        //Robot speed toward/away from the speaker (y-direction)
+        double robotYSpeed = swerveSubsystem.getFieldRelativeChassisSpeeds().vyMetersPerSecond;
+        //Robot speed toward/away from the speaker (x-direction)
+        double robotXSpeed = swerveSubsystem.getFieldRelativeChassisSpeeds().vxMetersPerSecond;
+        //Note speed in x-direction(forward, as opposed to up)
+        double noteSpeedX = robotXSpeed + ShooterConstants.shooterNoteSpeedX;
+
+        //Getting speaker pose relative to alliance color
+        Pose2d speakerPose = (DriverStation.getAlliance().get() == DriverStation.Alliance.Blue)? ShooterConstants.speakerPoseBlue : ShooterConstants.speakerPoseRed;
+        //Current robot pose
+        Pose2d robotPose = swerveSubsystem.getPose();
+
+        //'Moving' speaker position based on robot speed
+        double speakerYTranslation = robotYSpeed * ((speakerPose.getX()-robotPose.getX()) / noteSpeedX);
+
+        //Calculated angle to rotate to
+        double angle = Math.atan2((speakerPose.getY() + speakerYTranslation) - robotPose.getY(), speakerPose.getX()-robotPose.getX());
+
+        //Rotation PID Calculations
+        this.odometryRotation = rotationPID.calculate(robotPose.getRotation().getRadians(), angle);
+
+        addRequirements(swerveSubsystem);
+
+    }
+
+    @Override
+    public void execute() {
+
+        swerveSubsystem.drive(
+                forward.getAsDouble(),
+                sideways.getAsDouble(),
+                odometryRotation,
+                true,
+                true
+        );
+
+    }
+}

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

@@ -17,6 +17,8 @@ public class SpeakerCommand extends Command {
     /**
      * Command to shoot for the speaker once there is a note in the intake
      * @param indexerSubsystem Instance of {@link IndexerSubsystem}
+     * @param fieldRelativeSpeeds Instance of {@link ChassisSpeeds}
+     * @param swervePose Instance of {@link Pose2d}
      */
 
     public SpeakerCommand(IndexerSubsystem indexerSubsystem, ChassisSpeeds fieldRelativeSpeeds, Pose2d swervePose) {
@@ -28,11 +30,16 @@ public SpeakerCommand(IndexerSubsystem indexerSubsystem, ChassisSpeeds fieldRela
 
     @Override
     public void execute() {
+
+        //Robot speed toward the speaker
         double robotXSpeed = fieldRelativeSpeeds.vxMetersPerSecond;
+        //Note speed in x-direction(forward, as opposed to up)
         double noteSpeedX = robotXSpeed + ShooterConstants.shooterNoteSpeedX;
 
+        //Initial calculated distance to shoot from the speaker
         double shootingDistanceFromSpeaker = noteSpeedX * ShooterConstants.timeToSpeakerHeightWithGravity;
 
+        //Robot distance to speaker
         double robotDistanceFromSpeaker = Math.sqrt(
                 Math.pow(
                         (
@@ -52,14 +59,17 @@ public void execute() {
                 )
         );
 
+        //Difference between optimal location and current location
         double difference = robotDistanceFromSpeaker - shootingDistanceFromSpeaker;
 
+        //Accounting for indexer spinup time
         if (difference > 0) {
             shootingDistanceFromSpeaker += noteSpeedX * ShooterConstants.shootDelayTime;
         } else {
             shootingDistanceFromSpeaker -= noteSpeedX * ShooterConstants.shootDelayTime;
         }
 
+        //Removing negatives
         difference = Math.abs(robotDistanceFromSpeaker - shootingDistanceFromSpeaker);
 
         System.out.println("Robot Speed: " + robotXSpeed +
@@ -69,6 +79,7 @@ public void execute() {
                 "\n Difference: " + difference
         );
 
+        //Execute shoot
         if (difference < 0.4){
             indexerSubsystem.rotateAllWheelsPercent(1.0);
             System.out.println("Shooting!!!!!");

src/main/java/frc/robot/constants/VisionConstants.java

@@ -18,8 +18,8 @@ public class VisionConstants {
     public static final double alignRotationD = 0;
     public static final TrapezoidProfile.Constraints alignRotationConstraints = new TrapezoidProfile.Constraints(0.75, 0.5);
 
-    public static final double rotateToNoteP = 0.6;
-    public static final double rotateToNoteI = 0;
-    public static final double rotateToNoteD = 0;
-    public static final TrapezoidProfile.Constraints rotateToNoteConstraints = new TrapezoidProfile.Constraints(Math.PI, Math.PI/2);
+    public static final double rotateToP = 0.6;
+    public static final double rotateToI = 0;
+    public static final double rotateToD = 0;
+    public static final TrapezoidProfile.Constraints rotateToConstraints = new TrapezoidProfile.Constraints(Math.PI, Math.PI/2);
 }

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

@@ -7,6 +7,7 @@
 import edu.wpi.first.networktables.DoubleEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.Units;
+import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.constants.ShooterConstants;
 import frc.robot.utils.MathUtils;
@@ -26,6 +27,7 @@ public class ShooterSubsystem extends SubsystemBase {
     private final DoubleEntry ShooterSpeedBottom = NetworkTableInstance.getDefault()
             .getTable("Shooter").getDoubleTopic("Bottom").getEntry(0.0);
 
+
     public ShooterSubsystem(){
         topMotor.setInverted(true);
         bottomMotor.setInverted(true);
@@ -61,6 +63,13 @@ public void setVoltage(double v1, double v2) {
         bottomMotor.setVoltage(v2);
     }
 
+    /**
+     * Check if center limebreak is seeing something
+     * @return If the limebreak is seeing something
+     */
+
+
+
     @Override
     public void periodic() {
         ShooterSpeedBottom.set(getBottomMotorRPM());