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 9652da6..3fa4906 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,16 +5,16 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.00; //0.13; + public static double spindexer_intakePos1 = 0.05; //0.13; - public static double spindexer_intakePos2 = 0.19; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.24; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.38; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.65; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.46; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.27; //0.27; //0.4; + 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 spinStartPos = spindexer_outtakeBall3 - 0.1; 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 f5e77d5..567cf73 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 @@ -633,25 +633,25 @@ public class TeleopV3 extends LinearOpMode { // TELE.addLine("shooting"); // } if (shooterTicker == 0) { - shooterTicker++; spindexer.prepareShootAll(); + TELE.addLine("preparing to shoot"); + } else if (shooterTicker == 2) { + robot.transferServo.setPosition(transferServo_in); + spindexer.shootAll(); TELE.addLine("starting to shoot"); } else if (!spindexer.shootAllComplete()) { robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - spindexer.shootAll(); TELE.addLine("shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); //spindexer.processIntake(); TELE.addLine("stop shooting"); } - spindexer.processIntake(); + shooterTicker++; + //spindexer.processIntake(); } if (gamepad1.left_stick_button){ 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 488c0e2..79dcab3 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 @@ -343,21 +343,21 @@ public class Spindexer { // Find Next Open Position and start movement double currentSpindexerPos = servos.getSpinPos(); double commandedtravelDistance = 2.0; - //double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos); + double proposedTravelDistance = Math.abs(intakePositions[0] - currentSpindexerPos); //if (ballPositions[0].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { if (ballPositions[0].isEmpty) { // Position 1 commandedIntakePosition = 0; currentIntakeState = Spindexer.IntakeState.MOVING; } - //proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); + proposedTravelDistance = Math.abs(intakePositions[1] - currentSpindexerPos); //if (ballPositions[1].isEmpty && (proposedTravelDistance < commandedtravelDistance)) { if (ballPositions[1].isEmpty) { // Position 2 commandedIntakePosition = 1; currentIntakeState = Spindexer.IntakeState.MOVING; } - //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); + proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); if (ballPositions[2].isEmpty) { // Position 3 commandedIntakePosition = 2; @@ -416,18 +416,18 @@ public class Spindexer { case SHOOTNEXT: // Find Next Open Position and start movement - if (!ballPositions[0].isEmpty) { + if (!ballPositions[1].isEmpty) { // Position 1 - commandedIntakePosition = 0; - currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (!ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? - // Position 2 commandedIntakePosition = 1; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (!ballPositions[2].isEmpty) { // Possible error: should it be !ballPosition[2].isEmpty? - // Position 3 + } else if (!ballPositions[2].isEmpty) { + // Position 2 commandedIntakePosition = 2; currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; + } else if (!ballPositions[0].isEmpty) { + // Position 3 + commandedIntakePosition = 0; + currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; } else { // Empty return to intake state currentIntakeState = IntakeState.FINDNEXT; @@ -452,16 +452,16 @@ public class Spindexer { } else { shootWaitCount++; } - // wait 3 cycles - if ((shootWaitCount > 15) && (servos.spinEqual(outakePositions[commandedIntakePosition]))) { + // wait 10 cycles + if (shootWaitCount > 2) { currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; ballPositions[commandedIntakePosition].isEmpty = true; + shootWaitCount = 0; //stopSpindexer(); //detectBalls(true, false); - } else { - // Keep moving the spindexer - moveSpindexerToPos(outakePositions[commandedIntakePosition]); } + // Keep moving the spindexer + moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01)); break; default: