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,6 +78,13 @@ public class ShooterTest extends LinearOpMode {
 
         while (opModeIsActive()) {
 
+            if (AutoTrack){
+                hoodPos = hoodAnglePrediction(distance);
+                vel = velPrediction(distance);
+            }
+
+
+
                 shooter.setShooterMode(flyMode);
 
                 shooter.setManualPower(pow);
@@ -122,7 +132,6 @@ public class ShooterTest extends LinearOpMode {
                 powPID = Math.max(0, Math.min(1, powPID));
                 shooter.setVelocity(powPID);
 
-
                 if (shoot) {
                     robot.transferServo.setPosition(transferServo_in);
                 } else {
@@ -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;
+    }
+
 }