Servo twist starter for limelight angling (code is not finished)

GearUp12499-org · Jan 12, 2025 · f060255 · f060255
1 parent 853f4b1
commit f060255
Showing 1 changed file with 10 additions and 1 deletion.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimelightTestingTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/LimelightTestingTeleOp.java
@@ -25,6 +25,7 @@
 @TeleOp
 public class LimelightTestingTeleOp extends LinearOpMode {
     private Hardware hardware;
+
     private Limelight3A limelight;
 
     private static double distance(double x1, double y1, double x2, double y2) {
@@ -65,6 +66,14 @@ public double getAngle(List<double[]> longPair) {
         return Math.toDegrees(angle) - 90;
     }
 
+    public double ServoAngle(double angle) {
+        hardwareMap.get(clawTwist);
+        if (clockwise) {
+            return 0.48+(angle/270);
+        }
+        return 0.48-(angle/270);
+    }
+
     @Override
     public void runOpMode() {
         hardware = new Hardware(hardwareMap);
@@ -114,7 +123,7 @@ public void runOpMode() {
                 for (LLResultTypes.ColorResult colorTarget : colorTargets) {
                     List<double[]> corners = new ArrayList<double[]>();
                     for(List<Double> eachCorner: colorTarget.getTargetCorners()){
-                        if (corners.size() >= 4) {
+                        if (corners.size() == 4) {
                             break;
                         }
                         telemetry.addData("First Value of Corner", eachCorner);