From 8d75b245e35282e740543ecc0e3508c5cf0bf33e Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 18 Nov 2025 22:47:58 -0600 Subject: [PATCH] velocity math: to test --- .../ftc/teamcode/subsystems/Shooter.java | 26 +-- .../ftc/teamcode/teleop/TeleopV1.java | 198 ++++-------------- .../ftc/teamcode/tests/ShooterTest.java | 33 ++- .../ftc/teamcode/utils/Robot.java | 30 +-- 4 files changed, 88 insertions(+), 199 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 e2bb60c..c56478b 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,9 +2,12 @@ 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; import com.arcrobotics.ftclib.controller.PIDController; +import com.arcrobotics.ftclib.controller.PIDFController; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.PIDCoefficients; @@ -42,7 +45,7 @@ public class Shooter implements Subsystem { private double p = 0.0003, i = 0, d = 0.00001; - private PIDController controller; + private PIDFController controller; private double pow = 0.0; private String shooterMode = "AUTO"; @@ -62,9 +65,9 @@ public class Shooter implements Subsystem { fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - controller = new PIDController(p, i, d); + controller = new PIDFController(p, i, d, f); - controller.setPID(p, i, d); + controller.setPIDF(p, i, d, f); this.turret1 = robot.turr1; @@ -83,11 +86,9 @@ public class Shooter implements Subsystem { telemetry.addData("ECPR Revolutions", getECPRPosition()); telemetry.addData("MCPR Revolutions", getMCPRPosition()); telemetry.addData("TargetPosition", targetPosition); - telemetry.addData("Velocity", getVelocity(mcpr) * 60); telemetry.addData("TargetVelocity", velocity); telemetry.addData("hoodPos", gethoodPosition()); telemetry.addData("turretPos", getTurretPosition()); - telemetry.addData("PID Coefficients", "P: %.6f, I: %.6f, D: %.6f", p, i, d); telemetry.addData("Power Fly 1", fly1.getPower()); telemetry.addData("Power Fly 2", fly2.getPower()); telemetry.addData("Pow", pow); @@ -106,8 +107,8 @@ public class Shooter implements Subsystem { public void setTurretPosition(double pos) { turretPos = pos; } - public double getVelocity(double cpr) { - return 60 * (fly1.getVelocity() / (2 * cpr)); + public double getVelocity(double initPos) { + return (getECPRPosition() - initPos); } public void setVelocity(double vel) { velocity = vel; } @@ -122,11 +123,12 @@ public class Shooter implements Subsystem { controller.setTolerance(tolerance); } - public void setControllerCoefficients(double kp, double ki, double kd) { + public void setControllerCoefficients(double kp, double ki, double kd, double kf) { p = kp; i = ki; d = kd; - controller.setPID(p, i, d); + f = kf; + controller.setPIDF(p, i, d, f); } @@ -143,11 +145,11 @@ public class Shooter implements Subsystem { public String getTurretMode() { return turretMode; } public double getECPRPosition() { - return (fly1.getCurrentPosition() * 720 / ecpr); + return fly1.getCurrentPosition() / (2 * ecpr); } public double getMCPRPosition() { - return (fly1.getCurrentPosition() * 720) / mcpr; + return fly1.getCurrentPosition() / (2 * mcpr); } public void setShooterMode(String mode) { shooterMode = mode; } @@ -250,7 +252,7 @@ public class Shooter implements Subsystem { fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - double powPID = controller.calculate(getVelocity(mcpr) * ecpr, velocity); + double powPID = controller.calculate(fly1.getVelocity(), velocity); fly1.setPower(powPID); fly2.setPower(powPID); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java index 08bb18a..5f1c19c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV1.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Poses.*; 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; @@ -23,13 +22,11 @@ import org.firstinspires.ftc.teamcode.subsystems.Spindexer; import org.firstinspires.ftc.teamcode.subsystems.Transfer; import org.firstinspires.ftc.teamcode.utils.Robot; - @Config @TeleOp public class TeleopV1 extends LinearOpMode { - Robot robot; Drivetrain drivetrain; @@ -64,7 +61,6 @@ public class TeleopV1 extends LinearOpMode { ToggleButtonReader g2Square; - ToggleButtonReader g2Triangle; ToggleButtonReader g2RightBumper; @@ -86,7 +82,6 @@ public class TeleopV1 extends LinearOpMode { public double g1LeftBumperStamp = 0.0; - public double g2LeftBumperStamp = 0.0; public static int spindexerPos = 0; @@ -97,7 +92,7 @@ public class TeleopV1 extends LinearOpMode { public boolean scoreAll = false; - MecanumDrive drive ; + MecanumDrive drive; public boolean autotrack = false; @@ -118,16 +113,10 @@ public class TeleopV1 extends LinearOpMode { public boolean tri = false; - - @Override public void runOpMode() throws InterruptedException { - drive = new MecanumDrive(hardwareMap, teleStart); - - - - + drive = new MecanumDrive(hardwareMap, teleStart); robot = new Robot(hardwareMap); @@ -138,7 +127,7 @@ public class TeleopV1 extends LinearOpMode { g1 = new GamepadEx(gamepad1); - g1RightBumper = new ToggleButtonReader( + g1RightBumper = new ToggleButtonReader( g1, GamepadKeys.Button.RIGHT_BUMPER ); @@ -148,48 +137,42 @@ public class TeleopV1 extends LinearOpMode { g1, GamepadKeys.Button.LEFT_BUMPER ); - g2Circle = new ToggleButtonReader( + g2Circle = new ToggleButtonReader( g2, GamepadKeys.Button.B ); - g2Triangle = new ToggleButtonReader( + g2Triangle = new ToggleButtonReader( g2, GamepadKeys.Button.Y ); - g2Square = new ToggleButtonReader( + g2Square = new ToggleButtonReader( g2, GamepadKeys.Button.X ); - g2RightBumper = new ToggleButtonReader( + g2RightBumper = new ToggleButtonReader( g2, GamepadKeys.Button.RIGHT_BUMPER ); - - g2LeftBumper = new ToggleButtonReader( + g2LeftBumper = new ToggleButtonReader( g2, GamepadKeys.Button.LEFT_BUMPER ); - g2DpadUp = new ToggleButtonReader( + g2DpadUp = new ToggleButtonReader( g2, GamepadKeys.Button.DPAD_UP ); - - g2DpadDown = new ToggleButtonReader( + g2DpadDown = new ToggleButtonReader( g2, GamepadKeys.Button.DPAD_DOWN ); - g2DpadLeft = new ToggleButtonReader( + g2DpadLeft = new ToggleButtonReader( g2, GamepadKeys.Button.DPAD_LEFT ); - - g2DpadRight = new ToggleButtonReader( + g2DpadRight = new ToggleButtonReader( g2, GamepadKeys.Button.DPAD_RIGHT ); - - - drivetrain = new Drivetrain(robot, TELE, g1); drivetrain.setMode("Default"); @@ -202,7 +185,6 @@ public class TeleopV1 extends LinearOpMode { transfer = new Transfer(robot); - spindexer = new Spindexer(robot, TELE); spindexer.setTelemetryOn(true); @@ -213,17 +195,13 @@ public class TeleopV1 extends LinearOpMode { robot.rejecter.setPosition(rIn); - - - waitForStart(); if (isStopRequested()) return; - drive = new MecanumDrive(hardwareMap, teleStart); + drive = new MecanumDrive(hardwareMap, teleStart); - - while(opModeIsActive()){ + while (opModeIsActive()) { drive.updatePoseEstimate(); @@ -231,29 +209,25 @@ public class TeleopV1 extends LinearOpMode { TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); - TELE.addData("off", offset); robot.transferServo.setPosition(transferServoPos); - robot.hood.setPosition(pos); g1LeftBumper.readValue(); - if (g1LeftBumper.wasJustPressed()){ + if (g1LeftBumper.wasJustPressed()) { g2LeftBumperStamp = getRuntime(); - - spindexer.intakeShake(getRuntime()); leftBumper = true; } - if (leftBumper){ + if (leftBumper) { double time = getRuntime() - g2LeftBumperStamp; - if (time < 1.0){ + if (time < 1.0) { robot.rejecter.setPosition(rOut); } else { robot.rejecter.setPosition(rIn); @@ -261,9 +235,6 @@ public class TeleopV1 extends LinearOpMode { } - - - intake(); drivetrain.update(); @@ -280,63 +251,49 @@ public class TeleopV1 extends LinearOpMode { g2DpadUp.readValue(); - if (!scoreAll){ + if (!scoreAll) { spindexer.checkForBalls(); } - if(g2DpadUp.wasJustPressed()){ - pos -=0.02; + if (g2DpadUp.wasJustPressed()) { + pos -= 0.02; } - if(g2DpadDown.wasJustPressed()){ - pos +=0.02; + if (g2DpadDown.wasJustPressed()) { + pos += 0.02; } g2DpadLeft.readValue(); g2DpadRight.readValue(); - if(g2DpadLeft.wasJustPressed()){ - offset -=0.02; + if (g2DpadLeft.wasJustPressed()) { + offset -= 0.02; } - if(g2DpadRight.wasJustPressed()){ - offset +=0.02; + if (g2DpadRight.wasJustPressed()) { + offset += 0.02; } - - TELE.addData("hood", pos); - - - - - - - if (Math.abs(gamepad2.right_stick_x) < 0.1 && autotrack) { - - - shooter.trackGoal(drive.localizer.getPose(), new Pose2d(-10, 0, 0), offset); } else { autotrack = false; - shooter.moveTurret(0.3+offset); + shooter.moveTurret(0.3 + offset); } - if (gamepad2.right_stick_button){ + if (gamepad2.right_stick_button) { autotrack = true; } - - - if (g2RightBumper.wasJustPressed()){ + if (g2RightBumper.wasJustPressed()) { transfer.setTransferPower(1); transfer.transferIn(); shooter.setManualPower(1); @@ -345,19 +302,19 @@ public class TeleopV1 extends LinearOpMode { } - if (g2RightBumper.wasJustReleased()){ + if (g2RightBumper.wasJustReleased()) { transfer.setTransferPower(1); transfer.transferOut(); } - if (gamepad2.left_stick_y>0.5){ + if (gamepad2.left_stick_y > 0.5) { shooter.setManualPower(0); - } else if (gamepad2.left_stick_y<-0.5){ + } else if (gamepad2.left_stick_y < -0.5) { shooter.setManualPower(1); } - if (g2LeftBumper.wasJustPressed()){ + if (g2LeftBumper.wasJustPressed()) { g2LeftBumperStamp = getRuntime(); notShooting = false; scoreAll = true; @@ -366,7 +323,6 @@ public class TeleopV1 extends LinearOpMode { if (scoreAll) { double time = getRuntime() - g2LeftBumperStamp; - shooter.setManualPower(1); TELE.addData("greenImportant", green); @@ -378,7 +334,6 @@ public class TeleopV1 extends LinearOpMode { if (square) { - if (time < 0.3) { ticker = 0; @@ -386,7 +341,6 @@ public class TeleopV1 extends LinearOpMode { last = 0; second = 0; - transfer.transferOut(); transfer.setTransferPower(1); } else if (time < 2) { @@ -407,14 +361,12 @@ public class TeleopV1 extends LinearOpMode { ticker++; - } else if (time < 2.5) { ticker = 0; second = last; - transfer.transferIn(); } else if (time < 4) { transfer.transferOut(); @@ -434,11 +386,9 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - transfer.transferIn(); } else if (time < 6) { - transfer.transferOut(); if (ticker == 0) { @@ -459,7 +409,6 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - scoreAll = false; transfer.transferOut(); @@ -468,7 +417,6 @@ public class TeleopV1 extends LinearOpMode { } } else if (tri) { - if (time < 0.3) { ticker = 0; @@ -476,7 +424,6 @@ public class TeleopV1 extends LinearOpMode { last = 0; second = 0; - transfer.transferOut(); transfer.setTransferPower(1); } else if (time < 2) { @@ -497,14 +444,12 @@ public class TeleopV1 extends LinearOpMode { ticker++; - } else if (time < 2.5) { ticker = 0; second = last; - transfer.transferIn(); } else if (time < 4) { transfer.transferOut(); @@ -524,11 +469,9 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - transfer.transferIn(); } else if (time < 6) { - transfer.transferOut(); if (ticker == 0) { @@ -549,15 +492,13 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - scoreAll = false; transfer.transferOut(); shooter.setManualPower(0); } - } else if (circle){ - + } else if (circle) { if (time < 0.3) { @@ -566,7 +507,6 @@ public class TeleopV1 extends LinearOpMode { last = 0; second = 0; - transfer.transferOut(); transfer.setTransferPower(1); } else if (time < 2) { @@ -587,14 +527,12 @@ public class TeleopV1 extends LinearOpMode { ticker++; - } else if (time < 2.5) { ticker = 0; second = last; - transfer.transferIn(); } else if (time < 4) { transfer.transferOut(); @@ -614,11 +552,9 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - transfer.transferIn(); } else if (time < 6) { - transfer.transferOut(); if (ticker == 0) { @@ -639,15 +575,13 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - scoreAll = false; transfer.transferOut(); shooter.setManualPower(0); } - } else{ - + } else { if (time < 0.3) { @@ -669,7 +603,6 @@ public class TeleopV1 extends LinearOpMode { all = true; } - transfer.transferOut(); transfer.setTransferPower(1); } else if (time < 2) { @@ -694,7 +627,6 @@ public class TeleopV1 extends LinearOpMode { ticker++; - } else if (time < 2.5) { ticker = 0; @@ -711,7 +643,6 @@ public class TeleopV1 extends LinearOpMode { all = false; - } transfer.transferIn(); @@ -737,7 +668,6 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - if (gamepad2.right_trigger > 0.5) { green = false; @@ -752,7 +682,6 @@ public class TeleopV1 extends LinearOpMode { transfer.transferIn(); } else if (time < 6) { - transfer.transferOut(); if (ticker == 0) { @@ -775,7 +704,6 @@ public class TeleopV1 extends LinearOpMode { ticker = 0; - scoreAll = false; transfer.transferOut(); @@ -783,33 +711,16 @@ public class TeleopV1 extends LinearOpMode { } - } } - shooter.update(); - - - - - - - - - - - } - - - } - public void intake(){ - + public void intake() { g1RightBumper.readValue(); @@ -819,84 +730,63 @@ public class TeleopV1 extends LinearOpMode { g2Triangle.readValue(); - if (g1RightBumper.wasJustPressed()){ + if (g1RightBumper.wasJustPressed()) { notShooting = true; - - - - if (getRuntime() - g1RightBumperStamp < 0.3){ + if (getRuntime() - g1RightBumperStamp < 0.3) { intake.reverse(); } else { intake.toggle(); } - if (intake.getIntakeState()==1){ + if (intake.getIntakeState() == 1) { shooter.setManualPower(0); } - - - spindexer.intake(); transfer.transferOut(); - g1RightBumperStamp = getRuntime(); } - - if (intake.getIntakeState()==1 && notShooting) { + if (intake.getIntakeState() == 1 && notShooting) { spindexer.intakeShake(getRuntime()); } else { - if (g2Circle.wasJustPressed()){ + if (g2Circle.wasJustPressed()) { circle = true; tri = false; square = false; - - } - if (g2Triangle.wasJustPressed()){ + if (g2Triangle.wasJustPressed()) { circle = false; tri = true; square = false; } - if (g2Square.wasJustPressed()){ + if (g2Square.wasJustPressed()) { circle = false; tri = false; square = true; } - if (gamepad2.x){ + if (gamepad2.x) { circle = false; tri = false; square = false; } - - - } - intake.update(); - - - spindexer.update(); - - - - } } 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 de443ef..ea720c0 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,8 +1,14 @@ 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; @@ -20,12 +26,14 @@ public class ShooterTest extends LinearOpMode { public static double pow = 0.0; public static double vel = 0.0; - public static double mcpr = 28; // CPR of the motor + 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 posPower = 0.0; - public static double p = 0.001, i = 0, d = 0; + public static double posi = 0.5; + + public static double p = 0.001, i = 0, d = 0, f = 0; public static String flyMode = "MANUAL"; @@ -35,8 +43,18 @@ public class ShooterTest extends LinearOpMode { public static double servoPosition = 0.501; + public double stamp = 0.0; + + public double initPos = 0.0; + + public static double time = 1.0; + MultipleTelemetry TELE; + public boolean wait(double time) { + return (getRuntime() - stamp) > time; + } + @Override public void runOpMode() throws InterruptedException { @@ -52,7 +70,7 @@ public class ShooterTest extends LinearOpMode { shooter.setShooterMode(flyMode); - shooter.setControllerCoefficients(p, i, d); + shooter.setControllerCoefficients(p, i, d, f); waitForStart(); @@ -60,7 +78,7 @@ public class ShooterTest extends LinearOpMode { while (opModeIsActive()) { - shooter.setControllerCoefficients(p, i, d); + shooter.setControllerCoefficients(p, i, d, f); shooter.setTurretMode(turrMode); @@ -72,6 +90,8 @@ public class ShooterTest extends LinearOpMode { shooter.setTargetPosition(pos); + shooter.setTurretPosition(posi); + shooter.setTolerance(posTolerance); shooter.setPosPower(posPower); @@ -80,6 +100,11 @@ public class ShooterTest extends LinearOpMode { shooter.update(); + if (wait(time)){ + telemetry.addData("Velocity", shooter.getVelocity(initPos)); + stamp = getRuntime(); + initPos = shooter.getECPRPosition(); + } TELE.update(); } 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 7cebd8e..5d30e32 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 @@ -30,8 +30,6 @@ public class Robot { public DcMotorEx transfer; - - public DcMotorEx shooter1; public DcMotorEx shooter2; public Servo hood; @@ -63,25 +61,11 @@ public class Robot { public AprilTagProcessor aprilTagProcessor; - public WebcamName webcam; public DcMotorEx shooterEncoder; - - - - - - - - - - - - - - public Robot (HardwareMap hardwareMap) { + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map @@ -106,10 +90,6 @@ public class Robot { shooter2.setDirection(DcMotorSimple.Direction.REVERSE); - - - - hood = hardwareMap.get(Servo.class, "hood"); turr1 = hardwareMap.get(Servo.class, "t1"); @@ -132,11 +112,8 @@ public class Robot { pin5 = hardwareMap.get(DigitalChannel.class, "pin5"); - - analogInput = hardwareMap.get(AnalogInput.class, "analog"); - analogInput2 = hardwareMap.get(AnalogInput.class, "analog2"); transfer = hardwareMap.get(DcMotorEx.class, "transfer"); @@ -147,12 +124,7 @@ public class Robot { aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); - webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); - - - - } }