latest push

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 com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
 import com.acmerobotics.roadrunner.Pose2d;
 import com.arcrobotics.ftclib.controller.PIDController;
@@ -44,6 +43,8 @@ public class Shooter implements Subsystem {
 
     private int targetPosition = 0;
 
+    public double powPID = 1.0;
+
     private double p = 0.0003, i = 0, d = 0.00001;
 
     private PIDFController controller;
@@ -90,8 +91,6 @@ public class Shooter implements Subsystem {
 
     public void setTurretPosition(double pos) { turretPos = pos; }
 
-
-
     public double getVelocity(double vel) {
         return vel;
     }
@@ -224,6 +223,10 @@ public class Shooter implements Subsystem {
         turret2.setPosition(1 - pos);
     }
 
+    public double getpowPID() {
+        return powPID;
+    }
+
     @Override
     public void update() {
 
@@ -234,10 +237,7 @@ public class Shooter implements Subsystem {
             fly1.setPower(manualPower);
             fly2.setPower(manualPower);
         } else if (Objects.equals(shooterMode, "VEL")) {
-            fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+            powPID = velocity;
-            fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
-            double powPID = controller.calculate(-getVelocity(velo), velocity);
 
             fly1.setPower(powPID);
             fly2.setPower(powPID);
@@ -245,7 +245,7 @@ public class Shooter implements Subsystem {
         }
 
         if (Objects.equals(turretMode, "MANUAL")) {
-//            hoodServo.setPosition(hoodPos);
+            hoodServo.setPosition(hoodPos);
 
             moveTurret(turretPos);
 

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

@@ -19,7 +19,7 @@ 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 int pos = 0;
+    public static double pos = 0.0;
     public static double posPower = 0.0;
 
     public static double posi = 0.5;
@@ -38,10 +38,20 @@ public class ShooterTest extends LinearOpMode {
 
     public double initPos = 0.0;
 
-    public static double time = 1.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 int tolerance = 300;
+
     MultipleTelemetry TELE;
 
     public boolean wait(double time) {
@@ -81,9 +91,7 @@ public class ShooterTest extends LinearOpMode {
 
             shooter.setManualPower(pow);
 
-            shooter.setVelocity(vel);
+            shooter.sethoodPosition(pos);
-
-            shooter.setTargetPosition(pos);
 
             shooter.setTurretPosition(posi);
 
@@ -93,22 +101,33 @@ public class ShooterTest extends LinearOpMode {
 
             if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); }
 
-            if (wait(time)){
+            velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1));
-                velo = 60*((shooter.getECPRPosition() - initPos) / time);
+            stamp1 = getRuntime();
-                stamp = getRuntime();
+            initPos1 = shooter.getECPRPosition();
-                initPos = shooter.getECPRPosition();
+            if (Math.abs(vel - velo1) > 1500){
+                if (vel - velo1 > 0){
+                    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) {
+                powPID = powPID - 0.001;
             }
+            shooter.setVelocity(powPID);
 
             shooter.update();
 
             TELE.addData("ECPR Revolutions", shooter.getECPRPosition());
             TELE.addData("MCPR Revolutions", shooter.getMCPRPosition());
-            TELE.addData("Velocity", shooter.getVelocity(velo));
+            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.update();
 
         }

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

@@ -11,7 +11,6 @@ import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
 @Config
 @Autonomous
 
-
 public class ConfigureColorRangefinder extends LinearOpMode {
     @Override
     public void runOpMode() throws InterruptedException {
@@ -24,10 +23,9 @@ public class ConfigureColorRangefinder extends LinearOpMode {
         neither   --> no object
          */
         crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 60 / 360.0 * 255, 180 / 360.0 * 255); // green
-        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 20mm or closer requirement
+        crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 20mm or closer requirement
 
-
+        crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 2, 80); // purple
-        crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple
 
         crf.setLedBrightness(1);
 
@@ -86,6 +84,7 @@ class ColorRangefinder {
     public void setPin1Value(double lowerBound, double higherBound) {
         setDigital(PinNum.PIN1, DigitalMode.VALUE, lowerBound, higherBound);
     }
+
     /**
      * Sets the maximum distance (in millimeters) within which an object must be located for Pin 0's thresholds to trigger.
      * This is most useful when we want to know if an object is both close and the correct color.
@@ -108,7 +107,6 @@ class ColorRangefinder {
      * the color and look for blue.
      */
 
-
     /**
      * Invert the hue value before thresholding it, meaning that the colors become their opposite.
      * This is useful if we want to threshold red; instead of having two thresholds we would invert
@@ -162,6 +160,7 @@ class ColorRangefinder {
 
     /**
      * Read distance via I2C
+     *
      * @return distance in millimeters
      */
     public double readDistance() {
@@ -180,7 +179,8 @@ class ColorRangefinder {
         if (lowerBound == higherBound) {
             lo = (int) lowerBound;
             hi = (int) higherBound;
-        } else if (digitalMode.value <= DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range
+        } else if (digitalMode.value <=
+                   DigitalMode.VALUE.value) { // HSV/HUE/SATURATION/VALUE color range
             lo = (int) Math.round(lowerBound / 255.0 * 65535);
             hi = (int) Math.round(higherBound / 255.0 * 65535);
         } else { // distance in mm