renamed goals, added tunnable numbers for each of them

Author: worker-bob

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

@@ -31,10 +31,10 @@ public class Elevator extends SubsystemBase {
 
     private static final LoggedTunableNumber stow =
             new LoggedTunableNumber("Elevator/Stow Height", kElevatorHeights.stow());
-    private static final LoggedTunableNumber l1 = new LoggedTunableNumber("Elevator/L1", kElevatorHeights.l1());
-    private static final LoggedTunableNumber l2 = new LoggedTunableNumber("Elevator/L2", kElevatorHeights.l2());
-    private static final LoggedTunableNumber l3 = new LoggedTunableNumber("Elevator/L3", kElevatorHeights.l3());
-    private static final LoggedTunableNumber l4 = new LoggedTunableNumber("Elevator/L4", kElevatorHeights.l4());
+    private static final LoggedTunableNumber l1 = new LoggedTunableNumber("Elevator/Setpoints/L1", kElevatorHeights.l1());
+    private static final LoggedTunableNumber l2 = new LoggedTunableNumber("Elevator/Setpoints/L2", kElevatorHeights.l2());
+    private static final LoggedTunableNumber l3 = new LoggedTunableNumber("Elevator/Setpoints/L3", kElevatorHeights.l3());
+    private static final LoggedTunableNumber l4 = new LoggedTunableNumber("Elevator/Setpoints/L4", kElevatorHeights.l4());
 
     @Getter
     @Setter

src/main/java/frc/robot/subsystems/intake/pivot/IntakePivot.java

@@ -1,6 +1,7 @@
 package frc.robot.subsystems.intake.pivot;
 
 import static frc.robot.subsystems.elevator.ElevatorConstants.*;
+import static frc.robot.subsystems.intake.pivot.IntakePivotConstants.kTolorance;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.lib.LoggedTunableNumber;
@@ -26,11 +27,16 @@ public class IntakePivot extends SubsystemBase {
     private static final LoggedTunableNumber kA = new LoggedTunableNumber("Intake Pivot/Gains/kA", kGains.kA());
     private static final LoggedTunableNumber kG = new LoggedTunableNumber("Intake Pivot/Gains/kG", kGains.kG());
 
+    private static final LoggedTunableNumber intake = new LoggedTunableNumber("Intake Pivot/Setpoints/intake", 0);
+    private static final LoggedTunableNumber l1 = new LoggedTunableNumber("Intake Pivot/Setpoints/L1", 50);
+    private static final LoggedTunableNumber handoff = new LoggedTunableNumber("Intake Pivot/Setpoints/handoff", 150);
+
+
     public enum IntakePivotGoal {
         IDLE(() -> 0), // TODO: set idle angle
-        TO_INTAKE(() -> 0), // TODO: Set intake angle
-        TO_SCOREL1(() -> 50), // TODO: set scoring angle
-        TO_INTAKE_HANDOFF(() -> 150); // TODO: set handoff angle
+        INTAKE(intake), // TODO: Set intake angle
+        L1(l1), // TODO: set scoring angle
+        HANDOFF(handoff); // TODO: set handoff angle
 
         @Getter
         private DoubleSupplier pivotAngle;
@@ -70,7 +76,7 @@ public void periodic() {
     @AutoLogOutput(key = "Intake Pivot/At Goal")
     public boolean atGoal() {
         return EqualsUtil.epsilonEquals(
-                inputs.position, currentGoal.getPivotAngle().getAsDouble());
+                inputs.position, currentGoal.getPivotAngle().getAsDouble(), kTolorance);
     }
 
     public void runVolts(double volts) {

src/main/java/frc/robot/subsystems/outtake/endeffector/EndEffector.java

@@ -22,11 +22,13 @@ public class EndEffector extends SubsystemBase {
     private static final LoggedTunableNumber kA = new LoggedTunableNumber("End Effector Roller/Gains/kA", kGains.kA());
     private static final LoggedTunableNumber kG = new LoggedTunableNumber("End Effector Roller/Gains/kG", kGains.kG());
 
+    private static final LoggedTunableNumber intake = new LoggedTunableNumber("End Effector Roller/Setpoint/intake",  0.0);
+
     private EndEffectorRollerGoal currentGoal;
 
     public enum EndEffectorRollerGoal {
         IDLE(() -> 0), // Should be the current angle
-        INTAKE(() -> 0); // temporary
+        INTAKE(intake); // temporary
 
         @Getter
         private DoubleSupplier rollerVel;

src/main/java/frc/robot/subsystems/outtake/wrist/Wrist.java

@@ -27,10 +27,10 @@ public class Wrist extends SubsystemBase {
     private static final LoggedTunableNumber kA = new LoggedTunableNumber("Wrist/Gains/kA", kGains.kA());
     private static final LoggedTunableNumber kG = new LoggedTunableNumber("Wrist/Gains/kG", kGains.kG());
 
-    private static final LoggedTunableNumber l1 = new LoggedTunableNumber("Wrist/Gains/l1", 0);
-    private static final LoggedTunableNumber l2 = new LoggedTunableNumber("Wrist/Gains/l2", 3);
-    private static final LoggedTunableNumber l3 = new LoggedTunableNumber("Wrist/Gains/l3", 2);
-    private static final LoggedTunableNumber l4 = new LoggedTunableNumber("Wrist/Gains/l4", 1);
+    private static final LoggedTunableNumber l1 = new LoggedTunableNumber("Wrist/Setpoints/l1", 0);
+    private static final LoggedTunableNumber l2 = new LoggedTunableNumber("Wrist/Setpoints/l2", 3);
+    private static final LoggedTunableNumber l3 = new LoggedTunableNumber("Wrist/Setpoints/l3", 2);
+    private static final LoggedTunableNumber l4 = new LoggedTunableNumber("Wrist/Setpoints/l4", 1);
     private static final LoggedTunableNumber handoff = new LoggedTunableNumber("Wrist/Setpoints/handoff", -60);
     private static final LoggedTunableNumber algae = new LoggedTunableNumber("Wrist/Setpoints/algae", 80);