Implement shooting while moving vertically

src/main/java/frc4388/robot/RobotContainer.java

@@ -125,7 +125,7 @@ public class RobotContainer {
         // );
     
         private Command RobotRev = new SequentialCommandGroup(
-            new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
+            new InstantCommand(() -> m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds()), m_robotShooter),
             IntakeExtended,
             new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
         );
@@ -315,7 +315,7 @@ public class RobotContainer {
             .onTrue(new InstantCommand(() -> {
                 m_robotIntake.setMode(IntakeMode.Idle);
                 m_robotIntake.rollerStop();
-                m_robotShooter.spinUpShooting();
+                m_robotShooter.spinUpShooting(m_robotSwerveDrive.chassisXSpeeds());
             }))
             .onFalse(new InstantCommand(() -> {
                 m_robotShooter.spinUpIdle();
@@ -352,7 +352,7 @@ public class RobotContainer {
         //         m_robotClimber.toggleDeployed();
         //     }));
 
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
             .onTrue(new InstantCommand(() -> {
                 m_robotShooter.indexerStalled();
             }))

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -33,6 +33,8 @@ public class Shooter extends SubsystemBase {
     public boolean badShooterVelocity;
     public double distanceToHub = 5;
 
+    public double chassisXSpeed = 0;
+
 
     public Shooter(
         ShooterIO io,
@@ -66,7 +68,8 @@ public class Shooter extends SubsystemBase {
     private ShooterMode mode = ShooterMode.Idle;
     private boolean shooterButtonReady = false;
 
-    public void spinUpShooting() {
+    public void spinUpShooting(double chassisXSpeed) {
+        this.chassisXSpeed = chassisXSpeed;
         this.mode = ShooterMode.Shooting;
     }
 
@@ -157,14 +160,14 @@ public class Shooter extends SubsystemBase {
 
         //revtime calculations
         // double shooterAcceleration = 
-        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
+        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub,chassisXSpeed).in(RotationsPerSecond);
         double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
         // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
         Logger.recordOutput("Time to rev", revTime);
 
         switch (mode) {
             case Shooting:
-                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
+                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
 
                 int bitmask = (
                     (shooterButtonReady ? 1 : 0) +

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -36,7 +36,8 @@ public class    ShooterConstants {
 
     
     public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
-
+    public static final ConfigurableDouble CHASSIS_POS_OFFSET = new ConfigurableDouble("Chassis positive speed offset", 1.0);
+    public static final ConfigurableDouble CHASSIS_NEG_OFFSET = new ConfigurableDouble("Chassis negative speed offset", 1.0);
     // Shoot mode tolerances
     public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
     public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
@@ -49,16 +50,29 @@ public class    ShooterConstants {
     public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); 
 
     // 
-    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
+    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
         // Model derived from points
         // double speed = 
         //     1.11576*hubDistMeters*hubDistMeters +
         //     0.318464*hubDistMeters +
         //     30.6293;
-        double speed = 
+            double speed;
+        if (Math.abs(chassisXSpeed) < 0.1) {
+            speed = 
             0.0593402*hubDistMeters*hubDistMeters +
             4.90561*hubDistMeters +
             30.35696 + MODEL_TRIM.get();
+        } else if (chassisXSpeed > 0) { // positive = further from hub
+            speed = 
+            0.0593402*hubDistMeters*hubDistMeters +
+            4.90561*hubDistMeters +
+            30.35696 + (chassisXSpeed * CHASSIS_POS_OFFSET.get()) + MODEL_TRIM.get();
+        } else { // negative = moving towards hub
+            speed = 
+            0.0593402*hubDistMeters*hubDistMeters +
+            4.90561*hubDistMeters +
+            30.35696 + (chassisXSpeed * CHASSIS_NEG_OFFSET.get()) + MODEL_TRIM.get();
+        }
 
         // double speed =
         //     0.00610938*hubDistMeters*hubDistMeters
@@ -82,8 +96,8 @@ public class    ShooterConstants {
     // Motor Configuration
     public static Slot0Configs SHOOTER_PID = new Slot0Configs()
         .withKV(0.0)
-        .withKP(0.05)
-        .withKI(0.20)
+        .withKP(0.02)
+        .withKI(0.15)
         .withKD(0.0);
 
     
@@ -91,7 +105,7 @@ public class    ShooterConstants {
     
     
     
-    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
+    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02);
     public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
     public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
 

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -412,7 +412,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         if (ballVelocity > 1E-3 && Math.abs(chassisSpeeds.vyMetersPerSecond) > 1E-3){
             double aimOffset = chassisSpeeds.vyMetersPerSecond*(distanceToHub + SwerveDriveConstants.distanceTolerence.get())/(Math.abs(ballVelocity));
             fieldPos =  new Translation2d(fieldPos.getX(), fieldPos.getY() - aimOffset);
-            Logger.recordOutput("Offset Simple", aimOffset);
+            Logger.recordOutput("Offset Value", aimOffset);
 
         }
 
@@ -512,6 +512,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         ); // stop the modules without breaking
     }
 
+    public double chassisXSpeeds(){
+        if (TimesNegativeOne.isRed) {
+            return chassisSpeeds.vxMetersPerSecond;
+        } else {
+            return -chassisSpeeds.vxMetersPerSecond;
+        }
+    }
+
     public void stopModules() {
         // stopped = true;
         // swerveDriveTrain.setControl(new SwerveRequest.SwerveDriveBrake());