update

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java

@@ -29,6 +29,7 @@ public class ServoPositions {
 
     public static double turret_red = 0.43;
     public static double turret_blue = 0.4;
+    public static double turret_range = 0.9;
 
 
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java

@@ -30,15 +30,14 @@ public class TeleopV2 extends LinearOpMode {
     boolean intake = false;
     boolean reject = false;
     int ticker = 0;
-    List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
     List<Double> s1G = new ArrayList<>();
     List<Double> s2G = new ArrayList<>();
     List<Double> s3G = new ArrayList<>();
     List<Boolean> s1 = new ArrayList<>();
     List<Boolean> s2 = new ArrayList<>();
     List<Boolean> s3 = new ArrayList<>();
-    double desiredTurretAngle = 180;
+    public static double desiredTurretAngle = 180;
-    MecanumDrive drive = new MecanumDrive(hardwareMap, teleStart);
+    MecanumDrive drive;
     private double lastEncoderRevolutions = 0.0;
     private double lastTimeStamp = 0.0;
     private double velo1, velo;
@@ -48,6 +47,8 @@ public class TeleopV2 extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
 
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+
         for (LynxModule hub : allHubs) {
             hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
         }
@@ -57,6 +58,8 @@ public class TeleopV2 extends LinearOpMode {
                 telemetry, FtcDashboard.getInstance().getTelemetry()
         );
 
+        drive = new MecanumDrive(hardwareMap, teleStart);
+
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()) {
@@ -125,6 +128,8 @@ public class TeleopV2 extends LinearOpMode {
 
                 if (gP >= 0.43) {
                     s1.add(true);
+                } else {
+                    s1.add(false);
                 }
             }
 
@@ -140,6 +145,8 @@ public class TeleopV2 extends LinearOpMode {
 
                 if (gP >= 0.43) {
                     s2.add(true);
+                } else {
+                    s2.add(false);
                 }
             }
 
@@ -155,12 +162,24 @@ public class TeleopV2 extends LinearOpMode {
 
                 if (gP >= 0.43) {
                     s3.add(true);
+                } else {
+                    s3.add(false);
                 }
             }
 
-            boolean green1 = s1.get(s1.size() - 1);
+            boolean green1 = false;
-            boolean green2 = s2.get(s2.size() - 1);
+            boolean green2 = false;
-            boolean green3 = s3.get(s3.size() - 1);
+            boolean green3 = false;
+
+            if (!s1.isEmpty()) {
+                green1 = s1.get(s1.size() - 1);
+            }
+            if (!s2.isEmpty()) {
+                green2 = s2.get(s2.size() - 1);
+            }
+            if (!s3.isEmpty()) {
+                green3 = s3.get(s3.size() - 1);
+            }
 
             //SHOOTER:
 
@@ -222,6 +241,29 @@ public class TeleopV2 extends LinearOpMode {
 
             }
 
+            //TURRET:
+
+            double offset;
+
+            offset = desiredTurretAngle - Math.toDegrees(drive.localizer.getPose().heading.toDouble());
+
+            if (offset > 90) {
+                offset -= 360;
+            }
+
+            double pos = 0.3;
+
+            pos -= offset * (0.9 / 360);
+
+            if (pos < 0.02) {
+                pos = 0.02;
+            } else if (pos > 0.91) {
+                pos = 0.91;
+            }
+
+            robot.turr1.setPosition(pos);
+            robot.turr2.setPosition(1 - pos);
+
             //MISC:
 
             drive.updatePoseEstimate();
@@ -230,9 +272,9 @@ public class TeleopV2 extends LinearOpMode {
                 hub.clearBulkCache();
             }
 
-            TELE.addData("Spin1Green", s1.get(s1.size() - 1));
+            TELE.addData("Spin1Green", green1);
-            TELE.addData("Spin2Green", s2.get(s2.size() - 1));
+            TELE.addData("Spin2Green", green2);
-            TELE.addData("Spin3Green", s3.get(s3.size() - 1));
+            TELE.addData("Spin3Green", green3);
 
             TELE.addData("pose", drive.localizer.getPose());
             TELE.addData("heading", drive.localizer.getPose().heading.toDouble());

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java

@@ -105,6 +105,8 @@ public class Robot {
 
         shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
 
+        shooterEncoder = shooter1;
+
         hood = hardwareMap.get(Servo.class, "hood");
 
         hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");