pid is good, fix color

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

@@ -13,7 +13,6 @@ 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.SequentialAction;
 import com.acmerobotics.roadrunner.TrajectoryActionBuilder;
 import com.acmerobotics.roadrunner.Vector2d;
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
@@ -37,6 +36,12 @@ public class redDaniel extends LinearOpMode {
 
     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;
@@ -67,6 +72,10 @@ public class redDaniel extends LinearOpMode {
         };
     }
 
+    public void Obelisk (){
+        // TODO: write the code to detect order
+    }
+
     public Action Shoot(double spindexer){
         return new Action() {
             boolean transfer = false;
@@ -87,6 +96,93 @@ public class redDaniel extends LinearOpMode {
         };
     }
 
+    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 {
 

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

@@ -6,7 +6,11 @@ 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;
 

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,9 +49,9 @@ 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);
@@ -126,4 +126,4 @@ public class ActiveColorSensorTest extends LinearOpMode {
         }
     }
 
-}
+}

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

@@ -29,6 +29,8 @@ public class ColorSensorTest extends LinearOpMode{
             TELE.addData("Purple2:", robot.pin2.getState());
             TELE.addData("Green3:", robot.pin5.getState());
             TELE.addData("Purple3:", robot.pin4.getState());
+            TELE.addData("Hello Keshav (analog)", robot.analogInput.getVoltage());
+            TELE.addData("Hello again (analog2)", robot.analogInput2.getVoltage());
 
             TELE.update();
         }

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

