Flywheel calculated shot edits

Author: sophiep29

build.gradle

@@ -74,6 +74,8 @@ dependencies {
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    implementation 'org.apache.commons:commons-math3:3.6.1'
 }
 
 test {

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

@@ -113,25 +113,25 @@ public void execute() {
                 .withRotationalRate(
                     -rightX.getAsDouble()
                         * MaxAngularRate)); // Drive counterclockwise with negative X
-        System.out.println("TELEOP");
+        //        System.out.println("TELEOP");
         break;
 
       // (Needs check) Mode for when shots are from a still position
       case STILL_SHOT:
         // make X with wheels, set wheels to brake mode
         applyRequest.ModuleStates = states;
         subsystem.setControl(applyRequest);
-        System.out.println("Drivetrain: Still Shot Configuration");
+        //        System.out.println("Drivetrain: Still Shot Configuration");
         break;
 
       // (Incomplete) Mode for moving shots
       case SOTM:
         // shoot on the move, reference Mechanical Advantage build log
-        System.out.println("Drivetrain: SOTM Drive");
+        //        System.out.println("Drivetrain: SOTM Drive");
         break;
 
       case AUTO_ALIGN_HUB:
-        System.out.println("AUTO ALIGN");
+        //        System.out.println("AUTO ALIGN");
         // Recompute the desired heading toward the hub each loop
         m_targetAngle =
             ShotCalculator.getRotationTowardsHub(

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

@@ -59,10 +59,12 @@ public void execute() {
       // Spins flywheels at estimated speed given distance from hub
       case CALCULATED_SHOT:
         isOn = "true";
-
         subsystem.shoot(
             ShotCalculator.calculateFlywheelShot(drivetrain.getState().Pose.getTranslation()));
-        //         System.out.println("Flywheel: Calculated Shot");
+        System.out.println(
+            "Flywheel: Calculated Shot"
+                + ShotCalculator.calculateFlywheelShot(
+                    drivetrain.getState().Pose.getTranslation()));
         break;
 
       // Determines if robot should be prepared to shoot (if during active period or 5 seconds

src/main/java/frc/robot/helpers/ShotCalculator.java

@@ -7,16 +7,25 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import frc.robot.config.FlywheelConfig;
 import java.util.Optional;
+import org.apache.commons.math3.stat.*;
+import org.apache.commons.math3.stat.regression.SimpleRegression;
 
 public class ShotCalculator {
   /* Calculation Methods */
+  // Calculate variable shot
   public static double calculateFlywheelShot(Translation2d robotPosition) {
     // 1. Calculate distance between goal and robot position
     double distanceToGoal = calculateHubPosition().getDistance(robotPosition);
-    // 2. Retrieve correlating RPS from map
+    // 2. Retrieve correlating RPS from map based off of distance
+    if (distanceToGoal > Units.inchesToMeters(87) || distanceToGoal < Units.inchesToMeters(43)) {
+      System.out.println(getRPM(distanceToGoal));
+      return getRPM(distanceToGoal);
+    }
+    System.out.println(DISTANCE_TO_FLYWHEEL.get(distanceToGoal));
     return DISTANCE_TO_FLYWHEEL.get(distanceToGoal);
   }
 
+  // Determine if flywheel should be running before active period and run at mid level speed
   public static double calculateFlywheelDefaultShot(Translation2d robotPosition) {
     // 1. Determine if within shooter run period (five seconds before or during active period)
     if (shouldRunShooter(5)) {
@@ -144,8 +153,26 @@ public static boolean shouldRunShooter(double graceSeconds) {
     DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(141), 70.0);
     DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(90), 60.0);
     DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(61), 50.0);
-    DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(43), 40.0);
+    DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(43), 47.0);
     DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(74), 50.0);
     DISTANCE_TO_FLYWHEEL.put(Units.inchesToMeters(134), 65.0);
   }
+
+  // Distance (m) -> Speed (RPS)
+  private static final SimpleRegression regression = new SimpleRegression();
+
+  // Set inputs
+  static {
+    regression.addData(Units.inchesToMeters(174), 87.0);
+    regression.addData(Units.inchesToMeters(141), 70.0);
+    regression.addData(Units.inchesToMeters(90), 60.0);
+    regression.addData(Units.inchesToMeters(61), 50.0);
+    regression.addData(Units.inchesToMeters(43), 47.0);
+    regression.addData(Units.inchesToMeters(74), 50.0);
+    regression.addData(Units.inchesToMeters(134), 65.0);
+  }
+
+  public static double getRPM(double distance) {
+    return regression.getSlope() * distance + regression.getIntercept();
+  }
 }