From 77b42acdda34ca4add47a6121a03982d19f873eb Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 20 Nov 2025 21:11:06 -0600 Subject: [PATCH] encoder velocity finalized - adjusted color values --- .../ftc/teamcode/subsystems/Shooter.java | 31 +++++-------------- .../ftc/teamcode/tests/ShooterTest.java | 30 ++++++++++-------- .../utils/ConfigureColorRangefinder.java | 6 ++-- .../ftc/teamcode/utils/Robot.java | 2 +- 4 files changed, 27 insertions(+), 42 deletions(-) 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 c56478b..c218f4c 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 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(); } - } } 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 ea720c0..caaa023 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 @@ -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(); } 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 7d59ecd..0a518a5 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 @@ -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.setPin1Saturation(175, 255); - crf.setPin1Value(100,200); + 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.setPin0Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // purple - crf.setLedBrightness(0); + crf.setLedBrightness(1); waitForStart(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 5d30e32..f0f5f6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/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");