mockup of shooting on the move

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

@@ -12,6 +12,7 @@
 // GNU General Public License for more details.
 
 package frc.robot;
+import com.fasterxml.jackson.core.SerializableString;
 import edu.wpi.first.math.geometry.*;
 import com.gos.lib.properties.GosDoubleProperty;
 import com.pathplanner.lib.path.PathConstraints;
@@ -275,6 +276,9 @@ private ArmSetpoints() {
   }
 
   public static class ShooterConstants {
+    public static final GosDoubleProperty ACCEL_COMP_FACTOR =
+        new GosDoubleProperty(false, "Shooter/Acceleration Compensation", 0.100);
+
     private ShooterConstants() {
       throw new IllegalStateException("Static classes should not be constructed");
     }

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

@@ -185,23 +185,26 @@ private void configureButtonBindings() {
     intakeTrigger.whileTrue(m_shooter.intakeCommand(0.75, 0.5, 0.1)
         .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE)));
 
-    spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false)
-        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))
-        .alongWith(DriveCommands.alignmentDrive(
-            m_driveSubsystem,
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
-        )));
+//    spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false)
+//        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))
+//        .alongWith(DriveCommands.alignmentDrive(
+//            m_driveSubsystem,
+//            () -> -controller.getLeftY(),
+//            () -> -controller.getLeftX(),
+//            () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
+//        )));
+    spinUpTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), false));
 
-    shootTrigger.whileTrue(m_shooter.runShooterVelocity(true)
-        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))
-        .alongWith(DriveCommands.alignmentDrive(
-            m_driveSubsystem,
-            () -> -controller.getLeftY(),
-            () -> -controller.getLeftX(),
-            () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
-        )));
+//    shootTrigger.whileTrue(m_shooter.runShooterVelocity(true)
+//        .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM))
+//        .alongWith(DriveCommands.alignmentDrive(
+//            m_driveSubsystem,
+//            () -> -controller.getLeftY(),
+//            () -> -controller.getLeftX(),
+//            () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER)
+//        )));
+
+    shootTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), true));
 
     ampLineupTrigger.whileTrue(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)
         .finallyDo(() -> m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP).schedule()))

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

@@ -4,17 +4,22 @@
 import com.gos.lib.properties.pid.WpiPidPropertyBuilder;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.Constants;
+import frc.robot.Constants.ShooterConstants;
 import frc.robot.subsystems.arm.ArmSubsystem;
 import frc.robot.subsystems.drive.DriveSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
 import lib.utils.AimbotUtils;
+import lib.utils.FieldConstants;
+import lib.utils.FieldRelativeAccel;
+import lib.utils.FieldRelativeSpeed;
 
 
 public class AimbotCommand extends Command {
@@ -72,8 +77,35 @@ public void execute() {
     m_smallProperty.updateIfChanged();
     m_fastProperty.updateIfChanged();
 
+    // get the robots velocity and acceleration
+    FieldRelativeSpeed fieldRelativeSpeed = m_driveSubsystem.getFieldRelativeVelocity();
+    FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration();
+
+    // TODO make an actual equation for shot time based on distance
+    double shotTime = 0.5;
+
+    Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d();
+    Translation2d movingTarget = new Translation2d();
+
+    // loop over movement calcs to better adjust for acceleration
+    if (false) {
+      for (int i = 0; i < 5; i++) {
+        double virtualGoalX = target.getX()
+            - shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue());
+        double virtualGoalY = target.getY()
+            - shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue());
+
+        movingTarget = new Translation2d(virtualGoalX, virtualGoalY);
+      }
+    } else {
+      movingTarget = target;
+    }
+
     // get our desired rotation and error from it
-    double desiredRotationDegs = AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()).getDegrees();
+    double desiredRotationDegs =
+        AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(),
+            new Translation3d(movingTarget.getX(), movingTarget.getY(), 0.0))
+            .getDegrees();
     double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees());
 
     // if we're far from our setpoint, move faster

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

@@ -52,10 +52,7 @@
 
 import lib.logger.DataLogUtil;
 import lib.logger.DataLogUtil.DataLogTable;
-import lib.utils.AimbotUtils;
-import lib.utils.AllianceFlipUtil;
-import lib.utils.LocalADStarAK;
-import lib.utils.PoseEstimator;
+import lib.utils.*;
 import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
@@ -89,7 +86,6 @@ public class DriveSubsystem extends SubsystemBase {
           new Rotation2d(),
           new Rotation2d()
   };
-  private Rotation2d m_avgRotationRads = new Rotation2d();
 
   private final VisionSubsystem[] m_cameras;
 
@@ -105,6 +101,9 @@ public class DriveSubsystem extends SubsystemBase {
       new SwerveModuleState()
   };
 
