KeshavAnandCode/FTCDecodeMain
1
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

Merge branch 'Targeting'

Browse Source
This commit is contained in:
Keshav Anand
2026-01-23 20:24:16 -06:00
parent 16ffdd003f 4050a354f7
commit 3591e20001
3 changed files with 115 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java
View File

@@ -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);

110
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java
View File

@@ -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]);

52
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java
View File

@@ -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;
}
}