yoooo

# Conflicts:
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Blue_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Red_V2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
#	TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java

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

@@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos1 = 0.34;
+    public static double spindexer_intakePos1 = 0.39;
 
     public static double spindexer_intakePos2 = 0.5;
 

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

@@ -6,8 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.arcrobotics.ftclib.controller.PIDFController;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.CRServo;
-import com.qualcomm.robotcore.hardware.DcMotorSimple;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
@@ -33,7 +31,7 @@ public class PIDServoTest extends LinearOpMode {
 
         PIDFController controller = new PIDFController(p, i, d, f);
 
-        controller.setTolerance(0);
+        controller.setTolerance(0.001);
         robot = new Robot(hardwareMap);
 
         telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
@@ -46,12 +44,14 @@ public class PIDServoTest extends LinearOpMode {
             controller.setPIDF(p, i, d, f);
 
             if (mode == 0) {
-                pos = robot.turr1Pos.getCurrentPosition();
+                pos = (double) ((double)robot.turr1Pos.getCurrentPosition() /1024.0) * ((double) 44.0 /(double)77.0);
+
 
                 double pid = controller.calculate(pos, target);
 
                 robot.turr1.setPower(pid);
                 robot.turr2.setPower(-pid);
+
             } else if (mode == 1) {
                 pos = scalar * ((robot.spin1Pos.getVoltage() - restPos) / 3.3);
 
@@ -71,4 +71,5 @@ public class PIDServoTest extends LinearOpMode {
         }
 
     }
+
 }

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

@@ -73,12 +73,18 @@ public class FlywheelV2 {
         targetVelocity = commandedVelocity;
         velo = getVelo(shooter1CurPos, shooter2CurPos);
         // Flywheel PID code here
-        if (targetVelocity - velo > 500) {
+        if (targetVelocity - velo > 4500) {
             powPID = 1.0;
-        } else if (velo - targetVelocity > 500) {
+        } else if (velo - targetVelocity > 4500) {
             powPID = 0.0;
         } else {
-            double feed = Math.log((668.39 / (targetVelocity + 591.96)) - 0.116) / -4.18;
+
+            double a = 2539.07863;
+            double c = 1967.6498;
+            double d = -0.289647;
+            double h = -1.1569;
+
+            double feed = Math.log10((a / (targetVelocity + c)) + d) / h;
 
             // --- PROPORTIONAL CORRECTION ---
             double error = targetVelocity - velo;

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

@@ -6,26 +6,24 @@ import com.qualcomm.robotcore.hardware.HardwareMap;
 
 @Config
 public class Servos {
-    Robot robot;
-
-    PIDFController spinPID;
-
-    PIDFController turretPID;
-
     //PID constants
     // TODO: get PIDF constants
-    public static double spinP = 3.4, spinI = 0, spinD = 0.075, spinF = 0.02;
+    public static double spinP = 3.3, spinI = 0, spinD = 0.1, spinF = 0.02;
-    public static double turrP = 4.0, turrI = 0.0, turrD = 0.0, turrF = 0.0;
+    public static double turrP = 1.1, turrI = 0.25, turrD = 0.0625, turrF = 0.01;
-
     public static double spin_scalar = 1.0086;
     public static double spin_restPos = 0.0;
     public static double turret_scalar = 1.009;
     public static double turret_restPos = 0.0;
+    Robot robot;
+    PIDFController spinPID;
+    PIDFController turretPID;
 
     public Servos(HardwareMap hardwareMap) {
         robot = new Robot(hardwareMap);
         spinPID = new PIDFController(spinP, spinI, spinD, spinF);
         turretPID = new PIDFController(turrP, turrI, turrD, turrF);
+
+        turretPID.setTolerance(0.001);
     }
 
     // In the code below, encoder = robot.servo.getVoltage()
@@ -33,6 +31,7 @@ public class Servos {
     public double getSpinPos() {
         return spin_scalar * ((robot.spin1Pos.getVoltage() - spin_restPos) / 3.3);
     }
+
     //TODO: PID warp so 0 and 1 are usable positions
     public double setSpinPos(double pos) {
         spinPID.setPIDF(spinP, spinI, spinD, spinF);