color sensor test

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

@@ -0,0 +1,129 @@
+package org.firstinspires.ftc.teamcode.tests;
+
+import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*;
+
+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;
+
+import org.firstinspires.ftc.teamcode.utils.Robot;
+
+
+@TeleOp
+@Config
+public class ActiveColorSensorTest extends LinearOpMode {
+    Robot robot;
+    MultipleTelemetry TELE;
+
+    @Override
+    public void runOpMode() throws InterruptedException{
+        robot = new Robot(hardwareMap);
+        TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+        int b1Purple = 1;
+        int b1Total = 1;
+        int b2Purple = 1;
+        int b2Total = 1;
+        int b3Purple = 1;
+        int b3Total = 1;
+
+        double totalStamp1 = 0.0;
+        double purpleStamp1 = 0.0;
+        double totalStamp2 = 0.0;
+        double purpleStamp2 = 0.0;
+        double totalStamp3 = 0.0;
+        double purpleStamp3 = 0.0;
+
+        String b1 = "none";
+        String b2 = "none";
+        String b3 = "none";
+
+        double position = 0.0;
+
+        double stamp = getRuntime();
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        while (opModeIsActive()){
+
+            if ((getRuntime() % 0.3) >0.15) {
+                position = spindexer_intakePos + 0.02;
+            } else {
+                position = spindexer_intakePos - 0.02;
+            }
+            robot.spin1.setPosition(position);
+            robot.spin2.setPosition(1-position);
+
+            robot.intake.setPower(1);
+
+            if (robot.pin1.getState()){
+                if (robot.pin0.getState()){
+                    b1Purple += 1;
+                    purpleStamp1 = getRuntime();
+                }
+                b1Total += 1;
+                totalStamp1 = getRuntime();
+            }
+            if (getRuntime() - totalStamp1 > 1){
+                b1 = "none";
+                b1Total = 1;
+                b1Purple = 1;
+            }else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){
+                b1 = "Green";
+            }else if (b1Purple > 10){
+                b1 = "Purple";
+            }
+
+            if (robot.pin3.getState()){
+                if (robot.pin2.getState()){
+                    b2Purple += 1;
+                    purpleStamp2 = getRuntime();
+                }
+                b2Total += 1;
+                totalStamp2 = getRuntime();
+            }
+            if (getRuntime() - totalStamp2 > 1){
+                b2 = "none";
+                b2Total = 1;
+                b2Purple = 1;
+            }else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){
+                b2 = "Green";
+            }else if (b2Purple > 10){
+                b2 = "Purple";
+            }
+
+            if (robot.pin5.getState()){
+                if (robot.pin4.getState()){
+                    b3Purple += 1;
+                    purpleStamp3 = getRuntime();
+                }
+                b3Total += 1;
+                totalStamp3 = getRuntime();
+            }
+            if (getRuntime() - totalStamp3 > 1){
+                b3 = "none";
+                b3Total = 1;
+                b3Purple = 1;
+            }else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){
+                b3 = "Green";
+            }else if (b3Purple > 10){
+                b3 = "Purple";
+            }
+
+            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());
+            TELE.addData("1", b1);
+            TELE.addData("2",b2);
+            TELE.addData("3",b3);
+
+            TELE.update();
+        }
+    }
+
+}

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

@@ -19,10 +19,10 @@ public class ShooterTest extends LinearOpMode {
     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 pos = 0.0;
+    public static double hoodPos = 0.5;
     public static double posPower = 0.0;
 
-    public static double posi = 0.5;
+    public static double turretPos = 0.9;
 
     public static double p = 0.0003, i = 0, d = 0, f = 0;
 
@@ -30,34 +30,22 @@ public class ShooterTest extends LinearOpMode {
 
     public static String turrMode = "MANUAL";
 
-    public static int posTolerance = 40;
+    double initPos = 0.0;
 
-    public static double servoPosition = 0.501;
+    double velo1 = 0.0;
 
-    public double stamp = 0.0;
+    double stamp1 = 0.0;
 
-    public double initPos = 0.0;
+    double initPos1 = 0.0;
-
-    public static double time = 0.1;
-
-    public double velo = 0.0;
-
-    public double velo1 = 0.0;
-
-    public double stamp1 = 0.0;
-
-    public double initPos1 = 0.0;
 
     double powPID = 0.0;
 
+    public static double maxVel = 4500;
+
     public static int tolerance = 300;
 
     MultipleTelemetry TELE;
 
-    public boolean wait(double time) {
-        return (getRuntime() - stamp) > time;
-    }
-
     @Override
     public void runOpMode() throws InterruptedException {
 
@@ -85,31 +73,23 @@ public class ShooterTest extends LinearOpMode {
 
             shooter.setControllerCoefficients(p, i, d, f);
 
-            shooter.setTurretMode(turrMode);
-
             shooter.setShooterMode(flyMode);
 
             shooter.setManualPower(pow);
 
-            shooter.sethoodPosition(pos);
+            if (hoodPos != 0.501) {
-
+                robot.hood.setPosition(hoodPos);
-            shooter.setTurretPosition(posi);
+            }
-
-            shooter.setTolerance(posTolerance);
-
-            shooter.setPosPower(posPower);
-
-            if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
 
+            if (turretPos != 0.501) {
+                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) > 1500){
+            if (Math.abs(vel - velo1) > 3 * tolerance) {
-                if (vel - velo1 > 0){
+                powPID = vel / maxVel;
-                    powPID = 0.75;
-                } else if (vel - velo1 < 0){
-                    powPID = 0.25;
-                }
             } else if (vel - tolerance > velo1) {
                 powPID = powPID + 0.001;
             } else if (vel + tolerance < velo1) {
@@ -121,13 +101,12 @@ public class ShooterTest extends LinearOpMode {
 
             TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
             TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
-            TELE.addData("Velocity", velo);
             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("Ins Velocity", velo1);
+            TELE.addData("Velocity", velo1);
             TELE.update();
 
         }

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

@@ -1,13 +1,20 @@
 package org.firstinspires.ftc.teamcode.utils;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.hardware.rev.RevColorSensorV3;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
-
+@Config
-
 @TeleOp
 public class ConfigureColorRangefinder extends LinearOpMode {
+
+    public static int LED_Brightness = 35; //1 - 35, 2 - 50,
+
+    public static int lowerGreen = 90;
+
+    public static int higherGreen = 160;
+
     @Override
     public void runOpMode() throws InterruptedException {
         ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color"));
@@ -15,12 +22,12 @@ 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, 20); // green
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green
-        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 150 / 360.0 * 255, 360 / 360.0 * 255); // purple
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple
-        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, 100 / 360.0 *255);
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255);
-        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 10mm or closer requirement
+        crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement
 
-        crf.setLedBrightness(10);
+        crf.setLedBrightness(LED_Brightness);
     }
 }
 
@@ -135,6 +142,7 @@ class ColorRangefinder {
 
     /**
      * Read distance via I2C
+     *
      * @return distance in millimeters
      */
     public double readDistance() {