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