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