From 1a1c99791d1d709ac102af1ec40a15129b4c3ec5 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 16:31:33 -0500 Subject: [PATCH] Made Robot Singleton --- .../ftc/teamcode/utilsv2/Drivetrain.java | 4 +- .../ftc/teamcode/utilsv2/Flywheel.java | 2 +- .../ftc/teamcode/utilsv2/Intake.java | 20 ++ .../ftc/teamcode/utilsv2/Robot.java | 312 ++++++++++++++++++ .../ftc/teamcode/utilsv2/Shooter.java | 1 - .../ftc/teamcode/utilsv2/Spindexer.java | 18 - .../utilsv2/SpindexerTransferIntake.java | 161 +++++++++ .../ftc/teamcode/utilsv2/Turret.java | 20 +- 8 files changed, 505 insertions(+), 33 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Spindexer.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java index ecba638..6047868 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Drivetrain.java @@ -18,8 +18,8 @@ public class Drivetrain { private static final double STRAFE_MULTIPLIER = 1.2; - public static double FORWARD_ROTATION_CORRECTION = 0.03; - public static double STRAFE_ROTATION_CORRECTION = -0.03; + public static double FORWARD_ROTATION_CORRECTION = 0; + public static double STRAFE_ROTATION_CORRECTION = -0; private boolean tele = false; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java index 123d3dc..a5cfada 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -5,7 +5,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; import java.util.LinkedList; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java new file mode 100644 index 0000000..ab240b4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java @@ -0,0 +1,20 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import org.firstinspires.ftc.teamcode.utilsv2.Robot; + +public class Intake { + + Robot rob; + public Intake(Robot robot) { + + this.rob = robot; + + } + + public void setIntakePower(double pow){ + rob.intake.setPower(pow); + } + public void setTransferPower(double pow){ + rob.transfer.setPower(pow); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java new file mode 100644 index 0000000..c805842 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -0,0 +1,312 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.TouchSensor; +import com.qualcomm.robotcore.hardware.VoltageSensor; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +public class Robot { + // Singleton instance + private static Robot instance; + + /** + * Returns the existing Robot instance or creates one if it doesn't exist. + */ + public static Robot getInstance(HardwareMap hardwareMap) { + if (instance == null) { + instance = new Robot(hardwareMap); + } + return instance; + } + + /** + * Optional: clears the singleton. + * Useful when switching OpModes. + */ + public static void resetInstance() { + instance = null; + } + + public static boolean usingLimelight = true; + public static boolean usingCamera = false; + public DcMotorEx frontLeft; + public DcMotorEx frontRight; + public DcMotorEx backLeft; + public DcMotorEx backRight; + public DcMotorEx intake; + public DcMotorEx transfer; + public PIDFCoefficients shooterPIDF; + public static double shooterPIDF_P = 255; + public static double shooterPIDF_I = 0.0; + public static double shooterPIDF_D = 0.0; + public static double shooterPIDF_F = 75; + // public double[] shooterPIDF_StepSizes = {10.0, 1.0, 0.001, 0.0001}; + public DcMotorEx shooter1; + public DcMotorEx shooter2; + public Servo hood; + public Servo transferServo; + public Servo spindexBlocker; + public Servo rapidFireBlocker; + public Servo tilt1; + public Servo tilt2; + public Servo turr1; + public Servo turr2; + public Servo spin1; + public Servo spin2; + public TouchSensor beam1; + public TouchSensor beam2; + public TouchSensor beam3; + public RevColorSensorV3 revSensor; + + public VoltageSensor voltage; + + // Below is disregarded + public AnalogInput spin1Pos; + public AnalogInput spin2Pos; + public AnalogInput turr1Pos; + public AnalogInput transferServoPos; + public AprilTagProcessor aprilTagProcessor; + public WebcamName webcam; + public RevColorSensorV3 color1; + public RevColorSensorV3 color2; + public RevColorSensorV3 color3; + public Limelight3A limelight; + public Servo light; + + public Robot(HardwareMap hardwareMap) { + + //Define components w/ hardware map + frontLeft = hardwareMap.get(DcMotorEx.class, "fl"); + frontRight = hardwareMap.get(DcMotorEx.class, "fr"); + backLeft = hardwareMap.get(DcMotorEx.class, "bl"); + backRight = hardwareMap.get(DcMotorEx.class, "br"); + frontLeft.setDirection(DcMotorSimple.Direction.REVERSE); + backLeft.setDirection(DcMotorSimple.Direction.REVERSE); + + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + intake = hardwareMap.get(DcMotorEx.class, "intake"); + intake.setDirection(DcMotorSimple.Direction.REVERSE); + + shooter1 = hardwareMap.get(DcMotorEx.class, "shooter1"); + + shooter2 = hardwareMap.get(DcMotorEx.class, "shooter2"); + + shooter1.setDirection(DcMotorSimple.Direction.REVERSE); + shooterPIDF = new PIDFCoefficients(shooterPIDF_P, shooterPIDF_I, shooterPIDF_D, shooterPIDF_F); + shooter1.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter1.setVelocity(0); + shooter2.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF); + shooter2.setVelocity(0); + + hood = hardwareMap.get(Servo.class, "hood"); + + turr1 = hardwareMap.get(Servo.class, "turr1"); + + turr2 = hardwareMap.get(Servo.class, "turr2"); + + spin1 = hardwareMap.get(Servo.class, "spin1"); + + spin2 = hardwareMap.get(Servo.class, "spin2"); + + transfer = hardwareMap.get(DcMotorEx.class, "transfer"); + transfer.setDirection(DcMotorSimple.Direction.REVERSE); + + transferServo = hardwareMap.get(Servo.class, "transferServo"); + + transfer.setDirection(DcMotorSimple.Direction.REVERSE); + transfer.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + spindexBlocker = hardwareMap.get(Servo.class, "spinB"); + + rapidFireBlocker = hardwareMap.get(Servo.class, "rapidB"); + + tilt1 = hardwareMap.get(Servo.class, "tilt1"); + tilt2 = hardwareMap.get(Servo.class, "tilt2"); + +// beam1 = hardwareMap.get(TouchSensor.class, "beam1"); +// beam2 = hardwareMap.get(TouchSensor.class, "beam2"); +// beam3 = hardwareMap.get(TouchSensor.class, "beam3"); + + revSensor = hardwareMap.get(RevColorSensorV3.class, "rev"); + + // Below is disregarded + +// turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port +// +// spin1Pos = hardwareMap.get(AnalogInput.class, "spin1Pos"); +// +// spin2Pos = hardwareMap.get(AnalogInput.class, "spin2Pos"); +// +// transferServoPos = hardwareMap.get(AnalogInput.class, "tSPos"); +// +// color1 = hardwareMap.get(RevColorSensorV3.class, "c1"); +// +// color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); +// +// color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); + + if (usingLimelight) { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + } else if (usingCamera) { + webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + aprilTagProcessor = AprilTagProcessor.easyCreateWithDefaults(); + } + +// light = hardwareMap.get(Servo.class, "light"); + + voltage = hardwareMap.voltageSensor.iterator().next(); + } + + // Voids below are used to minimize hardware calls to minimize loop times + + // Used to cut off digits that are negligible + private final int maxDigits = 5; + private final int roundingFactor = (int) Math.pow(10, maxDigits); + + private double prevFrontLeftPower = -10.501; + public void setFrontLeftPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevFrontLeftPower){ + frontLeft.setPower(pow); + } + prevFrontLeftPower = pow; + } + + private double prevFrontRightPower = -10.501; + public void setFrontRightPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevFrontRightPower){ + frontRight.setPower(pow); + } + prevFrontRightPower = pow; + } + + private double prevBackLeftPower = -10.501; + public void setBackLeftPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevBackLeftPower){ + backLeft.setPower(pow); + } + prevBackLeftPower = pow; + } + + private double prevBackRightPower = -10.501; + public void setBackRightPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevBackRightPower){ + backRight.setPower(pow); + } + prevBackRightPower = pow; + } + + private double prevIntakePower = -10.501; + public void setIntakePower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevIntakePower){ + intake.setPower(pow); + } + prevIntakePower = pow; + } + + private double prevTransferPower = -10.501; + public void setTransferPower(double pow){ + pow = (double) Math.round(pow * roundingFactor) / roundingFactor; + if (pow != prevTransferPower){ + transfer.setPower(pow); + } + prevTransferPower = pow; + } + + // shooter motors are done in separate class + + private double prevHoodPos = -10.501; + public void setHoodPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevHoodPos){ + hood.setPosition(pos); + } + prevHoodPos = pos; + } + + private double prevTransferServoPos = -10.501; + public void setTransferServoPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTransferServoPos){ + transferServo.setPosition(pos); + } + prevTransferServoPos = pos; + } + + private double prevSpinPos = -10.501; + public void setSpinPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevSpinPos){ + spin1.setPosition(pos); + spin2.setPosition(pos); + } + prevSpinPos = pos; + } + + private double prevTurretPos = -10.501; + public void setTurretPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTurretPos){ + turr1.setPosition(pos); + turr2.setPosition(pos); + } + prevTurretPos = pos; + } + + private double prevTilt1Pos = -10.501; + public void setTilt1Pos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTilt1Pos){ + tilt1.setPosition(pos); + } + prevTilt1Pos = pos; + } + + private double prevTilt2Pos = -10.501; + public void setTilt2Pos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevTilt2Pos){ + tilt2.setPosition(pos); + } + prevTilt2Pos = pos; + } + + private double prevSpindexBlockerPos = -10.501; + public void setSpindexBlockerPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevSpindexBlockerPos){ + spindexBlocker.setPosition(pos); + } + prevSpindexBlockerPos = pos; + } + + private double prevRapidFireBlockerPos = -10.501; + public void setRapidFireBlockerPos(double pos){ + pos = (double) Math.round(pos * roundingFactor) / roundingFactor; + if (pos != prevRapidFireBlockerPos){ + rapidFireBlocker.setPosition(pos); + } + prevRapidFireBlockerPos = pos; + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java index 5ee5496..9c60f8f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.pedropathing.follower.Follower; -import org.firstinspires.ftc.teamcode.utils.Robot; public class Shooter { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Spindexer.java deleted file mode 100644 index 5965028..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Spindexer.java +++ /dev/null @@ -1,18 +0,0 @@ -package org.firstinspires.ftc.teamcode.utilsv2; - -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.utils.Robot; - -public class Spindexer { - - private Servo spin1; - private Servo spin2; - - public Spindexer (Robot rob, MultipleTelemetry TELE) { - - - - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java new file mode 100644 index 0000000..950fdfc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -0,0 +1,161 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.robotcore.hardware.Servo; + + +public class SpindexerTransferIntake { + + private Servo spin1; + private Servo spin2; + + public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) { + this.spin1 = rob.spin1; + this.spin2 = rob.spin2; + + } + + enum SpindexerMode { + RAPID, + SORTED + } + + enum RapidMode { + INTAKE, // Normal intake operation + TRANSFER_OFF, // Slow transfer, waiting for rings to settle + BEFORE_PULSE_OUT, // Brief forward delay before pulse + PULSE_OUT, // Small reverse pulse to unjam/reposition rings + PULSE_IN, // Feed rings back forward + HOLD_BALLS, // Maintain ring position + OPEN_GATE, // Open shooter gate + SHOOT // Feed ring into shooter + } + + private SpindexerMode mode = SpindexerMode.RAPID; + + private RapidMode rapidMode = RapidMode.INTAKE; + + public void setRapidMode (RapidMode rMode) { + this.rapidMode = rMode; + } + + public void setSpindexerMode (SpindexerMode spindexerMode){ + this.mode = spindexerMode; + } + + public void update() { + + switch (mode) { + + case RAPID: + + switch (rapidMode) { + + case INTAKE: + + // Run front intake + // Run transfer intake + // Keep shooter gate closed + // Keep kicker retracted + // Keep spindexer in default position + + // If first beam break sees ring: + // rapidMode = TRANSFER_OFF; + + // If shoot button pressed: + // rapidMode = OPEN_GATE; + + break; + + case TRANSFER_OFF: + + // Slow down transfer intake + // Allow rings to settle into storage + + // If both beam breaks occupied: + // rapidMode = BEFORE_PULSE_OUT; + + // If shoot button pressed: + // rapidMode = OPEN_GATE; + + break; + + case BEFORE_PULSE_OUT: + + // Continue running intake forward + + // After ~0.3 sec: + // rapidMode = PULSE_OUT; + + break; + + case PULSE_OUT: + + // Reverse intake slightly + // Helps separate rings and prevent jams + + // After pulse time: + // rapidMode = PULSE_IN; + + break; + + case PULSE_IN: + + // Run intake forward again + // Re-seat rings after pulse + + // After ~0.2 sec: + // rapidMode = HOLD_BALLS; + + break; + + case HOLD_BALLS: + + // If both sensors occupied: + // hold intake at low power + + // Else: + // run intake forward to pull rings back in + + // If shoot button pressed: + // rapidMode = OPEN_GATE; + + // If resume intake button pressed: + // rapidMode = INTAKE; + + break; + + case OPEN_GATE: + + // Open upper gate + // Keep intake feeding + + // After ~0.1 sec: + // rapidMode = SHOOT; + + break; + + case SHOOT: + + // Activate kicker + // Feed ring into shooter + + // After ~0.4 sec: + // rapidMode = INTAKE; + + break; + } + + break; + + case SORTED: + + // Future sorted-intake logic + // Color sorting + // Alliance filtering + // Different spindexer behavior + + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index ec15235..fd8edb8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -5,7 +5,6 @@ import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.robotcore.util.Range; -import org.firstinspires.ftc.teamcode.utils.Robot; import java.util.List; @@ -13,10 +12,10 @@ import java.util.List; public class Turret { Robot robot; - private final double servoTicksPer180 = 0.6; // TODO: Tune - private final double neutralPosition = 0.5; //TODO: Tune - private final double turretMin = 0.04; //TODO: Tune - private final double turretMax = 0.94; //TODO: Tune + private final double servoTicksPer180 = 0.58; + private final double neutralPosition = 0.51; + private final double turretMin = 0.05; + private final double turretMax = 0.95; private final double hVelK = 0; // TODO: Tune private final double xVelK = 0; // TODO: Tune private final double xAccK = 0; // TODO: Tune @@ -53,8 +52,8 @@ public class Turret { servoAngle = Range.clip(servoAngle, turretMin, turretMax); - robot.turr1.setPosition(servoAngle); - robot.turr2.setPosition(1.0 - servoAngle); + robot.setTurretPos(servoAngle); + detectObelisk(); @@ -81,8 +80,8 @@ public class Turret { } public void manual (double pos) { - robot.turr1.setPosition(pos); - robot.turr2.setPosition(pos); + robot.setTurretPos(pos); + } @@ -107,7 +106,6 @@ public class Turret { servoAngle = Range.clip(servoAngle, turretMin, turretMax); - robot.turr1.setPosition(servoAngle); - robot.turr2.setPosition(servoAngle); + robot.setTurretPos(servoAngle); } } \ No newline at end of file