From e3105a339dfcb4268085ed8b1facdc557b649e49 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Mon, 25 May 2026 15:19:05 -0700 Subject: [PATCH 01/11] added turretv2 starter code --- .../ftc/teamcode/utilsv2/Turret.java | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java 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 new file mode 100644 index 0000000..4e95a9b --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -0,0 +1,53 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.teamcode.utils.Robot; + +@Config +public class Turret { + Robot robot; + + private final double servoTicksPer180 = 0.6; // TODO: Tune + private final double neutralPosition = 0.3; //TODO: Tune + private final double turretMin = 0.04; //TODO: Tune + private final double turretMax = 0.94; //TODO: Tune + + + public Turret (Robot rob){ + this.robot = rob; + } + + public void trackGoal (double dx, double dy, double h) { + // dx, dy, dz is target - robot + // h is the raw heading where 0 degrees is positive x in the system of x, y + while (h > 180) h -= 360; + while (h < -180) h += 360; + + double fieldRelativeHeading = Math.toDegrees(Math.atan2(dy,dx)); // Angle assuming the robot is at zero degrees, CCW + double desiredAngle = fieldRelativeHeading - h; // Account for robot rotation + double angleDelta = desiredAngle - 180; // Subtract 180 as the neutral position is at 180 degrees + + // Shift to -180 --> 180 scale + while (angleDelta > 180) angleDelta -= 360; + while (angleDelta < -180) angleDelta += 360; + + double servoTicksFromNeutral = (angleDelta / 360.0) * (2 * servoTicksPer180); + double servoAngle = neutralPosition + servoTicksFromNeutral; + servoAngle = Range.clip(servoAngle, turretMin, turretMax); + + robot.turr1.setPosition(servoAngle); + robot.turr2.setPosition(1.0 - servoAngle); + + + + + + + + + + + } + +} From ed970eaf381dabe00cb4e118fb80c815a6d6c7f0 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Mon, 25 May 2026 15:39:10 -0700 Subject: [PATCH 02/11] changed to radians, accounted for velo and acc --- .../ftc/teamcode/utilsv2/Turret.java | 55 +++++++++++-------- 1 file changed, 32 insertions(+), 23 deletions(-) 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 4e95a9b..3c3741c 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 @@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.util.Range; + import org.firstinspires.ftc.teamcode.utils.Robot; @Config @@ -13,41 +14,49 @@ public class Turret { private final double turretMin = 0.04; //TODO: Tune private final double turretMax = 0.94; //TODO: Tune + private final double hVelK = 0.12; // TODO: Tune + private final double hAccK = 0.02; // TODO: Tune - public Turret (Robot rob){ + private final double xVelK = 0.10; // TODO: Tune + private final double xAccK = 0.02; // TODO: Tune + + private final double yVelK = 0.10; // TODO: Tune + private final double yAccK = 0.02; // TODO: Tune + + + public Turret(Robot rob) { this.robot = rob; } - public void trackGoal (double dx, double dy, double h) { + private double wrapAngle(double angle) { + while (angle > Math.PI) angle -= 2.0 * Math.PI; + while (angle < -Math.PI) angle += 2.0 * Math.PI; + return angle; + } + + public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) { // dx, dy, dz is target - robot // h is the raw heading where 0 degrees is positive x in the system of x, y - while (h > 180) h -= 360; - while (h < -180) h += 360; - double fieldRelativeHeading = Math.toDegrees(Math.atan2(dy,dx)); // Angle assuming the robot is at zero degrees, CCW - double desiredAngle = fieldRelativeHeading - h; // Account for robot rotation - double angleDelta = desiredAngle - 180; // Subtract 180 as the neutral position is at 180 degrees + double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot + double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot + double predictedH = h + (hVel * hVelK) + (0.5 * hAcc * hAccK); // Positive bc h = robot heading - // Shift to -180 --> 180 scale - while (angleDelta > 180) angleDelta -= 360; - while (angleDelta < -180) angleDelta += 360; + predictedH = wrapAngle(predictedH); + + double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx); + + double desiredAngle = fieldRelativeHeading - predictedH; + double angleDelta = desiredAngle - Math.PI; + angleDelta = wrapAngle(angleDelta); + + double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180); - double servoTicksFromNeutral = (angleDelta / 360.0) * (2 * servoTicksPer180); double servoAngle = neutralPosition + servoTicksFromNeutral; + servoAngle = Range.clip(servoAngle, turretMin, turretMax); robot.turr1.setPosition(servoAngle); robot.turr2.setPosition(1.0 - servoAngle); - - - - - - - - - - } - -} +} \ No newline at end of file From 9ab69f8fbe693f58fc5500c99e971de24faa0945 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Mon, 25 May 2026 16:29:38 -0700 Subject: [PATCH 03/11] added obelisk code --- .../ftc/teamcode/utilsv2/Turret.java | 54 +++++++++++++++++-- 1 file changed, 51 insertions(+), 3 deletions(-) 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 3c3741c..0e7eff7 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 @@ -1,10 +1,14 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.config.Config; +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; + @Config public class Turret { Robot robot; @@ -13,16 +17,16 @@ public class Turret { private final double neutralPosition = 0.3; //TODO: Tune private final double turretMin = 0.04; //TODO: Tune private final double turretMax = 0.94; //TODO: Tune - private final double hVelK = 0.12; // TODO: Tune private final double hAccK = 0.02; // TODO: Tune - private final double xVelK = 0.10; // TODO: Tune private final double xAccK = 0.02; // TODO: Tune - private final double yVelK = 0.10; // TODO: Tune private final double yAccK = 0.02; // TODO: Tune + private int obeliskID = 0; + + public Turret(Robot rob) { this.robot = rob; @@ -34,6 +38,50 @@ public class Turret { return angle; } + public void trackObelisk(double dx, double dy, double h) { + + double heading = wrapAngle(h); + + double fieldRelativeHeading = Math.atan2(dy, dx); + + double desiredAngle = fieldRelativeHeading - heading; + double angleDelta = desiredAngle - Math.PI; + angleDelta = wrapAngle(angleDelta); + + double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180); + + double servoAngle = neutralPosition + servoTicksFromNeutral; + + servoAngle = Range.clip(servoAngle, turretMin, turretMax); + + robot.turr1.setPosition(servoAngle); + robot.turr2.setPosition(1.0 - servoAngle); + + detectObelisk(); + + } + + public int getObeliskID() { + return obeliskID; + } + + private int detectObelisk() { + robot.limelight.pipelineSwitch(1); + LLResult result = robot.limelight.getLatestResult(); + if (result != null && result.isValid()) { + List fiducials = result.getFiducialResults(); + double prevTx = -1000; + for (LLResultTypes.FiducialResult fiducial : fiducials) { + double currentTx = fiducial.getTargetXDegrees(); + if (currentTx > prevTx){ + obeliskID = fiducial.getFiducialId(); + } + } + } + return obeliskID; + } + + public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) { // dx, dy, dz is target - robot // h is the raw heading where 0 degrees is positive x in the system of x, y From 658e8ea1d069daddaa04b60fd68b5876a449d02b Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Mon, 25 May 2026 16:35:25 -0700 Subject: [PATCH 04/11] Added flywheel copied basically but added average velo calculations --- .../ftc/teamcode/utilsv2/Flywheel.java | 131 ++++++++++++++++++ 1 file changed, 131 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java 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 new file mode 100644 index 0000000..0e9f773 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Flywheel.java @@ -0,0 +1,131 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import com.qualcomm.robotcore.hardware.PIDFCoefficients; + +import org.firstinspires.ftc.teamcode.utils.Robot; + +import java.util.LinkedList; + +public class Flywheel { + Robot robot; + + public PIDFCoefficients shooterPIDF1, shooterPIDF2; + + private double velo = 0.0; + private double velo1 = 0.0; + private double velo2 = 0.0; + + private double averageVelocity = 0.0; + + double targetVelocity = 0.0; + + boolean steady = false; + + private final LinkedList velocityHistory = new LinkedList<>(); + + public Flywheel(HardwareMap hardwareMap) { + + robot = new Robot(hardwareMap); + shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); + shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); + } + + public double getVelo() { + return velo; + } + + public double getVelo1() { + return velo1; + } + + public double getVelo2() { + return velo2; + } + + public double getAverageVelocity() { + return averageVelocity; + } + + public boolean getSteady() { + return steady; + } + + // Set the robot PIDF for the next cycle. + private double prevF = 0; + + public static double voltagePIDFDifference = 0.8; + + public void setPIDF(double p, double i, double d, double f) { + + shooterPIDF1.p = p; + shooterPIDF1.i = i; + shooterPIDF1.d = d; + shooterPIDF1.f = f; + + shooterPIDF2.p = p; + shooterPIDF2.i = i; + shooterPIDF2.d = d; + shooterPIDF2.f = f; + + if (Math.abs(prevF - f) > voltagePIDFDifference) { + + robot.shooter1.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF1); + + robot.shooter2.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, shooterPIDF2); + + prevF = f; + } + } + + // Convert from RPM to Ticks per Second + private double RPM_to_TPS(double RPM) { + return (RPM * 28.0) / 60.0; + } + + // Convert from Ticks per Second to RPM + private double TPS_to_RPM(double TPS) { + return (TPS * 60.0) / 28.0; + } + + private void updateVelocityAverage(double newVelocity) { + + velocityHistory.add(newVelocity); + + int velocityHistorySize = 5; + if (velocityHistory.size() > velocityHistorySize) { + velocityHistory.removeFirst(); + } + + double sum = 0.0; + + for (double v : velocityHistory) { + sum += v; + } + + averageVelocity = sum / velocityHistory.size(); + } + + public void manageFlywheel(double commandedVelocity) { + + if (Math.abs(targetVelocity - commandedVelocity) > 0.0001) { + targetVelocity = commandedVelocity; + } + + robot.shooter1.setVelocity(RPM_to_TPS(targetVelocity)); + + robot.shooter2.setVelocity(RPM_to_TPS(targetVelocity)); + + velo1 = TPS_to_RPM(robot.shooter1.getVelocity()); + + velo2 = TPS_to_RPM(robot.shooter2.getVelocity()); + + velo = (velo1 + velo2) / 2.0; + + updateVelocityAverage(velo); + + steady = (Math.abs(commandedVelocity - averageVelocity) < 50); + } +} \ No newline at end of file From bfb37f13f82a0db70be747f7e7ad179fffaedd48 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Mon, 1 Jun 2026 16:24:11 -0500 Subject: [PATCH 05/11] added neutral shift --- .../ftc/teamcode/utilsv2/Shooter.java | 5 +++++ .../ftc/teamcode/utilsv2/Turret.java | 19 +++++++++---------- 2 files changed, 14 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java 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 new file mode 100644 index 0000000..74a39b6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java @@ -0,0 +1,5 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +public class Shooter { + +} 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 0e7eff7..5c0e638 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 @@ -14,15 +14,15 @@ public class Turret { Robot robot; private final double servoTicksPer180 = 0.6; // TODO: Tune - private final double neutralPosition = 0.3; //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 hVelK = 0.12; // TODO: Tune - private final double hAccK = 0.02; // TODO: Tune - private final double xVelK = 0.10; // TODO: Tune - private final double xAccK = 0.02; // TODO: Tune - private final double yVelK = 0.10; // TODO: Tune - private final double yAccK = 0.02; // TODO: Tune + private final double hVelK = 0; // TODO: Tune + private final double hAccK = 0; // TODO: Tune + private final double xVelK = 0; // TODO: Tune + private final double xAccK = 0; // TODO: Tune + private final double yVelK = 0; // TODO: Tune + private final double yAccK = 0; // TODO: Tune private int obeliskID = 0; @@ -94,8 +94,7 @@ public class Turret { double fieldRelativeHeading = Math.atan2(predictedDy, predictedDx); - double desiredAngle = fieldRelativeHeading - predictedH; - double angleDelta = desiredAngle - Math.PI; + double angleDelta = fieldRelativeHeading - predictedH; angleDelta = wrapAngle(angleDelta); double servoTicksFromNeutral = (angleDelta / (2.0 * Math.PI)) * (2.0 * servoTicksPer180); @@ -105,6 +104,6 @@ public class Turret { servoAngle = Range.clip(servoAngle, turretMin, turretMax); robot.turr1.setPosition(servoAngle); - robot.turr2.setPosition(1.0 - servoAngle); + robot.turr2.setPosition(servoAngle); } } \ No newline at end of file From 184ec893a4c117dd8a1dce7429bf5e99b01eb941 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Mon, 1 Jun 2026 17:12:17 -0500 Subject: [PATCH 06/11] Added shooter class to manager flywheel turret and targetter --- .../ftc/teamcode/utilsv2/Flywheel.java | 4 +- .../ftc/teamcode/utilsv2/Shooter.java | 157 ++++++++++++++++++ .../ftc/teamcode/utilsv2/Turret.java | 10 +- .../teamcode/utilsv2/VelocityCommander.java | 34 ++++ 4 files changed, 200 insertions(+), 5 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java 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 0e9f773..123d3dc 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 @@ -26,9 +26,9 @@ public class Flywheel { private final LinkedList velocityHistory = new LinkedList<>(); - public Flywheel(HardwareMap hardwareMap) { + public Flywheel(Robot rob) { - robot = new Robot(hardwareMap); + robot = rob; shooterPIDF1 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); shooterPIDF2 = new PIDFCoefficients(Robot.shooterPIDF_P, Robot.shooterPIDF_I, Robot.shooterPIDF_D, Robot.shooterPIDF_F); } 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 74a39b6..901981a 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 @@ -1,5 +1,162 @@ 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 { + Robot robot; + Flywheel fly; + Turret turr; + VelocityCommander commander; + + double goalX = 0.0; + double goalY = 0.0; + double obeliskX = 72; + double obeliskY = 144; + + private boolean red = true; + + + Follower follow; + + public Shooter(Robot rob, MultipleTelemetry TELE, Follower follower, boolean redAlliance) { + this.robot = rob; + this.fly = new Flywheel(rob); + this.turr = new Turret(rob); + this.follow = follower; + this.commander = new VelocityCommander(); + + setRedAlliance(redAlliance); + + if (redAlliance) { + goalX = 144; + goalY = 144; + } else { + goalX = 0; + goalY = 144; + } + } + + public void setRedAlliance(boolean input) { + this.red = input; + } + + private double flywheelVelocity = 0.0; + private double turretPosition = 0.5; + + enum ShooterState { + READ_OBELISK, + TRACK_GOAL, + MANUAL_FLYWHEEL_TRACK_TURR, + MANUAL_TURRET_TRACK_FLY, + MANUAL, + NOTHING + } + + + private ShooterState state = ShooterState.NOTHING; + + public void setState(ShooterState shooterState) { + this.state = shooterState; + } + + public void setTurretPosition(double input) { + this.turretPosition = input; + } + + public void setFlywheelVelocity(double input) { + this.flywheelVelocity = input; + } + + public int getObeliskID() { + return turr.getObeliskID(); + } + + + public void update() { + + switch (state) { + case NOTHING: + break; + case MANUAL: + fly.manageFlywheel(flywheelVelocity); + turr.manual(turretPosition); + break; + case TRACK_GOAL: + turr.trackGoal( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getHeading(), + follow.getAngularVelocity(), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + flywheelVelocity = commander.getVeloPredictive( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + fly.manageFlywheel(flywheelVelocity); + break; + case READ_OBELISK: + turr.trackObelisk( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getHeading() + ); + + flywheelVelocity = commander.getVeloPredictive( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + fly.manageFlywheel(flywheelVelocity); + break; + + case MANUAL_TURRET_TRACK_FLY: + turr.manual(turretPosition); + flywheelVelocity = commander.getVeloPredictive( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + + fly.manageFlywheel(flywheelVelocity); + break; + + case MANUAL_FLYWHEEL_TRACK_TURR: + turr.trackGoal( + (follow.getPose().getX() - goalX), + (follow.getPose().getY() - goalY), + follow.getHeading(), + follow.getAngularVelocity(), + follow.getVelocity().getXComponent(), + follow.getAcceleration().getXComponent(), + follow.getVelocity().getYComponent(), + follow.getAcceleration().getYComponent() + ); + fly.manageFlywheel(flywheelVelocity); + 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 5c0e638..ec15235 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 @@ -18,7 +18,6 @@ public class Turret { private final double turretMin = 0.04; //TODO: Tune private final double turretMax = 0.94; //TODO: Tune private final double hVelK = 0; // TODO: Tune - private final double hAccK = 0; // TODO: Tune private final double xVelK = 0; // TODO: Tune private final double xAccK = 0; // TODO: Tune private final double yVelK = 0; // TODO: Tune @@ -81,14 +80,19 @@ public class Turret { return obeliskID; } + public void manual (double pos) { + robot.turr1.setPosition(pos); + robot.turr2.setPosition(pos); + } - public void trackGoal(double dx, double dy, double h, double hVel, double hAcc, double xVel, double xAcc, double yVel, double yAcc) { + + public void trackGoal(double dx, double dy, double h, double hVel, double xVel, double xAcc, double yVel, double yAcc) { // dx, dy, dz is target - robot // h is the raw heading where 0 degrees is positive x in the system of x, y double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot - double predictedH = h + (hVel * hVelK) + (0.5 * hAcc * hAccK); // Positive bc h = robot heading + double predictedH = h + (hVel * hVelK); // Positive bc h = robot heading predictedH = wrapAngle(predictedH); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java new file mode 100644 index 0000000..297b723 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.utilsv2; + +public class VelocityCommander { + private final double goalH = 20.0; //TODO: Tune + private final double xVelK = 0; // TODO: Tune + private final double xAccK = 0; // TODO: Tune + private final double yVelK = 0; // TODO: Tune + private final double yAccK = 0; // TODO: Tune + + public VelocityCommander() { + + } + + private double distToRPM (double dist){ + return Math.sqrt(dist*dist + goalH*goalH); + //TODO: Add regression here using goalH + } + + public double getVeloStationary (double distance){ + return distToRPM(distance); + } + + public double getVeloPredictive(double dx, double dy, double xVel, double xAcc, double yVel, double yAcc) { + + double predictedDx = dx - (xVel * xVelK) - (0.5 * xAcc * xAccK); // Negative bc dx = target - robot + double predictedDy = dy - (yVel * yVelK) - (0.5 * yAcc * yAccK); // Negative bc dy = target - robot + + double predictedDist = Math.sqrt(dx*dx + dy*dy); + + return distToRPM(predictedDist); + } + + +} From aabc746a2eec5a436f960fe4fdb2daf5f4c5a22b Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 15:57:31 -0500 Subject: [PATCH 07/11] Stash update --- .../ftc/teamcode/utilsv2/Shooter.java | 3 +-- .../ftc/teamcode/utilsv2/Spindexer.java | 18 ++++++++++++++++++ 2 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Spindexer.java 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 901981a..5ee5496 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 @@ -33,11 +33,10 @@ public class Shooter { if (redAlliance) { goalX = 144; - goalY = 144; } else { goalX = 0; - goalY = 144; } + goalY = 144; } public void setRedAlliance(boolean input) { 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 new file mode 100644 index 0000000..5965028 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Spindexer.java @@ -0,0 +1,18 @@ +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) { + + + + } +} From 1a1c99791d1d709ac102af1ec40a15129b4c3ec5 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 16:31:33 -0500 Subject: [PATCH 08/11] 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 From ae25df03930cb94166c500e2c01e44716c484ccd Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 17:08:26 -0500 Subject: [PATCH 09/11] Fixed spidnexer i think --- .../ftc/teamcode/teleop/TeleopV4.java | 9 +- .../ftc/teamcode/utilsv2/Drivetrain.java | 2 - .../ftc/teamcode/utilsv2/Intake.java | 20 --- .../ftc/teamcode/utilsv2/Robot.java | 11 +- .../utilsv2/SpindexerTransferIntake.java | 160 ++++++++++-------- 5 files changed, 98 insertions(+), 104 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 7d80708..c280bd7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -6,8 +6,7 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import org.firstinspires.ftc.teamcode.utilsv2.Drivetrain; -import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.*; @TeleOp @Config @@ -18,7 +17,7 @@ public class TeleopV4 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - robot = new Robot(hardwareMap); + robot = Robot.getInstance(hardwareMap); TELE = new MultipleTelemetry( FtcDashboard.getInstance().getTelemetry(), telemetry @@ -26,8 +25,6 @@ public class TeleopV4 extends LinearOpMode { drivetrain = new Drivetrain(robot, TELE); - drivetrain.setTelemetry(true); - waitForStart(); if (isStopRequested()) return; @@ -42,6 +39,8 @@ public class TeleopV4 extends LinearOpMode { gamepad1.left_stick_x ); + + TELE.update(); } 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 6047868..8623588 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 @@ -4,8 +4,6 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.hardware.DcMotor; -import org.firstinspires.ftc.teamcode.utils.Robot; - @Config public class Drivetrain { 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 deleted file mode 100644 index ab240b4..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Intake.java +++ /dev/null @@ -1,20 +0,0 @@ -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 index c805842..58a54d4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -63,9 +63,9 @@ public class Robot { public Servo turr2; public Servo spin1; public Servo spin2; - public TouchSensor beam1; - public TouchSensor beam2; - public TouchSensor beam3; + public TouchSensor insideBeam; + public TouchSensor outsideBeam; + public RevColorSensorV3 revSensor; public VoltageSensor voltage; @@ -139,9 +139,8 @@ public class Robot { 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"); + insideBeam = hardwareMap.get(TouchSensor.class, "beam1"); + outsideBeam = hardwareMap.get(TouchSensor.class, "beam2"); revSensor = hardwareMap.get(RevColorSensorV3.class, "rev"); 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 index 950fdfc..fefb67d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -1,161 +1,179 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; -import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; public class SpindexerTransferIntake { - private Servo spin1; - private Servo spin2; + private final Robot robot; public SpindexerTransferIntake(Robot rob, MultipleTelemetry TELE) { - this.spin1 = rob.spin1; - this.spin2 = rob.spin2; - + this.robot = rob; } - enum SpindexerMode { + private final double sensorDistanceThreshold = 4.0; + private final long pulseTime = 50; // ms + + public 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 + public enum RapidMode { + INTAKE, + TRANSFER_OFF, + BEFORE_PULSE_OUT, + PULSE_OUT, + PULSE_IN, + HOLD_BALLS, + OPEN_GATE, + SHOOT } private SpindexerMode mode = SpindexerMode.RAPID; - private RapidMode rapidMode = RapidMode.INTAKE; - public void setRapidMode (RapidMode rMode) { - this.rapidMode = rMode; + /** + * Time when current state was entered. + */ + private long stateStartTime = System.currentTimeMillis(); + + public void setRapidMode(RapidMode newMode) { + if (rapidMode != newMode) { + rapidMode = newMode; + stateStartTime = System.currentTimeMillis(); + } } - public void setSpindexerMode (SpindexerMode spindexerMode){ + public void setSpindexerMode(SpindexerMode spindexerMode) { this.mode = spindexerMode; } + private long stateTime() { + return System.currentTimeMillis() - stateStartTime; + } + public void update() { switch (mode) { case RAPID: + robot.setSpindexBlockerPos( + ServoPositions.spindexBlocker_Open + ); + switch (rapidMode) { case INTAKE: - // Run front intake - // Run transfer intake - // Keep shooter gate closed - // Keep kicker retracted - // Keep spindexer in default position + robot.setIntakePower(1); + robot.setTransferPower(1); + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Closed + ); + robot.setSpinPos( + ServoPositions.spindexer_A2 + ); + robot.setTransferServoPos( + ServoPositions.transferServo_out + ); - // If first beam break sees ring: - // rapidMode = TRANSFER_OFF; + if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) { + + setRapidMode(RapidMode.TRANSFER_OFF); + } - // If shoot button pressed: - // rapidMode = OPEN_GATE; break; case TRANSFER_OFF: - // Slow down transfer intake - // Allow rings to settle into storage + robot.setTransferPower(0.3); - // If both beam breaks occupied: - // rapidMode = BEFORE_PULSE_OUT; - - // If shoot button pressed: - // rapidMode = OPEN_GATE; + if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) { + setRapidMode(RapidMode.BEFORE_PULSE_OUT); + } break; case BEFORE_PULSE_OUT: - // Continue running intake forward + robot.setIntakePower(1.0); - // After ~0.3 sec: - // rapidMode = PULSE_OUT; + if (stateTime() >= 300) { + setRapidMode(RapidMode.PULSE_OUT); + } break; case PULSE_OUT: - // Reverse intake slightly - // Helps separate rings and prevent jams + robot.setIntakePower(-0.1); - // After pulse time: - // rapidMode = PULSE_IN; + if (stateTime() >= pulseTime) { + setRapidMode(RapidMode.PULSE_IN); + } break; case PULSE_IN: - // Run intake forward again - // Re-seat rings after pulse + robot.setIntakePower(1.0); + + if (stateTime() >= 200) { + setRapidMode(RapidMode.HOLD_BALLS); + } - // After ~0.2 sec: - // rapidMode = HOLD_BALLS; break; case HOLD_BALLS: - // If both sensors occupied: - // hold intake at low power + if (robot.insideBeam.isPressed() + && robot.outsideBeam.isPressed()) { - // Else: - // run intake forward to pull rings back in + robot.setIntakePower(0.1); + + } else { + + robot.setIntakePower(1); + } - // If shoot button pressed: - // rapidMode = OPEN_GATE; - // If resume intake button pressed: - // rapidMode = INTAKE; break; case OPEN_GATE: - // Open upper gate - // Keep intake feeding + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open + ); - // After ~0.1 sec: - // rapidMode = SHOOT; + if (stateTime() >= 100) { + setRapidMode(RapidMode.SHOOT); + } break; case SHOOT: - // Activate kicker - // Feed ring into shooter - - // After ~0.4 sec: - // rapidMode = INTAKE; - + robot.setTransferServoPos( + ServoPositions.transferServo_in + ); + if (stateTime() >= 400) { + setRapidMode(RapidMode.INTAKE); + } break; } - break; case SORTED: // Future sorted-intake logic - // Color sorting - // Alliance filtering - // Different spindexer behavior - break; } } -} +} \ No newline at end of file From 180e7629bff04f384a84dc008d73a72c5c8c5703 Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 17:18:04 -0500 Subject: [PATCH 10/11] Added spindexer to teleopv4 --- .../ftc/teamcode/teleop/TeleopV4.java | 55 +++++++++++++++++++ .../ftc/teamcode/utilsv2/Shooter.java | 2 +- .../utilsv2/SpindexerTransferIntake.java | 4 ++ 3 files changed, 60 insertions(+), 1 deletion(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index c280bd7..b86eb4c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -1,11 +1,19 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.Pose; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.teamcode.constants.Color; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utilsv2.*; @TeleOp @@ -13,7 +21,10 @@ import org.firstinspires.ftc.teamcode.utilsv2.*; public class TeleopV4 extends LinearOpMode { Robot robot; Drivetrain drivetrain; + Shooter shooter; MultipleTelemetry TELE; + Follower follower; + SpindexerTransferIntake spindexerTransferIntake; @Override public void runOpMode() throws InterruptedException { @@ -24,6 +35,15 @@ public class TeleopV4 extends LinearOpMode { ); drivetrain = new Drivetrain(robot, TELE); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); + + shooter = new Shooter(robot, TELE, follower, Color.redAlliance); + shooter.setState(Shooter.ShooterState.TRACK_GOAL); + spindexerTransferIntake = new SpindexerTransferIntake(robot, TELE); + spindexerTransferIntake.setSpindexerMode(SpindexerTransferIntake.SpindexerMode.RAPID); + waitForStart(); @@ -39,6 +59,41 @@ public class TeleopV4 extends LinearOpMode { gamepad1.left_stick_x ); + shooter.update(); + spindexerTransferIntake.update(); + + SpindexerTransferIntake.RapidMode state = spindexerTransferIntake.getRapidState(); + + if (gamepad1.xWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF || + state == SpindexerTransferIntake.RapidMode.BEFORE_PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_OUT || + state == SpindexerTransferIntake.RapidMode.PULSE_IN || + state == SpindexerTransferIntake.RapidMode.HOLD_BALLS)) { + + spindexerTransferIntake.setRapidMode(SpindexerTransferIntake.RapidMode.OPEN_GATE); + } + + if (gamepad1.aWasPressed() && + (state == SpindexerTransferIntake.RapidMode.INTAKE || + state == SpindexerTransferIntake.RapidMode.TRANSFER_OFF)) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.HOLD_BALLS + ); + } + + if (gamepad1.yWasPressed() + && state == SpindexerTransferIntake.RapidMode.HOLD_BALLS) { + + spindexerTransferIntake.setRapidMode( + SpindexerTransferIntake.RapidMode.INTAKE + ); + } + + + TELE.update(); 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 9c60f8f..068aea2 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 @@ -45,7 +45,7 @@ public class Shooter { private double flywheelVelocity = 0.0; private double turretPosition = 0.5; - enum ShooterState { + public enum ShooterState { READ_OBELISK, TRACK_GOAL, MANUAL_FLYWHEEL_TRACK_TURR, 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 index fefb67d..0dc6cb4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java @@ -51,6 +51,10 @@ public class SpindexerTransferIntake { this.mode = spindexerMode; } + public RapidMode getRapidState(){ + return this.rapidMode; + } + private long stateTime() { return System.currentTimeMillis() - stateStartTime; } From a540d333f1430d6142d710159d1d121bfe0882fd Mon Sep 17 00:00:00 2001 From: Keshav Anand Date: Tue, 2 Jun 2026 18:12:32 -0500 Subject: [PATCH 11/11] shoooooottteeer test --- .../teamcode/constants/ServoPositions.java | 2 +- .../ftc/teamcode/teleop/TeleopV4.java | 6 +- .../ftc/teamcode/tests/NewShooterTest.java | 126 ++++++++++++++++++ .../ftc/teamcode/utilsv2/Robot.java | 2 +- 4 files changed, 130 insertions(+), 6 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java 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 50cc8cc..7aa9159 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 @@ -5,7 +5,7 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double rapidFireBlocker_Closed = 0.3; + public static double rapidFireBlocker_Closed = 0.35; public static double rapidFireBlocker_Open = 0.5; public static double spindexBlocker_Closed = 0.31; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index b86eb4c..a061c24 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -25,6 +25,7 @@ public class TeleopV4 extends LinearOpMode { MultipleTelemetry TELE; Follower follower; SpindexerTransferIntake spindexerTransferIntake; + @Override public void runOpMode() throws InterruptedException { @@ -49,7 +50,7 @@ public class TeleopV4 extends LinearOpMode { if (isStopRequested()) return; - while (opModeIsActive()){ + while (opModeIsActive()) { //Drivetrain @@ -93,9 +94,6 @@ public class TeleopV4 extends LinearOpMode { } - - - TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java new file mode 100644 index 0000000..fafc88c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java @@ -0,0 +1,126 @@ +package org.firstinspires.ftc.teamcode.tests; + +import com.acmerobotics.dashboard.FtcDashboard; +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 org.firstinspires.ftc.teamcode.constants.ServoPositions; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; +import org.firstinspires.ftc.teamcode.utilsv2.Robot; +import org.firstinspires.ftc.teamcode.utilsv2.Shooter; + +@Config +@TeleOp +public class NewShooterTest extends LinearOpMode { + + Robot robot; + + public static boolean intake = true; + public static boolean shoot = false; + public static double intakePower = 1.0; + public static double transferShootPower = -1; + public static double transferIntakePower = -1.0; + public static double turretPos = 0.51; + public static double hoodPos = 0.51; + public static double flywheel = 0; + + private enum ShootState { + IDLE, + WAIT_GATE, + WAIT_PUSH + } + + private ShootState shootState = ShootState.IDLE; + private long timestamp = 0; + + @Override + public void runOpMode() throws InterruptedException { + + Robot.resetInstance(); + + robot = Robot.getInstance(hardwareMap); + + Shooter shooter = new Shooter( + robot, + new MultipleTelemetry( + telemetry, + FtcDashboard.getInstance().getTelemetry() + ), + Constants.createFollower(hardwareMap), + true + ); + + shooter.setState(Shooter.ShooterState.MANUAL); + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + + robot.setHoodPos(hoodPos); + shooter.setTurretPosition(turretPos); + shooter.setFlywheelVelocity(flywheel); + robot.setSpinPos(ServoPositions.spindexer_A2); + + if (intake && !shoot) { + + shootState = ShootState.IDLE; + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Closed); + + robot.setTransferPower(transferIntakePower); + robot.setIntakePower(intakePower); + robot.setTransferServoPos( + ServoPositions.transferServo_out); + } + + else if (shoot) { + robot.setIntakePower(intakePower); + + + switch (shootState) { + + case IDLE: + + robot.setTransferPower(transferShootPower); + + timestamp = System.currentTimeMillis(); + shootState = ShootState.WAIT_GATE; + + break; + + case WAIT_GATE: + + if (System.currentTimeMillis() - timestamp >= 300) { + + robot.setRapidFireBlockerPos( + ServoPositions.rapidFireBlocker_Open); + + timestamp = System.currentTimeMillis(); + shootState = ShootState.WAIT_PUSH; + } + + break; + + case WAIT_PUSH: + + if (System.currentTimeMillis() - timestamp >= 100) { + + robot.setTransferServoPos( + ServoPositions.transferServo_in); + + shootState = ShootState.IDLE; + } + + break; + } + } + + shooter.update(); + } + } +} \ No newline at end of file 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 index 58a54d4..224cdca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java @@ -125,7 +125,7 @@ public class Robot { spin2 = hardwareMap.get(Servo.class, "spin2"); transfer = hardwareMap.get(DcMotorEx.class, "transfer"); - transfer.setDirection(DcMotorSimple.Direction.REVERSE); + transferServo = hardwareMap.get(Servo.class, "transferServo");