encoder velocity finalized - adjusted color values

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

@@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.subsystems;
 
 import static org.firstinspires.ftc.teamcode.tests.ShooterTest.*;
 
-import static java.lang.Runtime.getRuntime;
 
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
@@ -41,6 +40,8 @@ public class Shooter implements Subsystem {
     private double velocity = 0.0;
     private double posPower = 0.0;
 
+    public double velo = 0.0;
+
     private int targetPosition = 0;
 
     private double p = 0.0003, i = 0, d = 0.00001;
@@ -77,24 +78,6 @@ public class Shooter implements Subsystem {
 
     }
 
-    public void telemetryUpdate() {
-
-        // Telemetry
-
-        telemetry.addData("Mode", shooterMode);
-        telemetry.addData("ManualPower", manualPower);
-        telemetry.addData("ECPR Revolutions", getECPRPosition());
-        telemetry.addData("MCPR Revolutions", getMCPRPosition());
-        telemetry.addData("TargetPosition", targetPosition);
-        telemetry.addData("TargetVelocity", velocity);
-        telemetry.addData("hoodPos", gethoodPosition());
-        telemetry.addData("turretPos", getTurretPosition());
-        telemetry.addData("Power Fly 1", fly1.getPower());
-        telemetry.addData("Power Fly 2", fly2.getPower());
-        telemetry.addData("Pow", pow);
-
-    }
-
     public double gethoodPosition() {
         return (hoodServo.getPosition());
     }
@@ -107,8 +90,10 @@ public class Shooter implements Subsystem {
 
     public void setTurretPosition(double pos) { turretPos = pos; }
 
-    public double getVelocity(double initPos) {
+
-        return (getECPRPosition() - initPos);
+
+    public double getVelocity(double vel) {
+        return vel;
     }
 
     public void setVelocity(double vel) { velocity = vel; }
@@ -252,7 +237,7 @@ public class Shooter implements Subsystem {
             fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
             fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
 
-            double powPID = controller.calculate(fly1.getVelocity(), velocity);
+            double powPID = controller.calculate(-getVelocity(velo), velocity);
 
             fly1.setPower(powPID);
             fly2.setPower(powPID);
@@ -266,7 +251,5 @@ public class Shooter implements Subsystem {
 
         }
 
-        if (telemetryOn) { telemetryUpdate(); }
-
     }
 }

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

@@ -1,19 +1,10 @@
 package org.firstinspires.ftc.teamcode.tests;
 
-import android.app.Notification;
-
-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.Action;
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.PIDFCoefficients;
 
 import org.firstinspires.ftc.teamcode.utils.Robot;
 import org.firstinspires.ftc.teamcode.subsystems.Shooter;
@@ -33,7 +24,7 @@ public class ShooterTest extends LinearOpMode {
 
     public static double posi = 0.5;
 
-    public static double p = 0.001, i = 0, d = 0, f = 0;
+    public static double p = 0.0003, i = 0, d = 0, f = 0;
 
     public static String flyMode = "MANUAL";
 
@@ -49,6 +40,8 @@ public class ShooterTest extends LinearOpMode {
 
     public static double time = 1.0;
 
+    public double velo = 0.0;
+
     MultipleTelemetry TELE;
 
     public boolean wait(double time) {
@@ -72,6 +65,8 @@ public class ShooterTest extends LinearOpMode {
 
         shooter.setControllerCoefficients(p, i, d, f);
 
+        initPos = shooter.getECPRPosition();
+
         waitForStart();
 
         if (isStopRequested()) return;
@@ -98,13 +93,22 @@ public class ShooterTest extends LinearOpMode {
 
             if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
 
-            shooter.update();
-
             if (wait(time)){
-                telemetry.addData("Velocity", shooter.getVelocity(initPos));
+                velo = 60*((shooter.getECPRPosition() - initPos) / time);
                 stamp = getRuntime();
                 initPos = shooter.getECPRPosition();
             }
+
+            shooter.update();
+
+            TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
+            TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
+            TELE.addData("Velocity", shooter.getVelocity(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.update();
 
         }

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

@@ -23,15 +23,13 @@ public class ConfigureColorRangefinder extends LinearOpMode {
         only pin1 --> red
         neither   --> no object
          */
-        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 80 / 360.0 * 255, 140 / 360.0 * 255); // green
+        crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 60 / 360.0 * 255, 180 / 360.0 * 255); // green
-        crf.setPin1Saturation(175, 255);
-        crf.setPin1Value(100,200);
         crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 20mm or closer requirement
 
 
         crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple
 
-        crf.setLedBrightness(0);
+        crf.setLedBrightness(1);
 
         waitForStart();
 

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

@@ -88,7 +88,7 @@ public class Robot {
 
         shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2");
 
-        shooter2.setDirection(DcMotorSimple.Direction.REVERSE);
+        shooter1.setDirection(DcMotorSimple.Direction.REVERSE);
 
         hood = hardwareMap.get(Servo.class, "hood");