@@ -30,7 +30,12 @@ public class ShooterTest extends LinearOpMode {
 
     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;
 
@@ -38,7 +43,7 @@ public class ShooterTest extends LinearOpMode {
 
     double powPID = 0.0;
 
-    public static int maxVel = 4000;
+    public static int maxVel = 4500;
 
     public static boolean shoot = false;
 
@@ -48,7 +53,9 @@ public class ShooterTest extends LinearOpMode {
 
     public static int tolerance = 50;
 
-    public static double kP = 0.0005;           // small proportional gain (tune this)
+    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;
 
@@ -72,12 +79,16 @@ public class ShooterTest extends LinearOpMode {
 
         initPos = shooter.getECPRPosition();
 
+        int ticker = 0;
+
         waitForStart();
 
         if (isStopRequested()) return;
 
         while (opModeIsActive()) {
 
+            ticker++;
+
             if (AutoTrack){
                 hoodPos = hoodAnglePrediction(distance);
                 vel = velPrediction(distance);
@@ -85,69 +96,88 @@ public class ShooterTest extends LinearOpMode {
 
 
 
-                shooter.setShooterMode(flyMode);
+            shooter.setShooterMode(flyMode);
 
-                shooter.setManualPower(pow);
+            shooter.setManualPower(pow);
 
-                robot.hood.setPosition(hoodPos);
+            robot.hood.setPosition(hoodPos);
-                robot.turr1.setPosition(turretPos);
+            robot.turr1.setPosition(turretPos);
-                robot.turr2.setPosition(1 - turretPos);
+            robot.turr2.setPosition(1 - turretPos);
-                if (intake) {
+            if (intake) {
-                    robot.transfer.setPower(0);
+                robot.transfer.setPower(0);
-                    robot.intake.setPower(0.75);
+                robot.intake.setPower(0.75);
-                    robot.spin1.setPosition(spindexer_intakePos);
+                robot.spin1.setPosition(spindexer_intakePos1);
-                    robot.spin2.setPosition(1 - spindexer_intakePos);
+                robot.spin2.setPosition(1 - spindexer_intakePos1);
-                } else {
+            } else {
-                    robot.transfer.setPower(1);
+                robot.transfer.setPower(.75 + (powPID/4));
-                    robot.intake.setPower(0.75 + (powPID/4));
+                robot.intake.setPower(0);
-                    if (spindexPos == 1) {
+                if (spindexPos == 1) {
-                        robot.spin1.setPosition(spindexer_outtakeBall1);
+                    robot.spin1.setPosition(spindexer_outtakeBall1);
-                        robot.spin2.setPosition(1 - spindexer_outtakeBall1);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall1);
-                    } else if (spindexPos == 2) {
+                } else if (spindexPos == 2) {
-                        robot.spin1.setPosition(spindexer_outtakeBall2);
+                    robot.spin1.setPosition(spindexer_outtakeBall2);
-                        robot.spin2.setPosition(1 - spindexer_outtakeBall2);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall2);
-                    } else if (spindexPos == 3) {
+                } else if (spindexPos == 3) {
-                        robot.spin1.setPosition(spindexer_outtakeBall3);
+                    robot.spin1.setPosition(spindexer_outtakeBall3);
-                        robot.spin2.setPosition(1 - spindexer_outtakeBall3);
+                    robot.spin2.setPosition(1 - spindexer_outtakeBall3);
-                    }
                 }
+            }
 
-                velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
+            double penguin = 0;
-                stamp1 = getRuntime();
+            if (ticker % 8 ==0){
-                initPos1 = shooter.getECPRPosition();
+                penguin = shooter.getECPRPosition();
+                stamp = getRuntime();
+                velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1));
+                initPos1 = penguin;
+                stamp1 = stamp;
+            }
 
-                double feed = vel / maxVel;        // Example: vel=2500 → feed=0.5
 
-                // --- PROPORTIONAL CORRECTION ---
+            velo = velo1;
-                double error = vel - velo1;
-                double correction = kP * error;
 
-                // limit how fast power changes (prevents oscillation)
+            double feed = vel / maxVel;        // Example: vel=2500 → feed=0.5
-                correction = Math.max(-maxStep, Math.min(maxStep, correction));
 
-                // --- FINAL MOTOR POWER ---
+            if (vel > 500){
-                powPID = feed + correction;
+                feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18;
+            }
 
-                // clamp to allowed range
+            // --- PROPORTIONAL CORRECTION ---
-                powPID = Math.max(0, Math.min(1, powPID));
+            double error = vel - velo1;
-                shooter.setVelocity(powPID);
+            double correction = kP * error;
 
-                if (shoot) {
+            // limit how fast power changes (prevents oscillation)
-                    robot.transferServo.setPosition(transferServo_in);
+            correction = Math.max(-maxStep, Math.min(maxStep, correction));
-                } else {
-                    robot.transferServo.setPosition(transferServo_out);
-                }
 
-                shooter.update();
+            // --- FINAL MOTOR POWER ---
+            powPID = feed + correction;
 
-                TELE.addData("Revolutions", shooter.getECPRPosition());
+            // clamp to allowed range
-                TELE.addData("hoodPos", shooter.gethoodPosition());
+            powPID = Math.max(0, Math.min(1, powPID));
-                TELE.addData("turretPos", shooter.getTurretPosition());
+
-                TELE.addData("Power Fly 1", robot.shooter1.getPower());
+            if (vel - velo > 1000){
-                TELE.addData("Power Fly 2", robot.shooter2.getPower());
+                powPID = 1;
-                TELE.addData("powPID", shooter.getpowPID());
+            } else if (velo - vel > 1000){
-                TELE.addData("Velocity", velo1);
+                powPID = 0;
-                TELE.update();
+            }
+
+            shooter.setVelocity(powPID);
+
+            if (shoot) {
+                robot.transferServo.setPosition(transferServo_in);
+            } else {
+                robot.transferServo.setPosition(transferServo_out);
+            }
+
+            shooter.update();
+
+            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", velo);
+            TELE.update();
 
 
         }

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

@@ -22,10 +22,10 @@ public class ConfigureColorRangefinder extends LinearOpMode {
         /* Using this example configuration, you can detect both artifact colors based on which pin is reading true:
             pin0 --> purple
             pin1 --> green */
-        crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
+        crf.setPin0Analog(ColorRangefinder.AnalogMode.GREEN); // green
-        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
+//        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
-        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
+//        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
-        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
+//        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
 
         crf.setLedBrightness(LED_Brightness);
     }