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

targeting angle determined

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-21 12:01:20 -06:00
parent 56b61ee88b
commit 1ad33fd45b
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -84,11 +84,11 @@ public class Targeting {
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 + cancelOffsetX) * sin45 + (robotY + cancelOffsetY) * cos45;
double rotatedX = (robotX + cancelOffsetX) * cos45 - (robotY + cancelOffsetY) * sin45;
// TODO: test these values determined from the fmap
double cos54 = Math.cos(Math.toRadians(-54));
double sin54 = Math.sin(Math.toRadians(-54));
double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
// Convert robot coordinates to inches
robotInchesX = rotatedX * unitConversionFactor;