Configurable Linear Interpolation

src/main/java/frc/robot/statemachine/StateMachine.java

@@ -51,7 +51,7 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle
                 // Teleop
                 ManualState manual = new ManualState(this, driverController, operatorController, drive, elevator, LEDs);
                 ScoreGamePiece scoreGamePiece = new ScoreGamePiece(this);
-                ScoreCoral scoreCoral = new ScoreCoral(this, buttonBoard, drive, endEffector, LEDs);
+                ScoreCoral scoreCoral = new ScoreCoral(this, buttonBoard, drive, endEffector, elevator, shoulder, LEDs);
                 ScoreAlgae scoreAlgae = new ScoreAlgae(this);
                 ScoreAlgaeNet scoreAlgaeNet = new ScoreAlgaeNet(this, buttonBoard, drive, endEffector, LEDs);
                 ScoreAlgaeProcessor scoreAlgaeProcessor = new ScoreAlgaeProcessor(this, buttonBoard, drive, endEffector,

src/main/java/frc/robot/statemachine/states/tele/ManualState.java

@@ -50,7 +50,5 @@ public ManualState(StateMachineBase stateMachine, CommandXboxController driver,
 
                 driverController.a().onTrue(
                                 DriveCommands.goToPointJoystickRot(drive, new Pose2d(3, 3, new Rotation2d()), rotVel));
-
-                startWhenActive(elevator.toL1(() -> 0.0));
         }
 }

src/main/java/frc/robot/statemachine/states/tele/ScoreCoral.java

