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

night before regionals

Browse Source
This commit is contained in:
DanTheMan-byte
2026-02-28 01:33:26 -06:00
parent e8d28b9e5f
commit a1340c5388
5 changed files with 38 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java
View File

@@ -2,6 +2,7 @@ package org.firstinspires.ftc.teamcode.autonomous;
import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance;
import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos0;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3;
@@ -371,7 +372,7 @@ public class Auto_LT_Close extends LinearOpMode {
pickupGateBY = bPickupGateBY;
pickupGateBH = bPickupGateBH;
obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0;
obeliskTurrPosAutoStart = turrDefault + blueObeliskTurrPos0;
obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1;
obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2;
obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3;

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java
View File

@@ -7,8 +7,8 @@ import com.acmerobotics.roadrunner.Pose2d;
public class Front_Poses {
public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1;
public static double bx1 = 20, by1 = -0.5, bh1 = -0.1;
public static double rx1 = 30, ry1 = 5, rh1 = 0.1;
public static double bx1 = 30, by1 = -5, bh1 = -0.1;
public static double rx2a = 41, ry2a = 18, rh2a = 140;
public static double bx2a = 41, by2a = -18, bh2a = -140;
@@ -33,8 +33,8 @@ public class Front_Poses {
public static double rfx1 = 10, rfy1 = 0, rfh1 = 0;
public static double rShootX = 40, rShootY = 10, rShootH = 50;
public static double bShootX = 40, bShootY = -10, bShootH = -50;
public static double rShootX = 60, rShootY = 10, rShootH = 50;
public static double bShootX = 60, bShootY = -10, bShootH = -50;
public static double rxPrep = 45, ryPrep = 10, rhPrep = 50;
public static double bxPrep = 45, byPrep = -10, bhPrep = -50;

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java
View File

@@ -16,10 +16,10 @@ public class ServoPositions {
public static double spindexer_outtakeBall2 = 0.66; //0.46; //0.6;
public static double spindexer_outtakeBall1 = 0.47; //0.27; //0.4;
public static double spinStartPos = 0.10;
public static double spinStartPos = 0.05;
public static double spinEndPos = 0.95;
public static double shootAllSpindexerSpeedIncrease = 0.013;
public static double shootAllSpindexerSpeedIncrease = 0.008;
public static double transferServo_out = 0.15;

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

@@ -361,8 +361,8 @@ public class TeleopV3 extends LinearOpMode {
// TELE.addData("robotInchesX", targeting.robotInchesX);
// 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 GridX", targeting.robotGridX);
TELE.addData("Targeting GridY", targeting.robotGridY);
// TELE.addData("Targeting FlyWheel", targetingSettings.flywheelRPM);
// TELE.addData("Targeting HoodAngle", targetingSettings.hoodAngle);
// TELE.addData("timeSinceStamp", getRuntime() - shootStamp);
@@ -376,7 +376,7 @@ public class TeleopV3 extends LinearOpMode {
TELE.update();
//light.update();
light.update();
ticker++;
}

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

@@ -87,12 +87,22 @@ public class Targeting {
public Settings calculateSettings(double robotX, double robotY, double robotHeading, double robotVelocity, boolean interpolate) {
Settings recommendedSettings = new Settings(0.0, 0.0);
int gridX;
int gridY;
if (!redAlliance){
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 + 20;
robotInchesY = rotatedY * unitConversionFactor + 20;
// Find approximate location in the grid
gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize));
gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
} else {
sin54 = Math.sin(Math.toRadians(-54));
}
// TODO: test these values determined from the fmap
double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54;
double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54;
@@ -101,21 +111,23 @@ public class Targeting {
robotInchesY = rotatedY * unitConversionFactor;
// Find approximate location in the grid
int gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
int gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1);
gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize));
}
int remX = Math.floorMod((int) robotInchesX, tileSize);
int remY = Math.floorMod((int) robotInchesY, tileSize);
//clamp
//if (redAlliance) {
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));
//}
} 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.
// if near upper or lower quarter or tile interpolate with next tile.