Last commit of 23344...

README.md

@@ -1,3 +1,5 @@
+Farewell, this is the last commit for the team 23344...to any who may use this repository, please feel free to contact us (keshavanandofficial@gmail.com), and we hope our code is of use to you and your team.
+
 ## NOTICE
 
 This repository contains the public FTC SDK for the DECODE (2025-2026) competition season.

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto12Ball_Back_Sorted.java

@@ -227,6 +227,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 }
                 break;
             case OPENGATE:
+                robot.setIntakePower(0);
                 if (currentTime - timeStamp > openGateTime){
                     follower.followPath(openGate_shoot1, true);
                     pathState = PathState.DRIVE_SHOOT1;
@@ -275,7 +276,7 @@ public class Auto12Ball_Back_Sorted extends LinearOpMode {
                 break;
             case DRIVE_PICKUP3:
                 if (!follower.isBusy()){
-                    shooter.setFlywheelVelocity(2200);
+                    shooter.setFlywheelVelocity(2250);
                     robot.setHoodPos(0.75);
                     follower.followPath(drivePickup3_pickup3, intakePower, false);
                     pathState = PathState.PICKUP3;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto21Ball_Front_Gate.java

@@ -47,7 +47,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
     public static double rapidShootTime = 0.4;
     public static double openGate1Time = 1.8;
     public static double openGate2Time = 1;
-    public static double openGateWaitTimeMax = 3.5;
+    public static double openGateWaitTimeMax = 3;
     public static int maxLoopCycles = 4;
 
     // Initialize path state machine
@@ -76,7 +76,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
     public static double shoot2X = 16, shoot2Y = 4, shoot2H = -30;
     public static double intakeGateControlX = 60, intakeGateControlY = -12;
     public static double toGateX = 60, toGateY = -10;
-    public static double intakeGateX = 62, intakeGateY = -12.5, intakeGateH = 25;
+    public static double intakeGateX = 62, intakeGateY = -12, intakeGateH = 25;
     public static double shootGateControlX = 40, shootGateControlY = -5;
     public static double shootGateX = 16, shootGateY = 4, shootGateH = -30;
     public static double shootLeaveControlX = 56, shootLeaveControlY = -10;
@@ -294,7 +294,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                 if (currentTime - timeStamp > rapidShootTime ||
                         (spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.OPEN_GATE &&
                                 spindexer.getRapidState() != SpindexerTransferIntake.RapidMode.SHOOT)){
-                    follower.followPath(shoot2_intakeGate, false);
+                    follower.followPath(shoot2_intakeGate, true);
                     pathState = PathState.INTAKE_GATE;
                     timeStamp = currentTime;
                     toGateBool = true;
@@ -302,10 +302,10 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                 break;
             case INTAKE_GATE:
                 if ((currentTime - timeStamp > openGateWaitTimeMax)){
-                    if (getRuntime() - runtime > 27){
-                        follower.followPath(intakeGate_awayFromGate, true);
-                        pathState = PathState.WAIT_SHOOT_LEAVE;
-                    } else if (getRuntime() - runtime > 22 || cycle >= maxLoopCycles - 1){
+                    if (cycle == 0){
+                        openGateWaitTimeMax = openGateWaitTimeMax + 0.75;
+                    }
+                    if (cycle >= maxLoopCycles - 1){
                         follower.followPath(intakeGate_shootLeave, true);
                         pathState = PathState.DRIVE_SHOOT_LEAVE;
                         shooter.setFlywheelVelocity(2300);
@@ -336,7 +336,7 @@ public class Auto21Ball_Front_Gate extends LinearOpMode {
                         follower.followPath(shootGate_leave, true);
                         pathState = PathState.WAIT_SHOOT_LEAVE;
                     } else {
-                        follower.followPath(shootGate_intakeGate, false);
+                        follower.followPath(shootGate_intakeGate, true);
                         pathState = PathState.INTAKE_GATE;
                         toGateBool = true;
                     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java

@@ -8,6 +8,7 @@ import com.pedropathing.geometry.Pose;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
+import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.teamcode.constants.Color;
 import org.firstinspires.ftc.teamcode.constants.ServoPositions;
 import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
@@ -31,7 +32,7 @@ public class TeleopV4 extends LinearOpMode {
     ParkTilter parkTilter;
     MeasuringLoopTimes loopTimes;
 
-    public static Pose relocalizePose = new Pose(56, 11, 0);
+    public static Pose relocalizePose = new Pose(54.7, 11.4, 0);
     public static Pose teleStart = new Pose(0,0,0);
 
     private boolean firstTickFull = true;
@@ -122,16 +123,18 @@ public class TeleopV4 extends LinearOpMode {
 
             if (gamepad1.crossWasPressed()){
                 if (Color.redAlliance){
-                    relocalizePose = new Pose(57.5, 5, 0);
+                    relocalizePose = new Pose(54.4, 12, 0);
                 } else {
-                    relocalizePose = new Pose(-57.5, 5, Math.toRadians(180));
+                    relocalizePose = new Pose(-54.4, 12, Math.toRadians(180));
                 }
                 follower.setPose(relocalizePose);
                 sleep(500);
                 gamepad1.rumble(100);
             }
 
+            if (!isStopRequested()){
                 follower.update();
+            }
             Pose currentPose = follower.getPose();
             teleStart = currentPose;
 
@@ -208,16 +211,11 @@ public class TeleopV4 extends LinearOpMode {
                     shooting = false;
                 }
 
-                if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed() && !shooting){
-                    intakeFull = true;
-                } else {
-                    intakeFull = false;
-                    firstTickFull = true;
-                }
-
-                if (intakeFull && firstTickFull){
-                    gamepad1.rumble(100);
+                if (SpindexerTransferIntake.intakeFull && firstTickFull){
+                    gamepad1.rumble(250);
                     firstTickFull = false;
+                } else if (!SpindexerTransferIntake.intakeFull){
+                    firstTickFull = true;
                 }
 
                 if (gamepad1.right_trigger > 0.5 &&
@@ -254,9 +252,9 @@ public class TeleopV4 extends LinearOpMode {
             }
 
             if (gamepad2.dpadUpWasPressed()){
-                hoodOffset+=0.02;
-            } else if (gamepad2.dpadDownWasPressed()){
                 hoodOffset-=0.02;
+            } else if (gamepad2.dpadDownWasPressed()){
+                hoodOffset+=0.02;
             }
 
             if (gamepad2.dpadRightWasPressed()){
@@ -275,15 +273,25 @@ public class TeleopV4 extends LinearOpMode {
 //            TELE.addData("Loop Time Max", loopTimes.getMaxLoopTimeOneMin());
 //            TELE.addData("Loop Time Min", loopTimes.getMinLoopTimeOneMin());
 //
-            TELE.addData("Distance From Goal", commander.getDistance());
+//            TELE.addData("Distance From Goal", commander.getDistance());
 //            TELE.addData("Hood Position", commander.getHoodPredicted());
 //            TELE.addData("Transfer Power", robot.transfer.getPower());
-            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
-            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
-            TELE.addData("Velocity 1", flywheel.getVelo1());
-            TELE.addData("Velocity 2", flywheel.getVelo2());
+//            TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM());
+//            TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity());
+//            TELE.addData("Velocity 1", flywheel.getVelo1());
+//            TELE.addData("Velocity 2", flywheel.getVelo2());
             TELE.addData("Flywheel Offset", flywheelOffset);
             TELE.addData("Hood Offset", hoodOffset);
+
+//            TELE.addData("FR Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("FL Drive", robot.frontRight.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BL Drive", robot.backLeft.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("BR Drive", robot.backRight.getCurrent(CurrentUnit.AMPS));
+
+//            TELE.addData("Flywheel 1", robot.shooter1.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Flywheel 2", robot.shooter2.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Transfer", robot.transfer.getCurrent(CurrentUnit.AMPS));
+//            TELE.addData("Intake", robot.intake.getCurrent(CurrentUnit.AMPS));
 //
 //            TELE.addData("Current Position", currentPose);
 //

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/NewShooterTest.java

@@ -36,7 +36,7 @@ public class NewShooterTest extends LinearOpMode {
     private boolean shooting = false;
     public static int flywheelVelo = 0;
     public static double hoodPos = 0.5;
-    public static double transferPower = -1;
+    public static double transferPower = -0.8;
 
     @Override
     public void runOpMode() throws InterruptedException {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Robot.java

@@ -15,6 +15,7 @@ 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.robotcore.external.navigation.CurrentUnit;
 import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
 
 public class Robot {
@@ -80,6 +81,11 @@ public class Robot {
     public Limelight3A limelight;
     public Servo light;
 
+    // Current Limits
+    public static double intakeCurrentLimit = 6.0;
+    public static double transferCurrentLimit = 5.0;
+    public static double drivetrainCurrentLimit = 7.0;
+
     public Robot(HardwareMap hardwareMap) {
 
         //Define components w/ hardware map
@@ -120,7 +126,6 @@ public class Robot {
 
         transfer = hardwareMap.get(DcMotorEx.class, "transfer");
 
-
         transferServo = hardwareMap.get(Servo.class, "transferServo");
 
         transfer.setDirection(DcMotorSimple.Direction.REVERSE);
@@ -138,6 +143,13 @@ public class Robot {
 
         revSensor = hardwareMap.get(RevColorSensorV3.class, "rev");
 
+        frontLeft.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        frontRight.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        backLeft.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        backRight.setCurrentAlert(drivetrainCurrentLimit, CurrentUnit.AMPS);
+        intake.setCurrentAlert(intakeCurrentLimit, CurrentUnit.AMPS);
+        transfer.setCurrentAlert(transferCurrentLimit, CurrentUnit.AMPS);
+
         // Below is disregarded
 
 //        turr1Pos = hardwareMap.get(AnalogInput.class, "t1Pos"); // Encoder of turret plugged in intake port
@@ -174,6 +186,12 @@ public class Robot {
 
     private double prevFrontLeftPower = -10.501;
     public void setFrontLeftPower(double pow){
+        if (frontLeft.isOverCurrent()){
+            double current = frontLeft.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevFrontLeftPower){
             frontLeft.setPower(pow);
@@ -183,6 +201,12 @@ public class Robot {
 
     private double prevFrontRightPower = -10.501;
     public void setFrontRightPower(double pow){
+        if (frontRight.isOverCurrent()){
+            double current = frontRight.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevFrontRightPower){
             frontRight.setPower(pow);
@@ -192,6 +216,12 @@ public class Robot {
 
     private double prevBackLeftPower = -10.501;
     public void setBackLeftPower(double pow){
+        if (backLeft.isOverCurrent()){
+            double current = backLeft.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevBackLeftPower){
             backLeft.setPower(pow);
@@ -201,6 +231,12 @@ public class Robot {
 
     private double prevBackRightPower = -10.501;
     public void setBackRightPower(double pow){
+        if (backRight.isOverCurrent()){
+            double current = backRight.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * drivetrainCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevBackRightPower){
             backRight.setPower(pow);
@@ -210,6 +246,12 @@ public class Robot {
 
     private double prevIntakePower = -10.501;
     public void setIntakePower(double pow){
+        if (intake.isOverCurrent()){
+            double current = intake.getCurrent(CurrentUnit.AMPS);
+            if (current != 0) {
+                pow = pow * intakeCurrentLimit / current;
+            }
+        }
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
         if (pow != prevIntakePower){
             intake.setPower(pow);
@@ -220,6 +262,12 @@ public class Robot {
     private double prevTransferPower = -10.501;
     public void setTransferPower(double pow){
         pow = (double) Math.round((float) pow * roundingFactor) / roundingFactor;
+//        if (transfer.isOverCurrent()){
+//            double current = transfer.getCurrent(CurrentUnit.AMPS);
+//            if (current != 0) {
+//                pow = pow * transferCurrentLimit / current;
+//            }
+//        }
         if (pow != prevTransferPower){
             transfer.setPower(pow);
         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Shooter.java

@@ -138,8 +138,8 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
-
-                robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
+                double hood1 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset));
+                robot.setHoodPos(hood1);
                 fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                 fly.setF(voltage);
                 break;
@@ -181,7 +181,8 @@ public class Shooter {
                 );
 
                 flywheelVelocity = commander.getPredictedRPM();
-                robot.setHoodPos(commander.getHoodPredicted() + TeleopV4.hoodOffset);
+                double hood2 = Math.max(0.35, Math.min(0.88, commander.getHoodPredicted() + TeleopV4.hoodOffset));
+                robot.setHoodPos(hood2);
                 fly.manageFlywheel(flywheelVelocity + TeleopV4.flywheelOffset);
                 break;
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/SpindexerTransferIntake.java

@@ -53,7 +53,7 @@ public class SpindexerTransferIntake {
     }
 
     int[] shootOrder = {0, 1, 2};
-    public static final double sensorDistanceThreshold = 5.35;
+    public static final double sensorDistanceThreshold = 5.0;//6.0;//4.85//5.35
     final long pulseTime = 70; // ms
 
     private DesiredPattern desiredPattern = DesiredPattern.GPP;
@@ -205,7 +205,7 @@ public class SpindexerTransferIntake {
     private RapidMode rapidMode = RapidMode.INTAKE;
     private SortedIntakeStates sortedIntakeStates = SortedIntakeStates.IDLE;
     private BallStates[] ballColors = {BallStates.UNKNOWN, BallStates.UNKNOWN, BallStates.UNKNOWN};
-    final double greenThresh = 0.39;
+    final double greenThresh = 0.41;
     final double spinMovementTime = 250;
 
     /**
@@ -289,6 +289,7 @@ public class SpindexerTransferIntake {
     private int greenTicks = 0;
     private int ballTicks = 0;
     private int holdBallsTicker = 0;
+    public static boolean intakeFull = false;
     public void update() {
 
 //        TELE.addData("Sorted State", sortedIntakeStates);
@@ -325,17 +326,16 @@ public class SpindexerTransferIntake {
                         robot.setSpinPos(
                                 ServoPositions.spindexer_A2
                         );
-                        robot.setTransferPower(-1);
+                        robot.setTransferPower(-0.8);
                         robot.setTransferServoPos(
                                 ServoPositions.transferServo_out
                         );
 
-                        if (robot.insideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
-
+                        if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             holdBallsTicker++;
                         }
 
-                        if (holdBallsTicker > 20){
+                        if (holdBallsTicker > 5){
                             setRapidMode(RapidMode.TRANSFER_OFF);
                             holdBallsTicker = 0;
                         }
@@ -347,6 +347,7 @@ public class SpindexerTransferIntake {
                         if (robot.insideBeam.isPressed() && robot.outsideBeam.isPressed()) {
                             setRapidMode(RapidMode.BEFORE_PULSE_OUT);
                         }
+                        robot.setTransferServoPos(ServoPositions.transferServo_in);
                         robot.setTransferPower(-0.3);
 
                         break;
@@ -355,7 +356,7 @@ public class SpindexerTransferIntake {
 
                         robot.setIntakePower(1.0);
 
-                        if (stateTime() >= 700) {
+                        if (stateTime() >= 100) {
                             setRapidMode(RapidMode.PULSE_OUT);
                         }
 
@@ -363,7 +364,7 @@ public class SpindexerTransferIntake {
 
                     case PULSE_OUT:
 
-                        robot.setIntakePower(-0.1);
+//                        robot.setIntakePower(0);
 
                         if (stateTime() >= pulseTime) {
                             setRapidMode(RapidMode.PULSE_IN);
@@ -375,7 +376,7 @@ public class SpindexerTransferIntake {
 
                         robot.setIntakePower(1.0);
 
-                        if (stateTime() >= 400) {
+                        if (stateTime() >= 100) {
                             setRapidMode(RapidMode.HOLD_BALLS);
                         }
 
@@ -384,7 +385,9 @@ public class SpindexerTransferIntake {
                     case HOLD_BALLS:
 
                         if (robot.insideBeam.isPressed()
-                                && robot.outsideBeam.isPressed() && robot.revSensor.getDistance(DistanceUnit.CM) < sensorDistanceThreshold) {
+                                && robot.outsideBeam.isPressed()) {
+
+                            intakeFull = true;
 
                             robot.setTransferPower(0);
                             robot.setIntakePower(0.1);
@@ -392,11 +395,12 @@ public class SpindexerTransferIntake {
 
                         } else {
                             setRapidMode(RapidMode.INTAKE);
+                            intakeFull = false;
                         }
                         break;
 
                     case OPEN_GATE:
-
+                        intakeFull = false;
                         if (stateTime() >= 100) {
                             setRapidMode(RapidMode.SHOOT);
                         }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java

@@ -17,8 +17,8 @@ public class Turret {
 
     private final double servoTicksPer180 = 0.58;
     public static double neutralPosition = 0.51;
-    private final double turretMin = 0.08;
-    private final double turretMax = 0.91;
+    private final double turretMin = 0.13;
+    private final double turretMax = 0.87;
     public static boolean limelightUsed = true;
     public static double B_PID_P = 0.0001, B_PID_I = 0.0, B_PID_D = 0.000005;
     LLResult result;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/VelocityCommander.java

@@ -79,9 +79,9 @@ public class VelocityCommander {
 
     private void distToTransferPow(double dist, double voltage){
         if (dist < 140){
-            transferPow = -1;
+            transferPow = -0.85;
         } else {
-            transferPow = -0.6;
+            transferPow = -0.7;
         }
     }
     public double getTransferPow(){return transferPow;}