@@ -1,19 +1,26 @@
 package frc.robot.statemachine.states.tele;
 
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.statemachine.reusable.State;
 import frc.robot.statemachine.reusable.StateMachineBase;
 import frc.robot.subsystems.EndEffector.EndEffector;
 import frc.robot.subsystems.LEDs.LEDs;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveCommands;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.shoulder.Shoulder;
 import frc.robot.util.ButtonBoardUtil;
 
 public class ScoreCoral extends State {
     public ScoreCoral(StateMachineBase stateMachine, ButtonBoardUtil buttonBoard, Drive drive, EndEffector endEffector,
-            LEDs LEDs) {
+            Elevator elevator, Shoulder shoulder, LEDs LEDs) {
         super(stateMachine);
 
         startWhenActive(DriveCommands.pointControl(drive, buttonBoard.getCoralReefTarget(), null));
+        startWhenActive(Commands.deferredProxy(
+                () -> elevator.toCoral(buttonBoard.getCoralReefLevel(), buttonBoard.getCoralReefTargetDist(drive))));
+        startWhenActive(Commands.deferredProxy(
+                () -> shoulder.scoreCoral(buttonBoard.getCoralReefLevel(), buttonBoard.getCoralReefTargetDist(drive))));
         startWhenActive(endEffector.runIntakeReverse());
         startWhenActive(LEDs.intakingAndScoringCoral());
     }

src/main/java/frc/robot/subsystems/elevator/Elevator.java

@@ -9,30 +9,17 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.util.ConfigurableLinearInterpolation;
+import frc.robot.util.AutoTargetUtils.Reef.CoralLevel;
 
 public class Elevator extends SubsystemBase {
     private final ElevatorIO io;
     private final ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
     private final SysIdRoutine sysId;
-
-    enum LevelHeight {
-        L1(0, 1, 0, 0),
-        L2(0, 0, 0, 0),
-        L3(0, 0, 0, 0),
-        L4(0, 0, 0, 0);
-
-        private double closeDist;
-        private double closeHeight;
-        private double farDist;
-        private double farHeight;
-
-        private LevelHeight(double closeDist, double closeHeight, double farDist, double farHeight) {
-            this.closeHeight = closeHeight;
-            this.closeDist = closeDist;
-            this.farDist = farDist;
-            this.farHeight = farHeight;
-        }
-    };
+    private final ConfigurableLinearInterpolation L1 = new ConfigurableLinearInterpolation("Elevator L1 Heights");
+    private final ConfigurableLinearInterpolation L2 = new ConfigurableLinearInterpolation("Elevator L2 Heights");
+    private final ConfigurableLinearInterpolation L3 = new ConfigurableLinearInterpolation("Elevator L3 Heights");
+    private final ConfigurableLinearInterpolation L4 = new ConfigurableLinearInterpolation("Elevator L4 Heights");
 
     public Elevator(ElevatorIO io) {
         this.io = io;
@@ -68,30 +55,24 @@ public Command elevatorMove(DoubleSupplier speed) {
                 .withName("Elevator manual move");
     }
 
-    public Command toL1(DoubleSupplier robotDistance) {
-        return Commands.run(() -> io.setPosition(calculateAngleForDist(robotDistance.getAsDouble(), LevelHeight.L1)))
+    public Command toCoral(CoralLevel level, DoubleSupplier robotDistance) {
+        return Commands.run(() -> io.setPosition(getHeightForCoral(level, robotDistance.getAsDouble())))
                 .withName("Set elevator to L1 Scoring level");
     }
 
-    public Command toL2(DoubleSupplier robotDistance) {
-        return Commands.run(() -> io.setPosition(calculateAngleForDist(robotDistance.getAsDouble(), LevelHeight.L2)))
-                .withName("Set elevator to L2 Scoring level");
-    }
-
-    public Command toL3(DoubleSupplier robotDistance) {
-        return Commands.run(() -> io.setPosition(calculateAngleForDist(robotDistance.getAsDouble(), LevelHeight.L4)))
-                .withName("Set elevator to L3 Scoring level");
-    }
-
-    public Command toL4(DoubleSupplier robotDistance) {
-        return Commands.run(() -> io.setPosition(calculateAngleForDist(robotDistance.getAsDouble(), LevelHeight.L4)))
-                .withName("Set elevator to L4 Scoring level");
-    }
-
-    private double calculateAngleForDist(double robotDist, LevelHeight desiredLevel) {
-        double heightForDist = ((desiredLevel.farHeight - desiredLevel.closeHeight)
-                / (desiredLevel.farDist - desiredLevel.closeDist)) * (robotDist - desiredLevel.closeHeight);
-        return heightForDist;
+    private double getHeightForCoral(CoralLevel level, double distance) {
+        switch (level) {
+            case L1:
+                return L1.calculate(distance);
+            case L2:
+                return L2.calculate(distance);
+            case L3:
+                return L3.calculate(distance);
+            case L4:
+                return L4.calculate(distance);
+            default:
+                return 0;
+        }
     }
 
     public ElevatorIOInputsAutoLogged getInputs() {

src/main/java/frc/robot/subsystems/shoulder/Shoulder.java

@@ -9,29 +9,17 @@
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.util.ConfigurableLinearInterpolation;
+import frc.robot.util.AutoTargetUtils.Reef.CoralLevel;
 
 public class Shoulder extends SubsystemBase {
     private final ShoulderIO io;
     private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged();
     private final SysIdRoutine sysId;
-
-    enum LevelAngle {
-        L1(0, 0, 0, 0),
-        L23(0, 0, 0, 0),
-        L4(0, 0, 0, 0);
-
-        private double closeDist;
-        private double closeAngle;
-        private double farDist;
-        private double farAngle;
-
-        private LevelAngle(double closeDist, double closeAngle, double farDist, double farAngle) {
-            this.closeAngle = closeAngle;
-            this.closeDist = closeDist;
-            this.farDist = farDist;
-            this.farAngle = farAngle;
-        }
-    };
+    private final ConfigurableLinearInterpolation L1 = new ConfigurableLinearInterpolation("Shoulder L1 Angles");
+    private final ConfigurableLinearInterpolation L2 = new ConfigurableLinearInterpolation("Shoulder L2 Angles");
+    private final ConfigurableLinearInterpolation L3 = new ConfigurableLinearInterpolation("Shoulder L3 Angles");
+    private final ConfigurableLinearInterpolation L4 = new ConfigurableLinearInterpolation("Shoulder L4 Angles");
 
     public Shoulder(ShoulderIO io) {
         this.io = io;
@@ -67,29 +55,25 @@ public Command manualPivot(DoubleSupplier desiredSpeed) {
                 .withName("Shoulder Manual Pivot");
     }
 
-    public Command scoreL1(DoubleSupplier robotDistance) {
+    public Command scoreCoral(CoralLevel level, DoubleSupplier robotDistance) {
         return Commands
-                .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L1)), this)
+                .run(() -> io.setShoulderAngle(calculateAngleForCoral(level, robotDistance.getAsDouble())), this)
                 .withName("Set Shoulder to L1 Scoring position");
     }
 
-    public Command scoreL2AndL3(DoubleSupplier robotDistance) {
-        return Commands
-                .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L23)),
-                        this)
-                .withName("Set Shoulder to L1 Scoring position");
-    }
-
-    public Command scoreL4(DoubleSupplier robotDistance) {
-        return Commands
-                .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L4)), this)
-                .withName("Set Shoulder to L1 Scoring position");
-    }
-
-    private double calculateAngleForDist(double robotDist, LevelAngle desiredLevel) {
-        return ((desiredLevel.farAngle - desiredLevel.closeAngle)
-                / (desiredLevel.farDist - desiredLevel.closeDist)) * (robotDist - desiredLevel.closeDist)
-                + desiredLevel.closeAngle;
+    private double calculateAngleForCoral(CoralLevel level, double robotDist) {
+        switch (level) {
+            case L1:
+                return L1.calculate(robotDist);
+            case L2:
+                return L2.calculate(robotDist);
+            case L3:
+                return L3.calculate(robotDist);
+            case L4:
+                return L4.calculate(robotDist);
+            default:
+                return 0;
+        }
     }
 
     public double getShoulderAngle() {

src/main/java/frc/robot/util/ButtonBoardUtil.java

@@ -153,6 +153,10 @@ public Pose2d getCoralReefTarget() {
         return coralReefTargetPose;
     }
 
+    public CoralLevel getCoralReefLevel() {
+        return coralReefLevel;
+    }
+
     public DoubleSupplier getCoralReefTargetDist(Drive drive) {
         return () -> AutoTargetUtils.robotDistToPose(drive, coralReefReferencePose);
     }

src/main/java/frc/robot/util/ConfigurableLinearInterpolation.java

@@ -0,0 +1,53 @@
+package frc.robot.util;
+
+import java.util.ArrayList;
+import java.util.Collections;
+
+import me.nabdev.oxconfig.ConfigurableClass;
+import me.nabdev.oxconfig.ConfigurableClassParam;
+import me.nabdev.oxconfig.OxConfig;
+
+public class ConfigurableLinearInterpolation implements ConfigurableClass {
+    ConfigurableClassParam<Double> x1;
+    ConfigurableClassParam<Double> y1;
+    ConfigurableClassParam<Double> x2;
+    ConfigurableClassParam<Double> y2;
+    private String key;
+    private String prettyName;
+    private final ArrayList<ConfigurableClassParam<?>> params = new ArrayList<>();
+
+    public ConfigurableLinearInterpolation(String key) {
+        this.key = key;
+        this.prettyName = key;
+        x1 = new ConfigurableClassParam<Double>(this, 0.0, (x) -> {
+        }, "x1");
+        y1 = new ConfigurableClassParam<Double>(this, 0.0, (x) -> {
+        }, "y1");
+        x2 = new ConfigurableClassParam<Double>(this, 0.0, (x) -> {
+        }, "x2");
+        y2 = new ConfigurableClassParam<Double>(this, 0.0, (x) -> {
+        }, "y2");
+        Collections.addAll(params, x1, y1, x2, y2);
+        OxConfig.registerConfigurableClass(this);
+
+    }
+
+    @Override
+    public ArrayList<ConfigurableClassParam<?>> getParameters() {
+        return params;
+    }
+
+    @Override
+    public String getKey() {
+        return key;
+    }
+
+    @Override
+    public String getPrettyName() {
+        return prettyName;
+    }
+
+    public double calculate(double x) {
+        return y1.get() + (y2.get() - y1.get()) * (x - x1.get()) / (x2.get() - x1.get());
+    }
+}