diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java index c218f4c..602e3ef 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Shooter.java +++ b/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); - fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - - double powPID = controller.calculate(-getVelocity(velo), velocity); + powPID = 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); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index caaa023..5040729 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/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.setTargetPosition(pos); + shooter.sethoodPosition(pos); shooter.setTurretPosition(posi); @@ -93,22 +101,33 @@ public class ShooterTest extends LinearOpMode { if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); } - if (wait(time)){ - velo = 60*((shooter.getECPRPosition() - initPos) / time); - stamp = getRuntime(); - initPos = shooter.getECPRPosition(); + velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); + stamp1 = getRuntime(); + initPos1 = 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(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index 0a518a5..27b7963 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/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, 0, 40); // purple + crf.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 2, 80); // 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