Auto

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java

@@ -26,6 +26,8 @@ public class ShooterTest extends LinearOpMode {
 
     public static String flyMode = "VEL";
 
+    public static boolean AutoTrack = false;
+
     double initPos = 0.0;
 
     double velo1 = 0.0;
@@ -48,6 +50,7 @@ public class ShooterTest extends LinearOpMode {
 
     public static double kP = 0.0005;           // small proportional gain (tune this)
     public static double maxStep = 0.06;         // prevents sudden jumps
+    public static double distance = 50;
 
     MultipleTelemetry TELE;
 
@@ -75,30 +78,37 @@ public class ShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
+            if (AutoTrack){
+                hoodPos = hoodAnglePrediction(distance);
+                vel = velPrediction(distance);
+            }
+
+
+
                 shooter.setShooterMode(flyMode);
 
                 shooter.setManualPower(pow);
 
                 robot.hood.setPosition(hoodPos);
                 robot.turr1.setPosition(turretPos);
-            robot.turr2.setPosition(1-turretPos);
-            if (intake){
+                robot.turr2.setPosition(1 - turretPos);
+                if (intake) {
                     robot.transfer.setPower(0);
                     robot.intake.setPower(0.75);
                     robot.spin1.setPosition(spindexer_intakePos);
-                robot.spin2.setPosition(1-spindexer_intakePos);
+                    robot.spin2.setPosition(1 - spindexer_intakePos);
                 } else {
                     robot.transfer.setPower(1);
                     robot.intake.setPower(0);
-                if (spindexPos == 1){
+                    if (spindexPos == 1) {
                         robot.spin1.setPosition(spindexer_outtakeBall1);
-                    robot.spin2.setPosition(1-spindexer_outtakeBall1);
-                } else if (spindexPos == 2){
+                        robot.spin2.setPosition(1 - spindexer_outtakeBall1);
+                    } else if (spindexPos == 2) {
                         robot.spin1.setPosition(spindexer_outtakeBall2);
-                    robot.spin2.setPosition(1-spindexer_outtakeBall2);
-                } else if (spindexPos == 3){
+                        robot.spin2.setPosition(1 - spindexer_outtakeBall2);
+                    } else if (spindexPos == 3) {
                         robot.spin1.setPosition(spindexer_outtakeBall3);
-                    robot.spin2.setPosition(1-spindexer_outtakeBall3);
+                        robot.spin2.setPosition(1 - spindexer_outtakeBall3);
                     }
                 }
 
@@ -122,8 +132,7 @@ public class ShooterTest extends LinearOpMode {
                 powPID = Math.max(0, Math.min(1, powPID));
                 shooter.setVelocity(powPID);
 
-
-            if (shoot){
+                if (shoot) {
                     robot.transferServo.setPosition(transferServo_in);
                 } else {
                     robot.transferServo.setPosition(transferServo_out);
@@ -140,7 +149,32 @@ public class ShooterTest extends LinearOpMode {
                 TELE.addData("Velocity", velo1);
                 TELE.update();
 
+
         }
 
     }
+
+    public double hoodAnglePrediction(double distance) {
+        double L = 0.298317;
+        double A = 1.02124;
+        double k = 0.0157892;
+        double n = 3.39375;
+
+        double dist = Math.sqrt(distance*distance+24*24);
+
+        return L + A * Math.exp(-Math.pow(k * dist, n));
+    }
+    public static double velPrediction(double distance) {
+
+        double x = Math.sqrt(distance*distance+24*24);
+
+
+
+        double A = -211149.992;
+        double B = -1.19943;
+        double C = 3720.15909;
+
+        return A * Math.pow(x, B) + C;
+    }
+
 }