From c91828f899757a11d613c08abe29719a97513168 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 28 Nov 2025 18:23:03 -0600 Subject: [PATCH] changes in 11/28 --- .../ftc/teamcode/autonomous/redDaniel.java | 9 ++-- .../teamcode/constants/ServoPositions.java | 4 +- .../ftc/teamcode/constants/ShooterVars.java | 6 ++- .../ftc/teamcode/tests/ShooterTest.java | 52 +++++++++++++++++-- .../utils/PositionalServoProgrammer.java | 15 ++++-- 5 files changed, 69 insertions(+), 17 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java index f1864f1..941934b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/redDaniel.java @@ -3,7 +3,7 @@ package org.firstinspires.ftc.teamcode.autonomous; import static org.firstinspires.ftc.teamcode.constants.Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import static org.firstinspires.ftc.teamcode.constants.ShooterVars.*; - +import static org.firstinspires.ftc.teamcode.utils.PositionalServoProgrammer.*; import androidx.annotation.NonNull; @@ -43,13 +43,12 @@ public class redDaniel extends LinearOpMode { double initPos = 0.0; double stamp = 0.0; double powPID = 0.0; - final int maxVel = 4500; double ticker = 0.0; public boolean run(@NonNull TelemetryPacket telemetryPacket){ velo = -60 * ((((double) robot.shooter1.getCurrentPosition() / 2048) - initPos) / (getRuntime() - stamp)); stamp = getRuntime(); initPos = (double) robot.shooter1.getCurrentPosition() / 2048; - if (Math.abs(velocity - velo) > 3 * velTolerance) { + if (Math.abs(velocity - velo) > initTolerance) { powPID = (double) velocity / maxVel; ticker = getRuntime(); } else if (velocity - velTolerance > velo) { @@ -75,11 +74,11 @@ public class redDaniel extends LinearOpMode { public boolean run(@NonNull TelemetryPacket telemetryPacket) { robot.spin1.setPosition(spindexer); robot.spin2.setPosition(1-spindexer); - if (robot.spin1Pos.getVoltage() / 3.3 == spindexer){ + if (scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) < spindexer + 0.01 && scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3) > spindexer - 0.01){ robot.transferServo.setPosition(transferServo_in); transfer = true; } - if (robot.transferServoPos.getVoltage() / 3.3 == transferServo_in && transfer){ + if (scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) < transferServo_in + 0.01 && scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3) > transferServo_in - 0.01 && transfer){ robot.transferServo.setPosition(transferServo_out); return false; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index d7b0916..d2ef894 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -13,9 +13,9 @@ public class ServoPositions { public static double spindexer_outtakeBall2 = 0.48; public static double spindexer_outtakeBall1 = 0.1; - public static double transferServo_out = 0.13; + public static double transferServo_out = 0.15; - public static double transferServo_in = 0.36; + public static double transferServo_in = 0.38; public static double hoodDefault = 0.35; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java index fe75b9b..128c52a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ShooterVars.java @@ -8,5 +8,9 @@ public class ShooterVars { public static double turret_Range = 355; - public static int velTolerance = 500; + public static int velTolerance = 300; + + public static int initTolerance = 1000; + + public static int maxVel = 4000; } 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 ad70ecd..5078b0c 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 @@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.subsystems.Shooter; @@ -42,12 +43,22 @@ public class ShooterTest extends LinearOpMode { double powPID = 0.0; - public static double maxVel = 4500; + public static int maxVel = 4000; public static int tolerance = 300; public static boolean shoot = false; + public static int spindexPos = 1; + + public static int initTolerance = 1000; + + public static boolean intake = true; + + double initVel = 0; + + double stamp = 0.0; + MultipleTelemetry TELE; @Override @@ -59,6 +70,9 @@ public class ShooterTest extends LinearOpMode { Shooter shooter = new Shooter(robot, TELE); + robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + shooter.setTelemetryOn(true); shooter.setTurretMode(turrMode); @@ -69,6 +83,8 @@ public class ShooterTest extends LinearOpMode { initPos = shooter.getECPRPosition(); + initVel = vel; + waitForStart(); if (isStopRequested()) return; @@ -84,19 +100,45 @@ public class ShooterTest extends LinearOpMode { robot.hood.setPosition(hoodPos); robot.turr1.setPosition(turretPos); robot.turr2.setPosition(1-turretPos); + if (intake){ + robot.transfer.setPower(0); + robot.intake.setPower(0.75); + robot.spin1.setPosition(spindexer_intakePos); + robot.spin2.setPosition(1-spindexer_intakePos); + } else { + robot.transfer.setPower(1); + robot.intake.setPower(0); + if (spindexPos == 1){ + robot.spin1.setPosition(spindexer_outtakeBall1); + robot.spin2.setPosition(1-spindexer_outtakeBall1); + } else if (spindexPos == 2){ + robot.spin1.setPosition(spindexer_outtakeBall2); + robot.spin2.setPosition(1-spindexer_outtakeBall2); + } else if (spindexPos == 3){ + robot.spin1.setPosition(spindexer_outtakeBall3); + robot.spin2.setPosition(1-spindexer_outtakeBall3); + } + } + + if (vel != initVel){ + stamp = getRuntime(); + initVel = vel; + } velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); stamp1 = getRuntime(); initPos1 = shooter.getECPRPosition(); - if (Math.abs(vel - velo1) > 3 * tolerance) { + if (Math.abs(vel - velo1) > initTolerance && getRuntime() - stamp < 2) { powPID = vel / maxVel; } else if (vel - tolerance > velo1) { - powPID = powPID + 0.0001; + powPID = powPID + 0.001; } else if (vel + tolerance < velo1) { - powPID = powPID - 0.0001; + powPID = powPID - 0.001; + } + if (powPID > 1.0){ + powPID = 1.0; } shooter.setVelocity(powPID); - robot.transfer.setPower((powPID / 2) + 0.5); if (shoot){ robot.transferServo.setPosition(transferServo_in); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index 0734891..0946902 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -15,6 +15,9 @@ public class PositionalServoProgrammer extends LinearOpMode { public static double turretPos = 0.501; public static double transferPos = 0.501; public static double hoodPos = 0.501; + + public static double scalar = 1.112; + public static double restPos = 0.158; @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); @@ -36,10 +39,14 @@ public class PositionalServoProgrammer extends LinearOpMode { if (hoodPos != 0.501){ robot.hood.setPosition(hoodPos); } - TELE.addData("spindexer", 1.111111111*((robot.spin1Pos.getVoltage() / 3.3) - 0.05)); - TELE.addData("hood", 1.111111111*((robot.hoodPos.getVoltage() / 3.3) - 0.05)); - TELE.addData("transferServo", 1.111111111*((robot.transferServoPos.getVoltage() / 3.3) - 0.05)); - TELE.addData("turret", 1.111111111*((robot.turr1Pos.getVoltage() / 3.3) - 0.05)); + TELE.addData("spindexer", scalar*((robot.spin1Pos.getVoltage() - restPos) / 3.3)); + TELE.addData("hood", 1-scalar*((robot.hoodPos.getVoltage() - restPos) / 3.3)); + TELE.addData("transferServo", scalar*((robot.transferServoPos.getVoltage() - restPos) / 3.3)); + TELE.addData("turret", scalar*((robot.turr1Pos.getVoltage() - restPos) / 3.3)); + TELE.addData("spindexerA", robot.spin1Pos.getVoltage()); + TELE.addData("hoodA", robot.hoodPos.getVoltage()); + TELE.addData("transferServoA", robot.transferServoPos.getVoltage()); + TELE.addData("turretA", robot.turr1Pos.getVoltage()); TELE.update(); } }