Add shoot on the move functionality.

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

@@ -13,14 +13,20 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Inches;
+
 import java.util.Set;
 
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.wpilibj.Alert;
 import edu.wpi.first.wpilibj.Alert.AlertType;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.RobotBase;
+import frc.robot.subsystems.shooter.ShooterConstants;
 
 /**
 * This class defines the runtime mode used by AdvantageKit. The mode is always "real" when running
@@ -39,9 +45,32 @@ public final class Constants {
     private static final RobotType robotType = RobotType.SIMBOT;
 
     public static final boolean spawnLessFuelInSim = true;
+    public static final boolean shootOnMove = true; // if true, we will allow the driver to shoot while moving, but with reduced max speed. if false, we will not allow the driver to shoot while moving.
+    //change to 0 if it really doesnt work, bc the db velocity will go to 0 and the target will just be the hub
+    //but I think it will work so yeah trust me butch <insert prayge hare>
+    public static final double shootOnMoveMaxSpeed = 2.0/5.0; 
+    public static final double aimOnMoveMaxSpeed = 2.0/3.0; // obsolete rn, but change if we ever add a aim mode again
 
     public static class DriveConstants {
         public static final double slowModeJoystickMultiplier = 0.4;
+
+        public static final Distance fieldLength = Inches.of(651.22);
+        public static final Distance fieldWidth = Inches.of(317.69);
+
+        public static final Distance allianceZoneBlue = Inches.of(156.61);
+        public static final Distance allianceZoneRed = fieldLength.minus(allianceZoneBlue);
+
+        public static final Translation2d blueLeftFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(5.0/6.0));
+        public static final Translation2d blueRightFerryTarget = new Translation2d(allianceZoneBlue.div(2.0), fieldWidth.times(1.0/6.0));
+        public static final Translation2d redLeftFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(5.0/6.0));
+        public static final Translation2d redRightFerryTarget = new Translation2d(fieldLength.minus(allianceZoneBlue.div(2.0)), fieldWidth.times(1.0/6.0));
+        public static Translation2d getHubTranslation(Alliance alliance){
+            return (
+                    alliance == Alliance.Blue
+                    ? ShooterConstants.Positions.blueHubPose
+                    : ShooterConstants.Positions.redHubPose
+                );
+        }
     }
 
     public static class FieldConstants {

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

@@ -50,7 +50,6 @@
 import frc.robot.subsystems.shooter.HoodIOServo;
 import frc.robot.subsystems.shooter.HoodIOSim;
 import frc.robot.subsystems.shooter.Shooter;
-import frc.robot.subsystems.shooter.RobotCommands;
 import frc.robot.subsystems.shooter.ShooterIO;
 import frc.robot.subsystems.shooter.ShooterIOSim;
 import frc.robot.subsystems.shooter.ShooterIOTalonFX;
@@ -250,7 +249,6 @@ public RobotContainer() {
         configureBindings();
         configureDriveBindings();
     }
-
     // Bind robot actions to commands here.
     private void configureBindings() {
         // Manually deploying and undeploying the intake.
@@ -270,15 +268,22 @@ private void configureBindings() {
             )
         );
 
-        // While the right trigger is held, we will shoot into the hub or ferry.
-        gamepad_.rightTrigger().whileTrue(RobotCommands.shoot(shooter_, hopper_, drivebase_));
-
-
         // When the hopper isnt shooting, set it to run its idle velocity.
         // hopper_.setDefaultCommand(hopper_.idleScrambler());
 
         // When the shooter isnt shooting, get it ready to shoot.
-        shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose));
+        shooter_.setDefaultCommand(shooter_.awaitShooting(drivebase_::getPose, drivebase_::getVirtualTarget));
+        // while in alliance zone, point drivebase at virtual target, but still allow translational driving
+        //lowkey we are not gonna need this but like maybe so idk imma just keep it
+
+        // While the right trigger is held, we will shoot into the hub.
+        // i wish i knew if this shoot command would interrrupt the other aim command, so this is kind of a "just in case", but if it is unnesecary it will be removed. 
+        //CHANGE BACK TO RIGHT TRIGGER!
+        // we might want to limit the acceleration on this while shooting, but idk how to do that and hopefully it wont matter too much. 
+        // just realized we could interrupt this with POV driving, but we would still be shooting, so we might want to create a block for that, but this too probably wont come up that much and i think i am not that numb-skulled to actually do this so idk
+        gamepad_.rightTrigger().whileTrue(
+            shooter_.shootCmd(drivebase_, hopper_, gamepad_, Constants.shootOnMove)
+        );
 
         //While the A button is held, the intake will run the eject sequence. If it the intake is stowed, it will also deploy it.
         gamepad_.a().whileTrue(intake_.ejectSequence());

src/main/java/frc/robot/commands/drive/DriveCommands.java

@@ -58,11 +58,15 @@
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.Mode;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.util.MapleSimUtil;
+import frc.robot.util.VirtualTarget;
 
 public class DriveCommands {
   private static final double kStoppedVelocity = 0.15 ;
@@ -238,6 +242,45 @@ public static Command joystickDriveAtAngle(
         // Reset PID controller when command starts
         .beforeStarting(() -> angleController.reset(drive.getRotation().getRadians()));
   }
+
+  public static Command pointAtTarget(Drive drive, CommandXboxController gamepad, Supplier<Translation2d> target, boolean shootOnMove){
+    return joystickDriveAtAngle(
+                    drive,
+                    () -> shootOnMove ? -gamepad.getLeftY() * Constants.shootOnMoveMaxSpeed : 0.0,
+                    () -> shootOnMove ? -gamepad.getLeftX() * Constants.shootOnMoveMaxSpeed : 0.0,
+                    () -> {
+                        Logger.recordOutput("Drive/Target", target.get());
+                        var translation = target.get().minus(drive.getPose().getTranslation());
+                        var rotation = new Rotation2d(translation.getX(), translation.getY());
+
+                        return rotation;
+                });
+  }
+
+  private static Translation2d getTarget(Drive drive) {
+    Translation2d target;
+
+    boolean blueDS = DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Blue;
+
+    boolean robotInAllianceZone = blueDS ? 
+      drive.getPose().getMeasureX().lt(DriveConstants.allianceZoneBlue):
+      drive.getPose().getMeasureX().gt(DriveConstants.allianceZoneRed);
+
+    if(robotInAllianceZone){
+      target = DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue));
+    }else if(drive.getPose().getMeasureY().gt(DriveConstants.fieldWidth.div(2.0))){
+        target = blueDS ? DriveConstants.blueLeftFerryTarget : DriveConstants.redLeftFerryTarget ;
+    }else{
+        target = blueDS ? DriveConstants.blueRightFerryTarget : DriveConstants.redRightFerryTarget ;
+    }
+
+    return target;
+  }
+
+  // make sure this isnt bad with intellisense later
+  public static Command pointAtShootingTarget(Drive drive, CommandXboxController gamepad, boolean shootOnMove){
+    return pointAtTarget(drive, gamepad, () -> (shootOnMove ? VirtualTarget.getVirtualTargetFromTarget(drive, getTarget(drive), ShooterConstants.hangTimeOnShot) : getTarget(drive)), shootOnMove);
+  }
 
   /**
    * Measures the velocity feedforward constants for the drive motors.

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -12,7 +12,6 @@
 // GNU General Public License for more details.
 
 package frc.robot.subsystems.drive;
-
 import static edu.wpi.first.units.Units.MetersPerSecond;
 import static edu.wpi.first.units.Units.Volts;
 
@@ -61,9 +60,12 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.Constants;
+import frc.robot.Constants.DriveConstants;
 import frc.robot.Constants.Mode;
+import frc.robot.subsystems.shooter.ShooterConstants;
 import frc.robot.generated.CompTunerConstants;
 import frc.robot.util.LocalADStarAK;
+import frc.robot.util.VirtualTarget;
 
 public class Drive extends SubsystemBase {
     // These Constants should be the same for every drivebase, so just use the comp bot constants.
@@ -219,10 +221,8 @@ public void periodic() {
             for (var module : modules) {
                 module.stop();
             }
-        }
 
-        // Log empty setpoint states when disabled
-        if (DriverStation.isDisabled()) {
+            // Log empty setpoint states when disabled
             Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {});
             Logger.recordOutput("SwerveStates/SetpointsOptimized", new SwerveModuleState[] {});
         }
@@ -466,6 +466,12 @@ public double getMaxAngularSpeedRadPerSec() {
     public RobotConfig getPathplannerConfig() {
         return PP_CONFIG;
     }
+
+
+
+    public Translation2d getVirtualTarget() {
+        return VirtualTarget.getVirtualTargetFromTarget(this, DriveConstants.getHubTranslation(DriverStation.getAlliance().orElse(Alliance.Blue)), ShooterConstants.hangTimeOnShot);
+    }
 
     /** Returns an array of module translations. */
     public Translation2d[] getModuleTranslations() {

src/main/java/frc/robot/subsystems/hopper/Hopper.java

@@ -141,6 +141,9 @@ public Command feed(AngularVelocity feeder, AngularVelocity scrambler) {
      * Runs the scrambler at its active speed, and the feeder.
      * @return
      */
+
+    // idk how to do this but like we should run the feeder motor backwards for about 0.5 secs to get arid of the balls stuck in there after we stop shooting
+    // theoretically this would improve consistency
     public Command forwardFeed() {
         return feed(HopperConstants.scramblerShootingVelocity, HopperConstants.feedingVelocity);
     }