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 97824bb..f65f972 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 @@ -120,6 +120,7 @@ public class TeleopV3 extends LinearOpMode { private double transferStamp = 0.0; private int tickerA = 1; private boolean transferIn = false; + boolean turretInterpolate = false; public static double velPrediction(double distance) { if (distance < 30) { @@ -394,7 +395,7 @@ public class TeleopV3 extends LinearOpMode { double distanceToGoal = Math.sqrt(dx * dx + dy * dy); targetingSettings = targeting.calculateSettings - (robotX, robotY, robotHeading, 0.0); + (robotX,robotY,robotHeading,0.0, turretInterpolate); turret.trackGoal(deltaPose); @@ -636,6 +637,7 @@ public class TeleopV3 extends LinearOpMode { } } + // // if (shootAll) { // @@ -834,9 +836,10 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("shootall commanded", shootAll); // Targeting Debug TELE.addData("robotX", robotX); - TELE.addData("robotY", robotY); + TELE.addData( "robotY", robotY); TELE.addData("robotInchesX", targeting.robotInchesX); - TELE.addData("robotInchesY", targeting.robotInchesY); + 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); 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 e5b7d5b..3d95256 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 @@ -52,7 +52,9 @@ public class Spindexer { } enum IntakeState { - UNKNOWN, + UNKNOWN_START, + UNKNOWN_MOVE, + UNKNOWN_DETECT, INTAKE, FINDNEXT, MOVING, @@ -62,8 +64,8 @@ public class Spindexer { SHOOTWAIT, }; - public IntakeState currentIntakeState = IntakeState.UNKNOWN; - + public IntakeState currentIntakeState = IntakeState.UNKNOWN_START; + public int unknownColorDetect = 0; enum BallColor { UNKNOWN, GREEN, @@ -131,13 +133,13 @@ public class Spindexer { for (int i = 0; i < 3; i++) { resetBallPosition(i); } - currentIntakeState = IntakeState.UNKNOWN; + currentIntakeState = IntakeState.UNKNOWN_START; } // Detects if a ball is found and what color. // Returns true is there was a new ball found in Position 1 // FIXIT: Reduce number of times that we read the color sensors for loop times. - public boolean detectBalls() { + public boolean detectBalls(boolean detectRearColor, boolean detectFrontColor) { boolean newPos1Detection = false; int spindexerBallPos = 0; @@ -153,18 +155,20 @@ public class Spindexer { // Mark Ball Found newPos1Detection = true; - // Detect which color - double green = robot.color1.getNormalizedColors().green; - double red = robot.color1.getNormalizedColors().red; - double blue = robot.color1.getNormalizedColors().blue; + if (detectRearColor) { + // Detect which color + double green = robot.color1.getNormalizedColors().green; + double red = robot.color1.getNormalizedColors().red; + double blue = robot.color1.getNormalizedColors().blue; - double gP = green / (green + red + blue); + double gP = green / (green + red + blue); - // FIXIT - Add filtering to improve accuracy. - if (gP >= 0.4) { - ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple - } else { - ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple + // FIXIT - Add filtering to improve accuracy. + if (gP >= 0.4) { + ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // purple + } } } // Position 2 @@ -173,18 +177,19 @@ public class Spindexer { if (distanceFrontDriver < 60) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - // FIXIT: Comment out for now due to loop time concerns -// double green = robot.color2.getNormalizedColors().green; -// double red = robot.color2.getNormalizedColors().red; -// double blue = robot.color2.getNormalizedColors().blue; -// -// double gP = green / (green + red + blue); + if (detectFrontColor) { + double green = robot.color2.getNormalizedColors().green; + double red = robot.color2.getNormalizedColors().red; + double blue = robot.color2.getNormalizedColors().blue; -// if (gP >= 0.4) { -// b2 = 2; // purple -// } else { -// b2 = 1; // green -// } + double gP = green / (green + red + blue); + + if (gP >= 0.4) { + ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple + } + } } else { if (!ballPositions[spindexerBallPos].isEmpty) { if (ballPositions[spindexerBallPos].foundEmpty > 3) { @@ -201,18 +206,19 @@ public class Spindexer { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; - // FIXIT: Comment out for now due to loop time concerns -// double green = robot.color3.getNormalizedColors().green; -// double red = robot.color3.getNormalizedColors().red; -// double blue = robot.color3.getNormalizedColors().blue; + if (detectFrontColor) { + double green = robot.color3.getNormalizedColors().green; + double red = robot.color3.getNormalizedColors().red; + double blue = robot.color3.getNormalizedColors().blue; -// double gP = green / (green + red + blue); + double gP = green / (green + red + blue); -// if (gP >= 0.4) { -// b3 = 2; // purple -// } else { -// b3 = 1; // green -// } + if (gP >= 0.4) { + ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple + } else { + ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // purple + } + } } else { if (!ballPositions[spindexerBallPos].isEmpty) { if (ballPositions[spindexerBallPos].foundEmpty > 3) { @@ -255,15 +261,35 @@ public class Spindexer { public boolean processIntake() { switch (currentIntakeState) { - case UNKNOWN: + case UNKNOWN_START: // For now just set position ONE if UNKNOWN commandedIntakePosition = 0; servos.setSpinPos(intakePositions[0]); - currentIntakeState = Spindexer.IntakeState.MOVING; + currentIntakeState = Spindexer.IntakeState.UNKNOWN_MOVE; + break; + case UNKNOWN_MOVE: + // Stopping when we get to the new position + if (servos.spinEqual(intakePositions[commandedIntakePosition])) { + currentIntakeState = Spindexer.IntakeState.UNKNOWN_DETECT; + stopSpindexer(); + detectBalls(true, true); + unknownColorDetect = 0; + } else { + // Keep moving the spindexer + moveSpindexerToPos(intakePositions[commandedIntakePosition]); + } + break; + case UNKNOWN_DETECT: + if (unknownColorDetect >5) { + currentIntakeState = Spindexer.IntakeState.FINDNEXT; + } else { + detectBalls(true, true); + unknownColorDetect++; + } break; case INTAKE: // Ready for intake and Detecting a New Ball - if (detectBalls()) { + if (detectBalls(true, false)) { ballPositions[commandedIntakePosition].isEmpty = false; currentIntakeState = Spindexer.IntakeState.FINDNEXT; } else { @@ -311,7 +337,7 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); - detectBalls(); + detectBalls(false, false); } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]); @@ -320,7 +346,7 @@ public class Spindexer { case FULL: // Double Check Colors - detectBalls(); + detectBalls(false, false); // Minimize hardware calls if (ballPositions[0].isEmpty || ballPositions[1].isEmpty || ballPositions[2].isEmpty) { // Error handling found an empty spot, get it ready for a ball currentIntakeState = Spindexer.IntakeState.FINDNEXT; @@ -378,7 +404,7 @@ public class Spindexer { if (servos.spinEqual(intakePositions[commandedIntakePosition])) { currentIntakeState = Spindexer.IntakeState.INTAKE; stopSpindexer(); - detectBalls(); + detectBalls(true, false); } else { // Keep moving the spindexer moveSpindexerToPos(intakePositions[commandedIntakePosition]); 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 7a2acb0..d9850a5 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 @@ -84,28 +84,58 @@ public class Targeting { { } - public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity) { + public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { Settings recommendedSettings = new Settings(0.0, 0.0); double cos45 = Math.cos(Math.toRadians(-45)); double sin45 = Math.sin(Math.toRadians(-45)); - double rotatedY = (robotX -40.0) * sin45 + (robotY +7.0) * cos45; - double rotatedX = (robotX -40.0) * cos45 - (robotY +7.0) * sin45; + double rotatedY = (robotX - 40.0) * sin45 + (robotY + 7.0) * cos45; + double rotatedX = (robotX - 40.0) * cos45 - (robotY + 7.0) * sin45; // Convert robot coordinates to inches robotInchesX = rotatedX * unitConversionFactor; robotInchesY = rotatedY * unitConversionFactor; // Find approximate location in the grid - robotGridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) +1); - robotGridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); + int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); + int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); - // Use Grid Location to perform lookup - // Keep it simple for now but may want to interpolate results - if ((robotGridY < 6) && (robotGridX <6)) { - recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; - recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + //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 ((robotGridY < 6) && (robotGridX <6)) { + recommendedSettings.flywheelRPM = KNOWNTARGETING[robotGridY][robotGridX].flywheelRPM; + recommendedSettings.hoodAngle = KNOWNTARGETING[robotGridY][robotGridX].hoodAngle; + } + return recommendedSettings; + } else { + + // bilinear interpolation + int x0 = robotGridX; + int x1 = Math.min(x0 + 1, KNOWNTARGETING[0].length - 1); + int y0 = gridY; + int y1 = Math.min(y0 + 1, KNOWNTARGETING.length - 1); + + 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; + + return recommendedSettings; } - return recommendedSettings; } }