here you go keshav

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java

@@ -0,0 +1,298 @@
+package org.firstinspires.ftc.teamcode.autonomous;
+
+import static org.firstinspires.ftc.teamcode.constants.Poses.*;
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*;
+import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.ParallelAction;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
+import com.acmerobotics.roadrunner.Vector2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.acmerobotics.roadrunner.Action;
+import com.acmerobotics.roadrunner.ftc.Actions;
+
+import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive;
+import org.firstinspires.ftc.teamcode.subsystems.AprilTag;
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+@Config
+@Autonomous
+public class redDaniel extends LinearOpMode {
+
+    Robot robot;
+
+    MultipleTelemetry TELE;
+
+    MecanumDrive drive;
+
+    AprilTag aprilTag;
+
+    int b1 = 0; // 0 = no ball, 1 = green, 2 = purple
+
+    int b2 = 0;// 0 = no ball, 1 = green, 2 = purple
+
+    int b3 = 0;// 0 = no ball, 1 = green, 2 = purple
+    // TODO: change this velocity PID
+    public Action initShooter(int velocity){
+        return new Action(){
+            double velo = 0.0;
+            double initPos = 0.0;
+            double stamp = 0.0;
+            double powPID = 0.0;
+            double ticker = 0.0;
+            public boolean run(@NonNull TelemetryPacket telemetryPacket){
+                velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp));
+                stamp = getRuntime();
+                initPos = (double) robot.shooter1.getCurrentPosition() / 2048;
+                if (Math.abs(velocity - velo) > initTolerance) {
+                    powPID = (double) velocity / maxVel;
+                    ticker = getRuntime();
+                } else if (velocity - velTolerance > velo) {
+                    powPID = powPID + 0.0001;
+                    ticker = getRuntime();
+                } else if (velocity + velTolerance < velo) {
+                    powPID = powPID - 0.0001;
+                    ticker  = getRuntime();
+                }
+                robot.shooter1.setPower(powPID);
+                robot.shooter2.setPower(powPID);
+                robot.transfer.setPower((powPID / 4) + 0.75);
+
+                return getRuntime() - ticker < 0.5;
+            }
+        };
+    }
+
+    public void Obelisk (){
+        // TODO: write the code to detect order
+    }
+
+    public Action Shoot(double spindexer){
+        return new Action() {
+            boolean transfer = false;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                robot.spin1.setPosition(spindexer);
+                robot.spin2.setPosition(1-spindexer);
+                if (scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01 && scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01){
+                    robot.transferServo.setPosition(transferServo_in);
+                    transfer = true;
+                }
+                if (scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) < transferServo_in + 0.01 && scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) > transferServo_in - 0.01 && transfer){
+                    robot.transferServo.setPosition(transferServo_out);
+                    return false;
+                }
+                return true;
+            }
+        };
+    }
+
+    public Action intake (){
+        return new Action() {
+            double position = 0.0;
+            final double intakeTime = 4.0; // TODO: change this so it serves as a backup
+            final double stamp = getRuntime();
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if ((getRuntime() % 0.3) >0.15) {
+                    position = spindexer_intakePos1 + 0.02;
+                } else {
+                    position = spindexer_intakePos1 - 0.02;
+                }
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1-position);
+
+                robot.intake.setPower(1);
+
+                return !(robot.pin1.getState() && robot.pin3.getState() && robot.pin5.getState()) || getRuntime() - stamp > intakeTime;
+            }
+        };
+    }
+
+    public Action ColorDetect (){
+        return new Action() {
+            int t1 = 0;
+            int t2 = 0;
+            int t3 = 0;
+            int tP1 = 0;
+            int tP2 = 0;
+            int tP3 = 0;
+            final double stamp = getRuntime();
+            final double detectTime = 3.0;
+            double position = 0.0;
+            @Override
+            public boolean run(@NonNull TelemetryPacket telemetryPacket) {
+                if ((getRuntime() % 0.3) >0.15) {
+                    position = spindexer_intakePos1 + 0.02;
+                } else {
+                    position = spindexer_intakePos1 - 0.02;
+                }
+                robot.spin1.setPosition(position);
+                robot.spin2.setPosition(1-position);
+                if (robot.pin1.getState()) {
+                    t1 += 1;
+                    if (robot.pin0.getState()){
+                        tP1 += 1;
+                    }
+                }
+                if (robot.pin3.getState()) {
+                    t2 += 1;
+                    if (robot.pin0.getState()){
+                        tP2 += 1;
+                    }
+                }
+                if (robot.pin5.getState()) {
+                    t3 += 1;
+                    if (robot.pin0.getState()){
+                        tP3 += 1;
+                    }
+                }
+                if (t1 > 20){
+                    if (tP1 > 20){
+                        b1 = 2;
+                    } else {
+                        b1 = 1;
+                    }
+                }
+                if (t2 > 20){
+                    if (tP2 > 20){
+                        b2 = 2;
+                    } else {
+                        b2 = 1;
+                    }
+                }
+                if (t3 > 20){
+                    if (tP3 > 20){
+                        b3 = 2;
+                    } else {
+                        b3 = 1;
+                    }
+                }
+                return !(b1 + b2 + b3 >= 5) || (getRuntime() - stamp < detectTime);
+
+            }
+        };
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+
+        robot = new Robot(hardwareMap);
+
+        TELE = new MultipleTelemetry(
+                telemetry, FtcDashboard.getInstance().getTelemetry()
+        );
+
+        drive = new MecanumDrive(hardwareMap, new Pose2d(
+                0, 0, 0
+        ));
+
+        aprilTag = new AprilTag(robot, TELE);
+
+        TrajectoryActionBuilder shoot0 = drive.actionBuilder(new Pose2d(0, 0, 0))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder pickup1 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .turnTo(Math.toRadians(h2))
+                .strafeToLinearHeading(new Vector2d(x2, y2), h2);
+
+        TrajectoryActionBuilder shoot1 = drive.actionBuilder(new Pose2d(x2, y2, h2))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder pickup2 = drive.actionBuilder(new Pose2d(x1, y1, h1))
+
+                .strafeToLinearHeading(new Vector2d(x2_b, y2_b), h2_b)
+
+                .strafeToLinearHeading(new Vector2d(x3, y3), h3);
+
+        TrajectoryActionBuilder shoot2 = drive.actionBuilder(new Pose2d(x3, y3, h3))
+                .strafeToLinearHeading(new Vector2d(x1, y1), h1);
+
+        TrajectoryActionBuilder park = drive.actionBuilder(new Pose2d(x1, y1, h1))
+                .strafeToLinearHeading(new Vector2d(x1, y1 + 30), h1);
+
+        while (opModeInInit()) {
+
+            if (gamepad2.dpadUpWasPressed()) {
+                hoodDefault -= 0.01;
+            }
+
+            if (gamepad2.dpadDownWasPressed()) {
+                hoodDefault += 0.01;
+            }
+
+            robot.hood.setPosition(hoodDefault);
+
+            robot.turr1.setPosition(turret_red);
+            robot.turr2.setPosition(1 - turret_red);
+
+            robot.transferServo.setPosition(transferServo_out);
+
+            aprilTag.initTelemetry();
+
+            aprilTag.update();
+
+            TELE.update();
+        }
+
+        waitForStart();
+
+        if (isStopRequested()) return;
+
+        if (opModeIsActive()) {
+
+            robot.hood.setPosition(hoodDefault);
+
+            Actions.runBlocking(
+                    new ParallelAction(
+                            shoot0.build()
+                    )
+            );
+
+
+
+            Actions.runBlocking(
+                    pickup1.build()
+            );
+
+            Actions.runBlocking(
+                    shoot1.build()
+            );
+
+
+            Actions.runBlocking(
+                    pickup2.build()
+            );
+
+            Actions.runBlocking(
+                    shoot2.build()
+            );
+
+
+            Actions.runBlocking(
+                    park.build()
+            );
+
+            drive.updatePoseEstimate();
+
+            teleStart = drive.localizer.getPose();
+
+            TELE.addLine("finished");
+
+            TELE.update();
+
+            sleep(2000);
+
+        }
+
+    }
+}

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

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.constants;
+
+public class Color {
+}

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

