Turret improvements

CompBot/build.gradle

@@ -61,7 +61,6 @@ configurations {
 // Defining my dependencies. In this case, WPILib (+ friends), and vendor libraries.
 // Also defines JUnit 5.
 dependencies {
-    implementation 'org.apache.commons:commons-math3:3.6.1'
     annotationProcessor wpi.java.deps.wpilibAnnotations()
     implementation wpi.java.deps.wpilib()
     implementation wpi.java.vendor.java()

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

@@ -1,16 +1,36 @@
 package frc.robot;
 
+import static edu.wpi.first.units.Units.Inches;
 import static edu.wpi.first.units.Units.Meters;
 import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.units.measure.Distance;
+import frc.robot.aiming.AimConstraints;
+import frc.robot.aiming.AimStrategy;
+import frc.robot.aiming.PhysicsAim;
 
 public class Constants {
   // Checked the FIRST Game Manual and fixed the field dimensions.
   public static class FieldConstants {
+    /** The longer side, corresponds to X values */
     public static final Distance kFieldLength = Meters.of(16.541);
+    /** The shorter side, corresponds to Y values */
     public static final Distance kFieldWidth = Meters.of(8.069);
-
+    /** The length of the alliance zone, corresponds to X axis */
+    public static final Distance kAllianceZoneLength = Inches.of(182.11);
+
     public static final Pose3d kBlueHub = new Pose3d(4.632516, 4.011139, 1.83, Rotation3d.kZero);
+    public static final Pose3d kFeedTarget = new Pose3d(4.5, 2, 1.0, Rotation3d.kZero);
+  }
+
+  public static class AimConstants {
+    public static final AimStrategy kAim = new PhysicsAim(
+        new AimConstraints(
+            Rotation2d.fromDegrees(49.5), // Min pitch
+            Rotation2d.fromDegrees(72.0), // Max pitch
+            18), // Max output (speed)
+        2,
+        10);
   }
 }

CompBot/src/main/java/frc/robot/Robot.java

@@ -47,9 +47,9 @@ public Robot() {
   public void robotPeriodic() {
     FieldManager.getInstance().clearFuel();
 
+    robotContainer.superstructure.periodic();
     StatusSignalUtil.refreshAll();
     CommandScheduler.getInstance().run();
-    robotContainer.superstructure.periodic();
 
     FieldManager.getInstance().drawFuel();
 

CompBot/src/main/java/frc/robot/aiming/AimConstraints.java

@@ -0,0 +1,20 @@
+package frc.robot.aiming;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * A class representing the physical constraints of the shooter.
+ */
+public record AimConstraints(
+    Rotation2d minShooterAngle,
+    Rotation2d maxShooterAngle,
+    double maxOutput) {
+
+  /** Returns whether the given aim parameters satisfy this constraint */
+  public boolean check(AimParams params) {
+    boolean outputOk = params.output <= maxOutput();
+    boolean pitchOk = (params.pitch.getRadians() >= minShooterAngle.getRadians())
+        && (params.pitch.getRadians() <= maxShooterAngle.getRadians());
+    return outputOk && pitchOk;
+  }
+}

CompBot/src/main/java/frc/robot/aiming/AimMeasurement.java

@@ -2,12 +2,11 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.units.measure.Distance;
-import edu.wpi.first.units.measure.LinearVelocity;
 import edu.wpi.first.units.measure.Time;
 
 public record AimMeasurement(
       Distance distance,
       Rotation2d pitch,
-      LinearVelocity velocity,
+      double shooterControl,
       Time time) {
 }

CompBot/src/main/java/frc/robot/aiming/AimParams.java

@@ -1,17 +1,16 @@
 package frc.robot.aiming;
 
-import static edu.wpi.first.units.Units.Degrees;
-import static edu.wpi.first.units.Units.MetersPerSecond;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.units.measure.LinearVelocity;
-
-import frc.robot.util.OnboardLogger;
 
 /**
  * The parameters of a shooter at a particlar moment in time. This is the type that is returned when
  * a shot is generated, meaning that this is what is applied to each subystem.
  */
 public class AimParams {
+  /** The status of the parameters object. */
+  public AimStatus status = AimStatus.Unchecked;
+  /** The manner of speed control the paramters requires */
+  public SpeedControl control = SpeedControl.ProjectileVelocity;
   /** the launch angle of the fuel out of the robot. */
   public Rotation2d pitch = Rotation2d.kZero;
   /**
@@ -20,23 +19,56 @@ public class AimParams {
    * shooter.
    */
   public Rotation2d yaw = Rotation2d.kZero;
-  /** the velocity that the fuel should be ejected out at, relative to the robot. */
-  public LinearVelocity velocity = MetersPerSecond.zero();
+  /**
+   * the output that the fuel should be ejected out at. If control is ProjectileVelocity, then this
+   * is m/s relative to the robot.
+   */
+  public double output = 0.0;
   /** the tolerated error in the shot's pitch */
   public Rotation2d deltaPitch = Rotation2d.fromDegrees(4);
   /** the tolerated error in the shot's yaw */
   public Rotation2d deltaYaw = Rotation2d.fromDegrees(2);
   /** the tolerated error in the shot's velocity */
-  public LinearVelocity deltaVelocity = MetersPerSecond.of(0.35);
+  public double deltaOutput = 0.35;
+
+  public AimParams() {}
+
+  public AimParams(AimStatus status) {
+    this.status = status;
+  }
 
-  private OnboardLogger ologger = new OnboardLogger("AimParams");
+  public enum AimStatus {
+    /** The program has not yet evaluated the valididty of this parameters object */
+    Unchecked,
+    /** Not a possible shot, do not try to attempt. Values are invalid. */
+    Impossible,
+    /** A shot that is calculated to go in */
+    Possible;
+
+    public boolean isOk() {
+      return this == Possible;
+    }
+  }
+
+  public static AimParams impossible() {
+    return new AimParams(AimStatus.Impossible);
+  }
+
+  /** Returns whether the aim parameters calculated are feasible */
+  public boolean isOk() {
+    return status.isOk();
+  }
 
-  public AimParams() {
-    ologger.registerMeasurement("Pitch", () -> pitch.getMeasure(), Degrees);
-    ologger.registerMeasurement("Yaw", () -> yaw.getMeasure(), Degrees);
-    ologger.registerMeasurement("Velocity", () -> velocity, MetersPerSecond);
-    ologger.registerMeasurement("Error/Pitch", () -> deltaPitch.getMeasure(), Degrees);
-    ologger.registerMeasurement("Error/Yaw", () -> deltaYaw.getMeasure(), Degrees);
-    ologger.registerMeasurement("Error/Velocity", () -> deltaVelocity, MetersPerSecond);
+  public enum SpeedControl {
+    /**
+     * The velocity parameter of the {@link AimParams} is the projectile's desired velocity, in
+     * meters per second.
+     */
+    ProjectileVelocity,
+    /**
+     * The velocity parameter of the {@link AimParams} is the shooter's control input, in its
+     * appropriate units.
+     */
+    MechanismControl;
   }
 }

CompBot/src/main/java/frc/robot/aiming/AimStrategy.java

@@ -1,18 +1,15 @@
 package frc.robot.aiming;
 
-import frc.robot.superstructure.StateManager;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Translation2d;
 
 /**
  * An interface representing a method of aim-calculation; i.e. calculating what shot parameters are
  * necessary for a given robot configuration
  */
-public abstract class AimStrategy {
-  public AimParams params = new AimParams();
-
+public interface AimStrategy {
   /**
    * Updates the AimParams with the new calculated shot based on the robot's state
-   *
-   * @param state The {@link frc.robot.superstructure.Superstructure}'s state.
    */
-  public abstract AimParams update(StateManager state);
+  public AimParams update(Pose3d target, Pose3d shooter, Translation2d velocity);
 }