From e5ba0947e375ef53b64a9aa2e4e368678d6f13a0 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 18 Nov 2025 19:27:15 -0600 Subject: [PATCH] test --- .../ftc/teamcode/subsystems/Shooter.java | 135 +++++------------- .../ftc/teamcode/tests/ShooterTest.java | 17 +-- 2 files changed, 41 insertions(+), 111 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 62be51a..e2bb60c 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 @@ -27,7 +27,6 @@ public class Shooter implements Subsystem { private final Servo turret2; - private final MultipleTelemetry telemetry; private boolean telemetryOn = false; @@ -46,15 +45,10 @@ public class Shooter implements Subsystem { private PIDController controller; private double pow = 0.0; - - - - private String shooterMode = "AUTO"; private String turretMode = "AUTO"; - public Shooter(Robot robot, MultipleTelemetry TELE) { this.fly1 = robot.shooter1; this.fly2 = robot.shooter2; @@ -68,10 +62,8 @@ 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.setPID(p, i, d); this.turret1 = robot.turr1; @@ -80,11 +72,6 @@ public class Shooter implements Subsystem { this.encoder = robot.shooterEncoder; - - - - - } public void telemetryUpdate() { @@ -111,34 +98,31 @@ public class Shooter implements Subsystem { return (hoodServo.getPosition()); } - public void sethoodPosition(double pos) {hoodPos = pos;} + public void sethoodPosition(double pos) { hoodPos = pos; } public double getTurretPosition() { - return ((turret1.getPosition()+ (1-turret2.getPosition()))/2); + return ((turret1.getPosition() + (1 - turret2.getPosition())) / 2); } - public void setTurretPosition(double pos) {turretPos = pos;} + public void setTurretPosition(double pos) { turretPos = pos; } public double getVelocity(double cpr) { - return 60*(fly1.getVelocity() / (2*cpr)); + return 60 * (fly1.getVelocity() / (2 * cpr)); } - public void setVelocity(double vel){velocity = vel;} + public void setVelocity(double vel) { velocity = vel; } + public void setPosPower(double power) { posPower = power; } - - public void setPosPower(double power){posPower = power;} - - - public void setTargetPosition(int pos){ + public void setTargetPosition(int pos) { targetPosition = pos; } - public void setTolerance(int tolerance){ + public void setTolerance(int tolerance) { controller.setTolerance(tolerance); } - public void setControllerCoefficients(double kp, double ki, double kd){ + public void setControllerCoefficients(double kp, double ki, double kd) { p = kp; i = ki; d = kd; @@ -146,45 +130,35 @@ public class Shooter implements Subsystem { } - public PIDCoefficients getControllerCoefficients(){ + public PIDCoefficients getControllerCoefficients() { return new PIDCoefficients(p, i, d); } + public void setManualPower(double power) { manualPower = power; } + public String getShooterMode() { return shooterMode; } + public String getTurretMode() { return turretMode; } - - - - public void setManualPower(double power){manualPower = power;} - - public String getShooterMode(){return shooterMode;} - - public String getTurretMode(){return turretMode;} - - - public double getECPRPosition(){ - return (fly1.getCurrentPosition() * 720 / ecpr); + public double getECPRPosition() { + return (fly1.getCurrentPosition() * 720 / ecpr); } - public double getMCPRPosition(){ + public double getMCPRPosition() { return (fly1.getCurrentPosition() * 720) / mcpr; } - public void setShooterMode(String mode){ shooterMode = mode;} + public void setShooterMode(String mode) { shooterMode = mode; } - public void setTurretMode(String mode){ turretMode = mode;} + public void setTurretMode(String mode) { turretMode = mode; } - - public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset){ + public double trackGoal(Pose2d robotPose, Pose2d goalPose, double offset) { fly1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - Pose2d deltaPose = new Pose2d( goalPose.position.x - robotPose.position.x, goalPose.position.y - robotPose.position.y, @@ -193,8 +167,8 @@ public class Shooter implements Subsystem { double distance = Math.sqrt( deltaPose.position.x * deltaPose.position.x - + deltaPose.position.y * deltaPose.position.y - + Poses.relativeGoalHeight * Poses.relativeGoalHeight + + deltaPose.position.y * deltaPose.position.y + + Poses.relativeGoalHeight * Poses.relativeGoalHeight ); telemetry.addData("dst", distance); @@ -203,39 +177,28 @@ public class Shooter implements Subsystem { double hoodAngle = getAngleByDist(distance); - // hoodServo.setPosition(hoodAngle); moveTurret(getTurretPosByDeltaPose(deltaPose, offset)); return distance; - - - - //0.9974 * 355 - } - public double getTurretPosByDeltaPose (Pose2d dPose, double offset){ + public double getTurretPosByDeltaPose(Pose2d dPose, double offset) { double deltaAngle = Math.toDegrees(dPose.heading.toDouble()); - - double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y/dPose.position.x)); + double aTanAngle = Math.toDegrees(Math.atan(dPose.position.y / dPose.position.x)); telemetry.addData("deltaAngle", deltaAngle); - - if (deltaAngle > 90) { - deltaAngle -=360; + deltaAngle -= 360; } - - // deltaAngle += aTanAngle; deltaAngle /= (335); @@ -244,10 +207,7 @@ public class Shooter implements Subsystem { telemetry.addData("AtanAngle", aTanAngle); - - return ((0.30-deltaAngle) + offset); - - + return ((0.30 - deltaAngle) + offset); } @@ -255,77 +215,56 @@ public class Shooter implements Subsystem { //56.5, 0.5 - - public double getPowerByDist(double dist){ + public double getPowerByDist(double dist) { //TODO: ADD LOGIC - return dist; + return dist; } - public double getAngleByDist(double dist){ - + public double getAngleByDist(double dist) { double newDist = dist - 56.5; - double pos = newDist*((0.44-0.5)/(62-56.5)) + 0.46; + double pos = newDist * ((0.44 - 0.5) / (62 - 56.5)) + 0.46; - - - - return pos; + return pos; } + public void setTelemetryOn(boolean state) { telemetryOn = state; } - - - public void setTelemetryOn(boolean state){telemetryOn = state;} - - public void moveTurret(double pos){ + public void moveTurret(double pos) { turret1.setPosition(pos); - turret2.setPosition(1-pos); + turret2.setPosition(1 - pos); } @Override public void update() { - if (Objects.equals(shooterMode, "MANUAL")){ + if (Objects.equals(shooterMode, "MANUAL")) { fly1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); fly2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); - fly1.setPower(manualPower); fly2.setPower(manualPower); - } - - else if (Objects.equals(shooterMode, "VEL")){ + } 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(mcpr) * ecpr, velocity); - - fly1.setPower(powPID); fly2.setPower(powPID); - } - - - if (Objects.equals(turretMode, "MANUAL")){ + if (Objects.equals(turretMode, "MANUAL")) { // hoodServo.setPosition(hoodPos); moveTurret(turretPos); - } - - - if (telemetryOn) {telemetryUpdate();} - - + 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 4ab4d23..de443ef 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 @@ -12,7 +12,6 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.subsystems.Shooter; - @TeleOp @Config public class ShooterTest extends LinearOpMode { @@ -28,7 +27,6 @@ public class ShooterTest extends LinearOpMode { public static double p = 0.001, i = 0, d = 0; - public static String flyMode = "MANUAL"; public static String turrMode = "MANUAL"; @@ -38,6 +36,7 @@ public class ShooterTest extends LinearOpMode { public static double servoPosition = 0.501; MultipleTelemetry TELE; + @Override public void runOpMode() throws InterruptedException { @@ -53,16 +52,13 @@ public class ShooterTest extends LinearOpMode { shooter.setShooterMode(flyMode); - shooter.setControllerCoefficients(p, i, d); - waitForStart(); - if(isStopRequested()) return; + if (isStopRequested()) return; - - while(opModeIsActive()){ + while (opModeIsActive()) { shooter.setControllerCoefficients(p, i, d); @@ -80,18 +76,13 @@ public class ShooterTest extends LinearOpMode { shooter.setPosPower(posPower); - if (servoPosition!=0.501) {shooter.sethoodPosition(servoPosition);} - + if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); } shooter.update(); TELE.update(); - - } - - } }