@@ -15,12 +15,12 @@ public class Poses {
 
     public static double x1 = 50, y1 = 0, h1 = 0;
 
-    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(135);
+    public static double x2 = 31, y2 = 32, h2 = Math.toRadians(140);
 
-    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(135);
+    public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
 
 
-    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(135);
+    public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
 
     public static Pose2d teleStart = new Pose2d(x1,-10,0);
 

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

@@ -6,21 +6,29 @@ import com.acmerobotics.dashboard.config.Config;
 @Config
 public class ServoPositions {
 
-    public static double spindexer_intakePos = 0.665;
+    public static double spindexer_intakePos1 = 0.665;
+
+    public static double spindexer_intakePos3 = 0.29;
+
+    public static double spindexer_intakePos2 = 0.99;
 
     public static double spindexer_outtakeBall3 = 0.845;
 
     public static double spindexer_outtakeBall2 = 0.48;
     public static double spindexer_outtakeBall1 = 0.1;
 
-    public static double transferServo_out = 0.13;
+    public static double transferServo_out = 0.15;
 
-    public static double transferServo_in = 0.4;
+    public static double transferServo_in = 0.38;
 
     public static double hoodDefault = 0.35;
 
-    public static double turret_red = 0.33;
-    public static double turret_blue = 0.3;
+    public static double hoodHigh = 0.21;
+
+    public static double hoodLow = 1.0;
+
+    public static double turret_red = 0.43;
+    public static double turret_blue = 0.4;
 
 
 }

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

@@ -7,4 +7,10 @@ public class ShooterVars {
     public static double turret_GearRatio = 0.9974;
 
     public static double turret_Range = 355;
+
+    public static int velTolerance = 300;
+
+    public static int initTolerance = 1000;
+
+    public static int maxVel = 4000;
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java

@@ -45,7 +45,7 @@ public class Shooter implements Subsystem {
 
     public double powPID = 1.0;
 
-    private double p = 0.0003, i = 0, d = 0.00001;
+    private double p = 0.0003, i = 0, d = 0.00001, f=0;
 
     private PIDFController controller;
     private double pow = 0.0;
@@ -133,7 +133,7 @@ public class Shooter implements Subsystem {
     }
 
     public double getMCPRPosition() {
-        return fly1.getCurrentPosition() / (2 * mcpr);
+        return (double) fly1.getCurrentPosition() / 4;
     }
 
     public void setShooterMode(String mode) { shooterMode = mode; }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Spindexer.java

@@ -9,8 +9,6 @@ import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
-import java.util.ArrayList;
-
 public class Spindexer implements Subsystem{
 
     private Servo s1;
@@ -118,14 +116,14 @@ public class Spindexer implements Subsystem{
     }
 
     public void intake () {
-        position = spindexer_intakePos;
+        position = spindexer_intakePos1;
     }
 
     public void intakeShake(double runtime) {
         if ((runtime % 0.25) >0.125) {
-            position = spindexer_intakePos + 0.04;
+            position = spindexer_intakePos1 + 0.04;
         } else {
-            position = spindexer_intakePos - 0.04;
+            position = spindexer_intakePos1 - 0.04;
 
         }
     }

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

@@ -49,67 +49,82 @@ public class ActiveColorSensorTest extends LinearOpMode {
         while (opModeIsActive()){
 
             if ((getRuntime() % 0.3) >0.15) {
-                position = spindexer_intakePos + 0.02;
+                position = spindexer_intakePos1 + 0.015;
             } else {
-                position = spindexer_intakePos - 0.02;
+                position = spindexer_intakePos1 - 0.015;
             }
             robot.spin1.setPosition(position);
             robot.spin2.setPosition(1-position);
 
             robot.intake.setPower(1);
 
+            // Reset the counters after 1 second of not reading a ball.
+            final double ColorCounterResetDelay = 1.0;
+            // Number of times the loop needs to run before deciding on a color.
+            final int ColorCounterTotalMinCount = 20;
+            // If the color sensor reads a color this percentage of time
+            // out of the total, declare the color.
+            // Usage: (Color Count)/(Total Count) > ColorCounterThreshold
+            final double ColorCounterThreshold  = 0.65;
+
             if (robot.pin1.getState()){
                 if (robot.pin0.getState()){
-                    b1Purple += 1;
-                    purpleStamp1 = getRuntime();
+                    b1Purple ++;
                 }
-                b1Total += 1;
+                b1Total++;
                 totalStamp1 = getRuntime();
             }
-            if (getRuntime() - totalStamp1 > 1){
+            if (getRuntime() - totalStamp1 > ColorCounterResetDelay) {
+                // Too Much time has passed without detecting ball
                 b1 = "none";
                 b1Total = 1;
                 b1Purple = 1;
-            }else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){
-                b1 = "Green";
-            }else if (b1Purple > 10){
+            }else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){
+            // Enough Time has passed and we met the threshold
                 b1 = "Purple";
+            }else if (b1Total > ColorCounterTotalMinCount) {
+            // Enough Time passed WITHOUT meeting the threshold
+                b1 = "Green";
             }
 
             if (robot.pin3.getState()){
                 if (robot.pin2.getState()){
-                    b2Purple += 1;
-                    purpleStamp2 = getRuntime();
+                    b2Purple ++;
                 }
-                b2Total += 1;
+                b2Total++;
                 totalStamp2 = getRuntime();
             }
-            if (getRuntime() - totalStamp2 > 1){
+            if (getRuntime() - totalStamp2 > ColorCounterResetDelay) {
+                // Too Much time has passed without detecting ball
                 b2 = "none";
                 b2Total = 1;
                 b2Purple = 1;
-            }else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){
-                b2 = "Green";
-            }else if (b2Purple > 10){
+            }else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){
+                // Enough Time has passed and we met the threshold
                 b2 = "Purple";
+            }else if (b2Total > ColorCounterTotalMinCount) {
+                // Enough Time passed WITHOUT meeting the threshold
+                b2 = "Green";
             }
 
             if (robot.pin5.getState()){
                 if (robot.pin4.getState()){
-                    b3Purple += 1;
-                    purpleStamp3 = getRuntime();
+                    b3Purple ++;
                 }
-                b3Total += 1;
+                b3Total++;
                 totalStamp3 = getRuntime();
             }
-            if (getRuntime() - totalStamp3 > 1){
+            if (getRuntime() - totalStamp3 > ColorCounterResetDelay) {
+                // Too Much time has passed without detecting ball
                 b3 = "none";
                 b3Total = 1;
                 b3Purple = 1;
-            }else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){
-                b3 = "Green";
-            }else if (b3Purple > 10){
+            }else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){
+                // Enough Time has passed and we met the threshold
                 b3 = "Purple";
+            }else if (b3Total > ColorCounterTotalMinCount) {
+                // Enough Time passed WITHOUT meeting the threshold
+                b3 = "Green";
             }
 
             TELE.addData("Green1:", robot.pin1.getState());

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

@@ -6,32 +6,58 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
 import org.firstinspires.ftc.teamcode.utils.Robot;
 
 @TeleOp
 @Config
-public class ColorSensorTest extends LinearOpMode{
+public class ColorSensorTest extends LinearOpMode {
     Robot robot;
     MultipleTelemetry TELE;
 
     @Override
-    public void runOpMode() throws InterruptedException{
+    public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
         TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
 
         waitForStart();
         if (isStopRequested()) return;
 
-        while (opModeIsActive()){
-            TELE.addData("Green1:", robot.pin1.getState());
-            TELE.addData("Purple1:", robot.pin0.getState());
-            TELE.addData("Green2:", robot.pin3.getState());
-            TELE.addData("Purple2:", robot.pin2.getState());
-            TELE.addData("Green3:", robot.pin5.getState());
-            TELE.addData("Purple3:", robot.pin4.getState());
+        while (opModeIsActive()) {
+
+// ----- COLOR 1 -----
+            double green1 = robot.color1.getNormalizedColors().green;
+            double blue1  = robot.color1.getNormalizedColors().blue;
+            double red1   = robot.color1.getNormalizedColors().red;
+
+            TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor());
+            TELE.addData("Color1 green", green1 / (green1 + blue1 + red1));
+            TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM));
+
+
+// ----- COLOR 2 -----
+            double green2 = robot.color2.getNormalizedColors().green;
+            double blue2  = robot.color2.getNormalizedColors().blue;
+            double red2   = robot.color2.getNormalizedColors().red;
+
+            TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor());
+            TELE.addData("Color2 green", green2 / (green2 + blue2 + red2));
+            TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM));
+
+
+// ----- COLOR 3 -----
+            double green3 = robot.color3.getNormalizedColors().green;
+            double blue3  = robot.color3.getNormalizedColors().blue;
+            double red3   = robot.color3.getNormalizedColors().red;
+
+            TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor());
+            TELE.addData("Color3 green", green3 / (green3 + blue3 + red3));
+            TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM));
+
 
             TELE.update();
         }
     }
 
