From 3f4fee0e2497e1ac89bce9d62d6e4e179826d16a Mon Sep 17 00:00:00 2001 From: Matt9150 Date: Thu, 29 Jan 2026 09:25:39 -0600 Subject: [PATCH] Add functions to get the ball color to spindexer. Attempt to make shoot all in teleop work better. --- .../autonomous/ProtoAutoClose_V4.java | 1 - .../teamcode/constants/ServoPositions.java | 9 +- .../ftc/teamcode/teleop/TeleopV3.java | 140 +++++++++++------- .../ftc/teamcode/utils/Spindexer.java | 40 +++-- 4 files changed, 119 insertions(+), 71 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java index 418719c..039c5c8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/ProtoAutoClose_V4.java @@ -309,7 +309,6 @@ public class ProtoAutoClose_V4 extends LinearOpMode { return false; } - } }; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 3fa4906..ac39dbd 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 @@ -11,12 +11,15 @@ public class ServoPositions { public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.71; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.51; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.32; //0.27; //0.4; + public static double spindexer_outtakeBall2 = 0.53; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.35; //0.27; //0.4; public static double spinStartPos = spindexer_outtakeBall3 - 0.1; + public static double shootAllAutoSpinStartPos = 0.2; + public static double shootAllSpindexerSpeedIncrease = 0.02; + public static double shootAllTime = 1.8; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 567cf73..53c9332 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -31,6 +31,7 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -250,56 +251,56 @@ public class TeleopV3 extends LinearOpMode { } - if (autoSpintake) { - - if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { - - - robot.spin1.setPosition(spindexPos); - robot.spin2.setPosition(1-spindexPos); - - } - - if (gamepad1.right_bumper) { - - shootAll = false; - - intakeTicker++; - -// if (intakeTicker % 20 < 2) { +// if (autoSpintake) { // -// robot.spin1.setPower(-1); -// robot.spin2.setPower(1); +// if (!servo.spinEqual(spindexPos) && !gamepad1.right_bumper) { +// +// +// robot.spin1.setPosition(spindexPos); +// robot.spin2.setPosition(1-spindexPos); +// +// } +// +// if (gamepad1.right_bumper) { +// +// shootAll = false; +// +// intakeTicker++; +// +//// if (intakeTicker % 20 < 2) { +//// +//// robot.spin1.setPower(-1); +//// robot.spin2.setPower(1); +//// +//// } else if (intakeTicker % 20 < 10) { +//// robot.spin1.setPower(-0.5); +//// robot.spin2.setPower(0.5); +//// } else if (intakeTicker % 20 < 12) { +//// robot.spin1.setPower(1); +//// robot.spin2.setPower(-1); +//// } else { +//// robot.spin1.setPower(0.5); +//// robot.spin2.setPower(-0.5); +//// } +// +// robot.intake.setPower(1); +// intakeStamp = getRuntime(); +// TELE.addData("Reverse?", reverse); +// TELE.update(); +// } else { +// if (!servo.spinEqual(spindexPos)) { +// robot.spin1.setPosition(spindexPos); +// robot.spin2.setPosition(1-spindexPos); // -// } else if (intakeTicker % 20 < 10) { -// robot.spin1.setPower(-0.5); -// robot.spin2.setPower(0.5); -// } else if (intakeTicker % 20 < 12) { -// robot.spin1.setPower(1); -// robot.spin2.setPower(-1); -// } else { -// robot.spin1.setPower(0.5); -// robot.spin2.setPower(-0.5); // } - - robot.intake.setPower(1); - intakeStamp = getRuntime(); - TELE.addData("Reverse?", reverse); - TELE.update(); - } else { - if (!servo.spinEqual(spindexPos)) { - robot.spin1.setPosition(spindexPos); - robot.spin2.setPosition(1-spindexPos); - - } - - spindexPos = spindexer_intakePos1; - - robot.intake.setPower(0); - - intakeTicker = 0; - } - } +// +// spindexPos = spindexer_intakePos1; +// +// robot.intake.setPower(0); +// +// intakeTicker = 0; +// } +// } //COLOR: @@ -587,7 +588,9 @@ public class TeleopV3 extends LinearOpMode { // } if (enableSpindexerManager) { - spindexer.processIntake(); + //if (!shootAll) { + spindexer.processIntake(); + //} // RIGHT_BUMPER if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { @@ -598,10 +601,7 @@ public class TeleopV3 extends LinearOpMode { } // LEFT_BUMPER - if (!shootAll && - (gamepad1.leftBumperWasReleased() || - gamepad1.leftBumperWasPressed() || - gamepad1.left_bumper)) { + if (!shootAll && gamepad1.leftBumperWasReleased()) { shootStamp = getRuntime(); shootAll = true; @@ -632,11 +632,37 @@ public class TeleopV3 extends LinearOpMode { // robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); // TELE.addLine("shooting"); // } + +// //robot.intake.setPower(-0.3); +// if (getRuntime() - shootStamp < 3.0) { +// +// if (shooterTicker == 0 && !servo.spinEqual(ServoPositions.shootAllAutoSpinStartPos)) { +// robot.spin1.setPosition(ServoPositions.shootAllAutoSpinStartPos); +// robot.spin2.setPosition(1 - ServoPositions.shootAllAutoSpinStartPos); +// } else { +// shooterTicker++; +// //robot.intake.setPower(0.0); +// robot.transferServo.setPosition(transferServo_in); +// double prevSpinPos = robot.spin1.getPosition(); +// robot.spin1.setPosition(prevSpinPos + ServoPositions.shootAllSpindexerSpeedIncrease); +// robot.spin2.setPosition(1 - prevSpinPos - ServoPositions.shootAllAutoSpinStartPos); +// } +// +// } else { +// robot.transferServo.setPosition(transferServo_out); +// //spindexPos = spindexer_intakePos1; +// shootAll = false; +// spindexer.resetSpindexer(); +// spindexer.processIntake(); +// +// } + + if (shooterTicker == 0) { spindexer.prepareShootAll(); TELE.addLine("preparing to shoot"); } else if (shooterTicker == 2) { - robot.transferServo.setPosition(transferServo_in); + //robot.transferServo.setPosition(transferServo_in); spindexer.shootAll(); TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { @@ -865,10 +891,10 @@ public class TeleopV3 extends LinearOpMode { // TELE.addData("robotInchesX", targeting.robotInchesX); // TELE.addData( "robotInchesY", targeting.robotInchesY); // TELE.addData("Targeting Interpolate", turretInterpolate); -// TELE.addData("Targeting GridX", targeting.robotGridX); -// TELE.addData("Targeting GridY", targeting.robotGridY); -// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); -// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); + TELE.addData("Targeting GridX", targeting.robotGridX); + TELE.addData("Targeting GridY", targeting.robotGridY); + TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM); + TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle); // TELE.addData("timeSinceStamp", getRuntime() - shootStamp); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 79dcab3..d842368 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -77,7 +77,7 @@ public class Spindexer { public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; public int unknownColorDetect = 0; - enum BallColor { + public enum BallColor { UNKNOWN, GREEN, PURPLE @@ -397,6 +397,7 @@ public class Spindexer { case SHOOT_ALL_PREP: // We get here with function call to prepareToShootMotif // Stopping when we get to the new position + commandedIntakePosition = 0; if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" @@ -416,17 +417,17 @@ public class Spindexer { case SHOOTNEXT: // Find Next Open Position and start movement - if (!ballPositions[1].isEmpty) { + if (!ballPositions[0].isEmpty) { // Position 1 + commandedIntakePosition = 0; + currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; + } else if (!ballPositions[1].isEmpty) { + // Position 2 commandedIntakePosition = 1; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else if (!ballPositions[2].isEmpty) { - // Position 2 - commandedIntakePosition = 2; - currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (!ballPositions[0].isEmpty) { // Position 3 - commandedIntakePosition = 0; + commandedIntakePosition = 2; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else { // Empty return to intake state @@ -446,14 +447,18 @@ public class Spindexer { break; case SHOOTWAIT: + double shootWaitMax = 3; // Stopping when we get to the new position if (prevIntakeState != currentIntakeState) { + if (commandedIntakePosition==2) { + shootWaitMax = 5; + } shootWaitCount = 0; } else { shootWaitCount++; } // wait 10 cycles - if (shootWaitCount > 2) { + if (shootWaitCount > shootWaitMax) { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; ballPositions[commandedIntakePosition].isEmpty = true; shootWaitCount = 0; @@ -461,7 +466,7 @@ public class Spindexer { //detectBalls(true, false); } // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01)); + //moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.02)); break; default: @@ -524,7 +529,10 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP; } public void shootAll () { - currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; + ballPositions[0].isEmpty = false; + ballPositions[1].isEmpty = false; + ballPositions[2].isEmpty = false; + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } public boolean shootAllComplete () @@ -543,4 +551,16 @@ public class Spindexer { public void update() { } + + public BallColor GetFrontDriverColor () { + return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTDRIVER.ordinal()]].ballColor; + } + + public BallColor GetFrontPassengerColor () { + return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]].ballColor; + } + + public BallColor GetRearCenterColor () { + return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor; + } }