+  private FieldRelativeSpeed m_fieldRelVel = new FieldRelativeSpeed();
+  private FieldRelativeSpeed m_lastFieldRelVel = new FieldRelativeSpeed();
+  private FieldRelativeAccel m_fieldRelAccel = new FieldRelativeAccel();
 
   public DriveSubsystem(
       GyroIO gyroIO,
@@ -246,24 +245,6 @@ public void periodic() {
         m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta));
       }
 
-      // Update average heading, used for aiming
-      m_prevRotations = new Rotation2d[] {
-              m_rawGyroRotation,
-              m_prevRotations[1],
-              m_prevRotations[2],
-              m_prevRotations[3],
-              m_prevRotations[4]
-      };
-
-      double totalRotationRads = 0.0;
-
-      for (Rotation2d mPrevRotation : m_prevRotations) {
-        totalRotationRads += mPrevRotation.getRadians();
-      }
-
-      totalRotationRads /= m_prevRotations.length;
-      m_avgRotationRads = new Rotation2d(totalRotationRads);
-
       // Apply update
       m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions);
     }
@@ -291,6 +272,11 @@ public void periodic() {
         -AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees());
 
     m_field.setRobotPose(getVisionPose());
+
+    // calculate field relative velocities and acceleration
+    m_fieldRelVel = new FieldRelativeSpeed(kinematics.toChassisSpeeds(getModuleStates()), getRotation());
+    m_fieldRelAccel = new FieldRelativeAccel(m_fieldRelVel, m_lastFieldRelVel, 0.02);
+    m_lastFieldRelVel = m_fieldRelVel;
   }
 
   /**
@@ -386,6 +372,16 @@ public Rotation2d getRotation() {
     return m_wpiPoseEstimator.getEstimatedPosition().getRotation();
   }
 
+  /** Returns the robot's field relative velocity, used for shoot on move */
+  public FieldRelativeSpeed getFieldRelativeVelocity() {
+    return m_fieldRelVel;
+  }
+
+  /** Returns the robot's field relative acceleration, used for shoot on move */
+  public FieldRelativeAccel getFieldRelativeAcceleration() {
+    return m_fieldRelAccel;
+  }
+
   /** Resets the current odometry pose. */
   public void setPose(Pose2d pose) {
     gyroIO.resetGyro(pose.getRotation().getDegrees());

src/main/java/lib/utils/FieldRelativeAccel.java

@@ -0,0 +1,35 @@
+package lib.utils;
+
+public class FieldRelativeAccel {
+  public double ax;
+  public double ay;
+  public double alpha;
+
+  public FieldRelativeAccel(double ax, double ay, double alpha) {
+    this.ax = ax;
+    this.ay = ay;
+    this.alpha = alpha;
+  }
+
+  public FieldRelativeAccel(FieldRelativeSpeed newSpeed, FieldRelativeSpeed oldSpeed, double time) {
+    this.ax = (newSpeed.vx - oldSpeed.vx) / time;
+    this.ay = (newSpeed.vy - oldSpeed.vy) / time;
+    this.alpha = (newSpeed.omega - oldSpeed.omega) / time;
+
+    if (Math.abs(this.ax) > 6.0) {
+      this.ax = 6.0 * Math.signum(this.ax);
+    }
+    if (Math.abs(this.ay) > 6.0) {
+      this.ay = 6.0 * Math.signum(this.ay);
+    }
+    if (Math.abs(this.alpha) > 4 * Math.PI) {
+      this.alpha = 4 * Math.PI * Math.signum(this.alpha);
+    }
+  }
+
+  public FieldRelativeAccel() {
+    this.ax = 0.0;
+    this.ay = 0.0;
+    this.alpha = 0.0;
+  }
+}

src/main/java/lib/utils/FieldRelativeSpeed.java

@@ -0,0 +1,28 @@
+package lib.utils;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+
+public class FieldRelativeSpeed {
+  public double vx;
+  public double vy;
+  public double omega;
+
+  public FieldRelativeSpeed(double vx, double vy, double omega) {
+    this.vx = vx;
+    this.vy = vy;
+    this.omega = omega;
+  }
+
+  public FieldRelativeSpeed(ChassisSpeeds chassisSpeed, Rotation2d gyro) {
+    this(chassisSpeed.vxMetersPerSecond * gyro.getCos() - chassisSpeed.vyMetersPerSecond * gyro.getSin(),
+        chassisSpeed.vyMetersPerSecond * gyro.getCos() + chassisSpeed.vxMetersPerSecond * gyro.getSin(),
+        chassisSpeed.omegaRadiansPerSecond);
+  }
+
+  public FieldRelativeSpeed() {
+    this.vx = 0.0;
+    this.vy = 0.0;
+    this.omega = 0.0;
+  }
+}