Finished Even Sillier Nick Stuff

FRCTeam3044 · Jan 18, 2025 · 0396fa2 · 0396fa2
1 parent 23e45b2
commit 0396fa2
Showing 1 changed file with 10 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/subsystems/EndEffecter/EndEffecter.java b/src/main/java/frc/robot/subsystems/EndEffecter/EndEffecter.java
@@ -1,5 +1,7 @@
 package frc.robot.subsystems.EndEffecter;
 
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -32,28 +34,26 @@ public EndEffecter(EndEffecterIO io) {
         this.io = io;
     }
 
-    public Command pivot(LevelAngle levelAngles) {
-        return null;
-    }
 
     public Command manualpivot(double desiredspeed) {
         return null;
     }
 
-    public Command scoreL1() {
-        return Commands.run(() -> pivot(levelAngles.L1) , this).withName("Set wrist to L1 Scoring position");
+    public Command scoreL1(DoubleSupplier robotDistance) {
+        return Commands.run(() -> io.setAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L1)), this).withName("Set wrist to L1 Scoring position");
     }
 
-    public Command scoreL2AndL3() {
-        return null;
+    public Command scoreL2AndL3(DoubleSupplier robotDistance) {
+        return Commands.run(() -> io.setAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L23)), this).withName("Set wrist to L1 Scoring position");
     }
 
-    public Command scoreL4() {
-        return null;
+    public Command scoreL4(DoubleSupplier robotDistance) {
+        return Commands.run(() -> io.setAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L4)), this).withName("Set wrist to L1 Scoring position");
     }
 
     private double calculateAngleForDist(double robotDist, LevelAngle desiredLevel){
-
+        double angleForDist = ((desiredLevel.farAngle-desiredLevel.closeAngle) / (desiredLevel.farDist-desiredLevel.closeDist))*(robotDist - desiredLevel.closeDist) + desiredLevel.closeAngle;
+        return  angleForDist;
     }
 
 }