+
 }

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

@@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.config.Config;
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
@@ -19,22 +20,22 @@ public class ShooterTest extends LinearOpMode {
 
     public static double pow = 0.0;
     public static double vel = 0.0;
-    public static double mcpr = 28.0; // CPR of the motor
     public static double ecpr = 1024.0; // CPR of the encoder
     public static double hoodPos = 0.5;
-    public static double posPower = 0.0;
-
     public static double turretPos = 0.9;
 
-    public static double p = 0.0003, i = 0, d = 0, f = 0;
+    public static String flyMode = "VEL";
 
-    public static String flyMode = "MANUAL";
-
-    public static String turrMode = "MANUAL";
+    public static boolean AutoTrack = false;
 
     double initPos = 0.0;
 
+    double velo = 0.0;
     double velo1 = 0.0;
+    double velo2 = 0.0;
+    double velo3 = 0.0;
+    double velo4 = 0.0;
+    double velo5 = 0.0;
 
     double stamp1 = 0.0;
 
@@ -42,12 +43,22 @@ public class ShooterTest extends LinearOpMode {
 
     double powPID = 0.0;
 
-    public static double maxVel = 4500;
-
-    public static int tolerance = 300;
+    public static int maxVel = 4500;
 
     public static boolean shoot = false;
 
+    public static int spindexPos = 1;
+
+    public static boolean intake = true;
+
+    public static int tolerance = 50;
+
+    double stamp = 0.0;
+
+    public static double kP = 0.001;           // small proportional gain (tune this)
+    public static double maxStep = 0.06;         // prevents sudden jumps
+    public static double distance = 50;
+
     MultipleTelemetry TELE;
 
     @Override
@@ -59,23 +70,31 @@ public class ShooterTest extends LinearOpMode {
 
         Shooter shooter = new Shooter(robot, TELE);
 
-        shooter.setTelemetryOn(true);
+        robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-        shooter.setTurretMode(turrMode);
+        shooter.setTelemetryOn(true);
 
         shooter.setShooterMode(flyMode);
 
-        shooter.setControllerCoefficients(p, i, d, f);
-
         initPos = shooter.getECPRPosition();
 
+        int ticker = 0;
+
         waitForStart();
 
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
 
-            shooter.setControllerCoefficients(p, i, d, f);
+            ticker++;
+
+            if (AutoTrack){
+                hoodPos = hoodAnglePrediction(distance);
+                vel = velPrediction(distance);
+            }
+
+
 
             shooter.setShooterMode(flyMode);
 
@@ -83,22 +102,67 @@ public class ShooterTest extends LinearOpMode {
 
             robot.hood.setPosition(hoodPos);
             robot.turr1.setPosition(turretPos);
-            robot.turr2.setPosition(turretPos);
-
-            velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
-            stamp1 = getRuntime();
-            initPos1 = shooter.getECPRPosition();
-            if (Math.abs(vel - velo1) > 3 * tolerance) {
-                powPID = vel / maxVel;
-            } else if (vel - tolerance > velo1) {
-                powPID = powPID + 0.001;
-            } else if (vel + tolerance < velo1) {
-                powPID = powPID - 0.001;
+            robot.turr2.setPosition(1 - turretPos);
+            if (intake) {
+                robot.transfer.setPower(0);
+                robot.intake.setPower(0.75);
+                robot.spin1.setPosition(spindexer_intakePos1);
+                robot.spin2.setPosition(1 - spindexer_intakePos1);
+            } else {
+                robot.transfer.setPower(.75 + (powPID/4));
+                robot.intake.setPower(0);
+                if (spindexPos == 1) {
+                    robot.spin1.setPosition(spindexer_outtakeBall1);
+                    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.spin1.setPosition(spindexer_outtakeBall3);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall3);
+                }
             }
-            shooter.setVelocity(powPID);
-            robot.transfer.setPower((powPID / 2) + 0.5);
 
-            if (shoot){
+            double penguin = 0;
+            if (ticker % 8 ==0){
+                penguin = shooter.getECPRPosition();
+                stamp = getRuntime();
+                velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
+                initPos1 = penguin;
+                stamp1 = stamp;
+            }
+
+
+            velo = velo1;
+
+            double feed = vel / maxVel;        // Example: vel=2500 → feed=0.5
+
+            if (vel > 500){
+                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
+            }
+
+            // --- PROPORTIONAL CORRECTION ---
+            double error = vel - velo1;
+            double correction = kP * error;
+
+            // limit how fast power changes (prevents oscillation)
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
+
+            // --- FINAL MOTOR POWER ---
+            powPID = feed + correction;
+
+            // clamp to allowed range
+            powPID = Math.max(0, Math.min(1, powPID));
+
+            if (vel - velo > 1000){
+                powPID = 1;
+            } else if (velo - vel > 1000){
+                powPID = 0;
+            }
+
+            shooter.setVelocity(powPID);
+
+            if (shoot) {
                 robot.transferServo.setPosition(transferServo_in);
             } else {
                 robot.transferServo.setPosition(transferServo_out);
@@ -106,17 +170,41 @@ public class ShooterTest extends LinearOpMode {
 
             shooter.update();
 
-            TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
-            TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
+            TELE.addData("Revolutions", shooter.getECPRPosition());
             TELE.addData("hoodPos", shooter.gethoodPosition());
             TELE.addData("turretPos", shooter.getTurretPosition());
             TELE.addData("Power Fly 1", robot.shooter1.getPower());
             TELE.addData("Power Fly 2", robot.shooter2.getPower());
             TELE.addData("powPID", shooter.getpowPID());
-            TELE.addData("Velocity", velo1);
+            TELE.addData("Velocity", velo);
             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;
+    }
+
 }

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

@@ -11,7 +11,7 @@ public class ConfigureColorRangefinder extends LinearOpMode {
 
     public static int LED_Brightness = 50;
 
-    public static int lowerGreen = 100;
+    public static int lowerGreen = 110;
 
     public static int higherGreen = 150;
 

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

@@ -1,6 +1,8 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.acmerobotics.dashboard.FtcDashboard;
 import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
@@ -8,13 +10,18 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 @Config
 public class PositionalServoProgrammer extends LinearOpMode {
     Robot robot;
+    MultipleTelemetry TELE;
     public static double spindexPos = 0.501;
     public static double turretPos = 0.501;
     public static double transferPos = 0.501;
     public static double hoodPos = 0.501;
+
+    public static double scalar = 1.112;
+    public static double restPos = 0.158;
     @Override
     public void runOpMode() throws InterruptedException {
         robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
         waitForStart();
         if (isStopRequested()) return;
         while (opModeIsActive()){
@@ -32,6 +39,15 @@ public class PositionalServoProgrammer extends LinearOpMode {
             if (hoodPos != 0.501){
                 robot.hood.setPosition(hoodPos);
             }
+            TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3));
+            TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3));
+            TELE.addData("spindexerA", robot.spin1Pos.getVoltage());
+            TELE.addData("hoodA", robot.hoodPos.getVoltage());
+            TELE.addData("transferServoA", robot.transferServoPos.getVoltage());
+            TELE.addData("turretA", robot.turr1Pos.getVoltage());
+            TELE.update();
         }
     }
 }

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

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.hardware.AnalogInput;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DcMotorEx;
@@ -7,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotorImplEx;
 import com.qualcomm.robotcore.hardware.DcMotorSimple;
 import com.qualcomm.robotcore.hardware.DigitalChannel;
 import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.I2cDevice;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
@@ -59,12 +61,30 @@ public class Robot {
 
     public AnalogInput analogInput2;
 
+    public AnalogInput spin1Pos;
+
+    public AnalogInput spin2Pos;
+
+    public AnalogInput hoodPos;
+
+    public AnalogInput turr1Pos;
+
+    public AnalogInput turr2Pos;
+
+    public AnalogInput transferServoPos;
+
     public AprilTagProcessor aprilTagProcessor;
 
     public WebcamName webcam;
 
     public DcMotorEx shooterEncoder;
 
+    public RevColorSensorV3 color1;
+
+    public RevColorSensorV3 color2;
+
+    public RevColorSensorV3 color3;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -92,14 +112,24 @@ public class Robot {
 
         hood = hardwareMap.get(Servo.class, "hood");
 
+        hoodPos = hardwareMap.get(AnalogInput.class, "hoodPos");
+
         turr1 = hardwareMap.get(Servo.class, "t1");
 
+        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos");
+
         turr2 = hardwareMap.get(Servo.class, "t2");
 
+        turr2Pos = hardwareMap.get(AnalogInput.class, "t2Pos");
+
         spin1 = hardwareMap.get(Servo.class, "spin1");
 
+        spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos");
+
         spin2 = hardwareMap.get(Servo.class, "spin2");
 
+        spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos");
+
         pin0 = hardwareMap.get(DigitalChannel.class, "pin0");
 
         pin1 = hardwareMap.get(DigitalChannel.class, "pin1");
@@ -120,11 +150,18 @@ public class Robot {
 
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
+        transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos");
+
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
 
         aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults();
 
         webcam = hardwareMap.get(WebcamName.class, "Webcam 1");
 
+        color1 = hardwareMap.get(RevColorSensorV3.class, "c1");
+
+        color2 = hardwareMap.get(RevColorSensorV3.class, "c2");
+
+        color3 = hardwareMap.get(RevColorSensorV3.class, "c3");
     }
 }