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 850a400..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,17 +5,17 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0; + public static double spindexer_intakePos1 = 0.05; //0.13; - public static double spindexer_intakePos2 = 0.19;//0.5; + public static double spindexer_intakePos2 = 0.24; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.38;//0.66; + public static double spindexer_intakePos3 = 0.43; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.65; + public static double spindexer_outtakeBall3 = 0.70; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.46; - public static double spindexer_outtakeBall1 = 0.27; - public static double spinStartPos = spindexer_outtakeBall1 - 0.08; + 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; 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 d4d3aac..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 @@ -3,6 +3,10 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Servos.spinD; @@ -122,7 +126,8 @@ public class TeleopV3 extends LinearOpMode { private int tickerA = 1; private boolean transferIn = false; boolean turretInterpolate = false; - public static double spinSpeedIncrease = 0.04; + public static double spinSpeedIncrease = 0.03; + public static int resetSpinTicks = 4; public static double velPrediction(double distance) { if (distance < 30) { @@ -582,16 +587,14 @@ public class TeleopV3 extends LinearOpMode { // } if (enableSpindexerManager) { - if (!shootAll) { - spindexer.processIntake(); - } + spindexer.processIntake(); // RIGHT_BUMPER - if (gamepad1.right_bumper) { + if (gamepad1.right_bumper && intakeTicker > resetSpinTicks) { robot.intake.setPower(1); - } else { robot.intake.setPower(0); + } // LEFT_BUMPER @@ -603,37 +606,52 @@ public class TeleopV3 extends LinearOpMode { shootAll = true; shooterTicker = 0; - spindexPos = spinStartPos;// TODO: Change starting position based on desired order to shoot green ball } - + intakeTicker++; if (shootAll) { - + intakeTicker = 0; intake = false; reject = false; - if (getRuntime() - shootStamp < 3.5) { - - if (shooterTicker == 0 && !servo.spinEqual(spindexPos)){ - robot.spin1.setPosition(spindexPos); - robot.spin2.setPosition(1-spindexPos); - } else { - robot.transferServo.setPosition(transferServo_in); - shooterTicker++; - double prevSpinPos = robot.spin1.getPosition(); - robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); - robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); - } - +// if (servo.getSpinPos() < spindexer_outtakeBall2 + 0.4) { +// +// if (shooterTicker == 0){ +// robot.transferServo.setPosition(transferServo_out); +// robot.spin1.setPosition(spinStartPos); +// robot.spin2.setPosition(1-spinStartPos); +// if (servo.spinEqual(spinStartPos)){ +// shooterTicker++; +// } +// TELE.addLine("starting to shoot"); +// } else { +// robot.transferServo.setPosition(transferServo_in); +// shooterTicker++; +// double prevSpinPos = servo.getSpinPos(); +// robot.spin1.setPosition(prevSpinPos + spinSpeedIncrease); +// robot.spin2.setPosition(1 - prevSpinPos - spinSpeedIncrease); +// TELE.addLine("shooting"); +// } + if (shooterTicker == 0) { + 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); + TELE.addLine("shoot"); } else { robot.transferServo.setPosition(transferServo_out); //spindexPos = spindexer_intakePos1; - shootAll = false; - spindexer.resetSpindexer(); - spindexer.processIntake(); + //spindexer.processIntake(); + TELE.addLine("stop shooting"); } + shooterTicker++; + //spindexer.processIntake(); } if (gamepad1.left_stick_button){ @@ -818,42 +836,40 @@ public class TeleopV3 extends LinearOpMode { hub.clearBulkCache(); } // - TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); - TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); - TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); - - TELE.addData("pose", drive.localizer.getPose()); - TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); - TELE.addData("distanceToGoal", distanceToGoal); - TELE.addData("hood", robot.hood.getPosition()); - TELE.addData("targetVel", vel); - TELE.addData("Velocity", flywheel.getVelo()); - TELE.addData("Velo1", flywheel.velo1); - TELE.addData("Velo2", flywheel.velo2); - TELE.addData("shootOrder", shootOrder); - TELE.addData("oddColor", oddBallColor); - - // Spindexer Debug +// TELE.addData("Spin1Green", green1 + ": " + ballIn(1)); +// TELE.addData("Spin2Green", green2 + ": " + ballIn(2)); +// TELE.addData("Spin3Green", green3 + ": " + ballIn(3)); +// +// TELE.addData("pose", drive.localizer.getPose()); +// TELE.addData("heading", drive.localizer.getPose().heading.toDouble()); +// TELE.addData("distanceToGoal", distanceToGoal); +// TELE.addData("hood", robot.hood.getPosition()); +// TELE.addData("targetVel", vel); +// TELE.addData("Velocity", flywheel.getVelo()); +// TELE.addData("Velo1", flywheel.velo1); +// TELE.addData("Velo2", flywheel.velo2); +// TELE.addData("shootOrder", shootOrder); +// TELE.addData("oddColor", oddBallColor); +// +// // Spindexer Debug TELE.addData("spinEqual", servo.spinEqual(spindexer_intakePos1)); TELE.addData("spinCommmandedPos", spindexer.commandedIntakePosition); TELE.addData("spinIntakeState", spindexer.currentIntakeState); TELE.addData("spinTestCounter", spindexer.counter); TELE.addData("autoSpintake", autoSpintake); - //TELE.addData("distanceRearCenter", spindexer.distanceRearCenter); - //TELE.addData("distanceFrontDriver", spindexer.distanceFrontDriver); - //TELE.addData("distanceFrontPassenger", spindexer.distanceFrontPassenger); - TELE.addData("shootall commanded", shootAll); - // Targeting Debug - TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); - 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("timeSinceStamp", getRuntime() - shootStamp); +// +// TELE.addData("shootall commanded", shootAll); +// // Targeting Debug +// TELE.addData("robotX", robotX); +// TELE.addData("robotY", robotY); +// 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("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 4496cae..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 @@ -39,6 +39,8 @@ public class Spindexer { public double distanceFrontDriver = 0.0; public double distanceFrontPassenger = 0.0; + private double prevPos = 0.0; + public Types.Motif desiredMotif = Types.Motif.NONE; // For Use enum RotatedBallPositionNames { @@ -70,7 +72,10 @@ public class Spindexer { SHOOT_ALL_READY } + int shootWaitCount = 0; + public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; + public IntakeState prevIntakeState = IntakeState.UNKNOWN_START; public int unknownColorDetect = 0; enum BallColor { UNKNOWN, @@ -246,10 +251,21 @@ public class Spindexer { return newPos1Detection; } - - public void moveSpindexerToPos(double pos) { + // Has code to unjam spindexer + private void moveSpindexerToPos(double pos) { robot.spin1.setPosition(pos); robot.spin2.setPosition(1-pos); +// double currentPos = servos.getSpinPos(); +// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ +// if (currentPos > pos){ +// robot.spin1.setPosition(servos.getSpinPos() + 0.05); +// robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05); +// } else { +// robot.spin1.setPosition(servos.getSpinPos() - 0.05); +// robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); +// } +// } +// prevPos = currentPos; } public void stopSpindexer() { @@ -278,6 +294,10 @@ public class Spindexer { } } + public boolean slotIsEmpty(int slot){ + return !ballPositions[slot].isEmpty; + } + public boolean isFull () { return (!ballPositions[0].isEmpty && !ballPositions[1].isEmpty && !ballPositions[2].isEmpty); } @@ -287,7 +307,7 @@ public class Spindexer { case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; - servos.setSpinPos(intakePositions[0]); + moveSpindexerToPos(intakePositions[0]); currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; break; case UNKNOWN_MOVE: @@ -323,27 +343,24 @@ 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; - servos.setSpinPos(intakePositions[commandedIntakePosition]); 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; - servos.setSpinPos(intakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.MOVING; } - //proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); + proposedTravelDistance = Math.abs(intakePositions[2] - currentSpindexerPos); if (ballPositions[2].isEmpty) { // Position 3 commandedIntakePosition = 2; - servos.setSpinPos(intakePositions[commandedIntakePosition]); currentIntakeState = Spindexer.IntakeState.MOVING; } if (currentIntakeState != Spindexer.IntakeState.MOVING) { @@ -380,47 +397,42 @@ public class Spindexer { case SHOOT_ALL_PREP: // We get here with function call to prepareToShootMotif // Stopping when we get to the new position - if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_READY; - } else { + if (!servos.spinEqual(outakePositions[commandedIntakePosition])) { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" } break; - case SHOOT_ALL_READY: + case SHOOT_ALL_READY: // Not used // Double Check Colors //detectBalls(false, false); // Minimize hardware calls if (ballPositions[0].isEmpty && ballPositions[1].isEmpty && ballPositions[2].isEmpty) { // All ball shot move to intake state - currentIntakeState = Spindexer.IntakeState.FINDNEXT; + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; } // Maintain Position - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + moveSpindexerToPos(outakePositions[commandedIntakePosition]); break; case SHOOTNEXT: // Find Next Open Position and start movement - if (!ballPositions[0].isEmpty) { + if (!ballPositions[1].isEmpty) { // Position 1 - commandedIntakePosition = 0; - servos.setSpinPos(outakePositions[commandedIntakePosition]); - currentIntakeState = Spindexer.IntakeState.SHOOTMOVING; - } else if (ballPositions[1].isEmpty) { // Possible error: should it be !ballPosition[1].isEmpty? - // Position 2 commandedIntakePosition = 1; - servos.setSpinPos(outakePositions[commandedIntakePosition]); 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; - servos.setSpinPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + 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; } - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); break; case SHOOTMOVING: @@ -429,25 +441,34 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; } else { // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); // Possible error: should it be using "outakePositions" instead of "intakePositions" + moveSpindexerToPos(outakePositions[commandedIntakePosition]); } break; case SHOOTWAIT: // Stopping when we get to the new position - if (servos.spinEqual(intakePositions[commandedIntakePosition])) { - currentIntakeState = Spindexer.IntakeState.INTAKE; - stopSpindexer(); - //detectBalls(true, false); + if (prevIntakeState != currentIntakeState) { + shootWaitCount = 0; } else { - // Keep moving the spindexer - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + shootWaitCount++; } + // wait 10 cycles + if (shootWaitCount > 2) { + currentIntakeState = Spindexer.IntakeState.SHOOTNEXT; + ballPositions[commandedIntakePosition].isEmpty = true; + shootWaitCount = 0; + //stopSpindexer(); + //detectBalls(true, false); + } + // Keep moving the spindexer + moveSpindexerToPos(outakePositions[commandedIntakePosition]+(shootWaitCount*0.01)); break; default: // Statements to execute if no case matches } + + prevIntakeState = currentIntakeState; //TELE.addData("commandedIntakePosition", commandedIntakePosition); //TELE.update(); // Signal a successful intake @@ -499,6 +520,22 @@ public class Spindexer { commandedIntakePosition = bestFitMotif(); } + public void prepareShootAll(){ + currentIntakeState = Spindexer.IntakeState.SHOOT_ALL_PREP; + } + public void shootAll () { + currentIntakeState = Spindexer.IntakeState.SHOOTWAIT; + } + + public boolean shootAllComplete () + { + return ((currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_PREP) && + (currentIntakeState != Spindexer.IntakeState.SHOOT_ALL_READY) && + (currentIntakeState != Spindexer.IntakeState.SHOOTMOVING) && + (currentIntakeState != Spindexer.IntakeState.SHOOTNEXT) && + (currentIntakeState != Spindexer.IntakeState.SHOOTWAIT)); + } + void shootAllToIntake () { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index bfb87d9..56b70a9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -67,19 +67,19 @@ public class Targeting { KNOWNTARGETING[3][4] = new Settings (3100.0, 0.47); KNOWNTARGETING[3][5] = new Settings (3100.0, 0.47); // ROW 4 - KNOWNTARGETING[4][0] = new Settings (4540.0, 0.1); - KNOWNTARGETING[4][1] = new Settings (4541.0, 0.1); - KNOWNTARGETING[4][2] = new Settings (4542.0, 0.1); - KNOWNTARGETING[4][3] = new Settings (4543.0, 0.1); - KNOWNTARGETING[4][4] = new Settings (4544.0, 0.1); - KNOWNTARGETING[4][5] = new Settings (4545.0, 0.1); - // ROW 1 - KNOWNTARGETING[5][0] = new Settings (4550.0, 0.1); - KNOWNTARGETING[5][1] = new Settings (4551.0, 0.1); - KNOWNTARGETING[5][2] = new Settings (4552.0, 0.1); - KNOWNTARGETING[5][3] = new Settings (4553.0, 0.1); - KNOWNTARGETING[5][4] = new Settings (4554.0, 0.1); - KNOWNTARGETING[5][5] = new Settings (4555.0, 0.1); + KNOWNTARGETING[4][0] = new Settings (3100, 0.49); + KNOWNTARGETING[4][1] = new Settings (3100, 0.49); + KNOWNTARGETING[4][2] = new Settings (3100, 0.5); + KNOWNTARGETING[4][3] = new Settings (3200, 0.5); + KNOWNTARGETING[4][4] = new Settings (3250, 0.49); + KNOWNTARGETING[4][5] = new Settings (3300, 0.49); + // ROW 5 + KNOWNTARGETING[5][0] = new Settings (3200, 0.48); + KNOWNTARGETING[5][1] = new Settings (3200, 0.48); + KNOWNTARGETING[5][2] = new Settings (3300, 0.48); + KNOWNTARGETING[5][3] = new Settings (3350, 0.48); + KNOWNTARGETING[5][4] = new Settings (3350, 0.48); + KNOWNTARGETING[5][5] = new Settings (3350, 0.48); } public Targeting() @@ -107,61 +107,81 @@ public class Targeting { // Determine if we need to interpolate based on tile position. // if near upper or lower quarter or tile interpolate with next tile. + int x0 = 0; + int y0 = 0; int x1 = 0; int y1 = 0; -// interpolate = false; -// if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && -// (robotGridX < 5) && (robotGridY <5)) { -// // +X, +Y -// interpolate = true; -// x1 = robotGridX + 1; -// y1 = robotGridY + 1; -// } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && -// (robotGridX > 0) && (robotGridY > 0)) { -// // -X, -Y -// interpolate = true; -// x1 = robotGridX - 1; -// y1 = robotGridY - 1; -// } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && -// (robotGridX < 5) && (robotGridY > 0)) { -// // +X, -Y -// interpolate = true; -// x1 = robotGridX + 1; -// y1 = robotGridY - 1; -// } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && -// (robotGridX > 0) && (robotGridY < 5)) { -// // -X, +Y -// interpolate = true; -// x1 = robotGridX - 1; -// y1 = robotGridY + 1; -// } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { -// // -X, Y -// interpolate = true; -// x1 = robotGridX - 1; -// y1 = robotGridY; -// } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { -// // X, -Y -// interpolate = true; -// x1 = robotGridX; -// y1 = robotGridY - 1; -// } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { -// // +X, Y -// interpolate = true; -// x1 = robotGridX + 1; -// y1 = robotGridY; -// } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { -// // X, +Y -// interpolate = true; -// x1 = robotGridX; -// y1 = robotGridY + 1; -// } + interpolate = false; + if ((remX > TILE_UPPER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && + (robotGridX < 5) && (robotGridY <5)) { + // +X, +Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY; + y1 = robotGridY + 1; + } else if ((remX < TILE_LOWER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && + (robotGridX > 0) && (robotGridY > 0)) { + // -X, -Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX > TILE_UPPER_QUARTILE) && (remY < TILE_LOWER_QUARTILE) && + (robotGridX < 5) && (robotGridY > 0)) { + // +X, -Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX < TILE_LOWER_QUARTILE) && (remY > TILE_UPPER_QUARTILE) && + (robotGridX > 0) && (robotGridY < 5)) { + // -X, +Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY + 1; + } else if ((remX < TILE_LOWER_QUARTILE) && (robotGridX > 0)) { + // -X, Y + interpolate = true; + x0 = robotGridX - 1; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY; + } else if ((remY < TILE_LOWER_QUARTILE) && (robotGridY > 0)) { + // X, -Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX; + y0 = robotGridY - 1; + y1 = robotGridY; + } else if ((remX > TILE_UPPER_QUARTILE) && (robotGridX < 5)) { + // +X, Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX + 1; + y0 = robotGridY; + y1 = robotGridY; + } else if ((remY > TILE_UPPER_QUARTILE) && (robotGridY < 5)) { + // X, +Y + interpolate = true; + x0 = robotGridX; + x1 = robotGridX; + y0 = robotGridY; + y1 = robotGridY + 1; + } else { + interpolate = false; + } //clamp robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1)); robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1)); // basic search - if(!interpolate) { + if(true) { //!interpolate) { if ((robotGridY < 6) && (robotGridX <6)) { recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridX][robotGridY].flywheelRPM; recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridX][robotGridY].hoodAngle; @@ -170,27 +190,29 @@ public class Targeting { } else { // bilinear interpolation - int x0 = robotGridX; + //int x0 = robotGridX; //int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); - int y0 = robotGridY; + //int y0 = robotGridY; //int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1); - double x = (robotInchesX - (x0 * tileSize)) / tileSize; - double y = (robotInchesY - (y0 * tileSize)) / tileSize; +// double x = (robotInchesX - (x0 * tileSize)) / tileSize; +// double y = (robotInchesY - (y0 * tileSize)) / tileSize; - double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; - double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; - double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; - double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; - - double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; - double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; - double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; - double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; - - recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; - recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; +// double rpm00 = KNOWNTARGETING[y0][x0].flywheelRPM; +// double rpm10 = KNOWNTARGETING[y0][x1].flywheelRPM; +// double rpm01 = KNOWNTARGETING[y1][x0].flywheelRPM; +// double rpm11 = KNOWNTARGETING[y1][x1].flywheelRPM; +// +// double angle00 = KNOWNTARGETING[y0][x0].hoodAngle; +// double angle10 = KNOWNTARGETING[y0][x1].hoodAngle; +// double angle01 = KNOWNTARGETING[y1][x0].hoodAngle; +// double angle11 = KNOWNTARGETING[y1][x1].hoodAngle; +// recommendedSettings.flywheelRPM = (1 - x) * (1 - y) * rpm00 + x * (1 - y) * rpm10 + (1 - x) * y * rpm01 + x * y * rpm11; +// recommendedSettings.hoodAngle = (1 - x) * (1 - y) * angle00 + x * (1 - y) * angle10 + (1 - x) * y * angle01 + x * y * angle11; + // Average target tiles + recommendedSettings.flywheelRPM = (KNOWNTARGETING[x0][y0].flywheelRPM + KNOWNTARGETING[x1][y1].flywheelRPM)/2.0; + recommendedSettings.hoodAngle = (KNOWNTARGETING[x0][y0].hoodAngle + KNOWNTARGETING[x1][y1].hoodAngle)/2.0; return recommendedSettings; } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 492a02f..d2def91 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -119,17 +119,14 @@ public class Turret { } public double getTy() { - limelightRead(); return ty; } public double getLimelightX() { - limelightRead(); return limelightPosX; } public double getLimelightY() { - limelightRead(); return limelightPosY; } diff --git a/build.dependencies.gradle b/build.dependencies.gradle index f9030be..28189d8 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -26,10 +26,10 @@ dependencies { implementation 'com.bylazar:fullpanels:1.0.2' //Panels - implementation "com.acmerobotics.roadrunner:ftc:0.1.25" - implementation "com.acmerobotics.roadrunner:core:1.0.1" - implementation "com.acmerobotics.roadrunner:actions:1.0.1" - implementation "com.acmerobotics.dashboard:dashboard:0.5.1" + implementation "com.acmerobotics.roadrunner:ftc:0.1.25" //RR + implementation "com.acmerobotics.roadrunner:core:1.0.1" //RR + implementation "com.acmerobotics.roadrunner:actions:1.0.1" //RR + implementation "com.acmerobotics.dashboard:dashboard:0.5.1" //FTC Dash implementation 'org.ftclib.ftclib:core:2.1.1' // FTC LIB