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

new method since no longer flippable due to angle being 54 and not 45

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-21 13:44:31 -06:00
parent e87c5bb845
commit 5a456e211f
1 changed files with 15 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -87,6 +87,11 @@ public class Targeting {
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) { 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);
if (redAlliance){
sin54 = Math.sin(Math.toRadians(54));
} else {
sin54 = Math.sin(Math.toRadians(-54))
}
// TODO: test these values determined from the fmap // TODO: test these values determined from the fmap
double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
@@ -102,6 +107,16 @@ public class Targeting {
int remX = Math.floorMod((int) robotInchesX, tileSize); int remX = Math.floorMod((int) robotInchesX, tileSize);
int remY = Math.floorMod((int) robotInchesY, tileSize); int remY = Math.floorMod((int) robotInchesY, tileSize);
//clamp
//if (redAlliance) {
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
//} else {
// robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
// robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
//}
// Determine if we need to interpolate based on tile position. // Determine if we need to interpolate based on tile position.
// if near upper or lower quarter or tile interpolate with next tile. // if near upper or lower quarter or tile interpolate with next tile.
int x0 = 0; int x0 = 0;
@@ -173,16 +188,6 @@ public class Targeting {
interpolate = false; interpolate = false;
} }
//clamp
if (redAlliance) {
robotGridX = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridY = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
} else {
robotGridY = Math.max(0, Math.min(gridX, KNOWNTARGETING[0].length - 1));
robotGridX = Math.max(0, Math.min(gridY, KNOWNTARGETING.length - 1));
}
// basic search // basic search
if (true) { //!interpolate) { if (true) { //!interpolate) {
if ((robotGridY < 6) && (robotGridX < 6)) { if ((robotGridY < 6) && (robotGridX < 6)) {