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

stash

Browse Source
This commit is contained in:
KeshavAnandCode
2025-12-02 19:45:15 -06:00
parent 55dbfaaa98
commit 873d0c5134
2 changed files with 19 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java
View File

@@ -1,6 +1,5 @@
package org.firstinspires.ftc.teamcode.constants;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.Pose2d;
@@ -19,11 +18,11 @@ public class Poses {
public static double x2_b = 58, y2_b = 42, h2_b = Math.toRadians(140);
public static double x3 = 34, y3 = 58, h3 = Math.toRadians(140);
public static Pose2d teleStart = new Pose2d(x1,-10,0);
public static double tx = 0, ty = 0, th = 0;
public static Pose2d teleStart = new Pose2d(tx, ty, th);
public static Pose2d goalPose = new Pose2d(-15, 0, 0);
}

16
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java
View File

@@ -1,5 +1,6 @@
package org.firstinspires.ftc.teamcode.teleop;
import static org.firstinspires.ftc.teamcode.constants.Poses.goalPose;
import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart;
import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1;
import static org.firstinspires.ftc.teamcode.tests.ShooterTest.kP;
@@ -8,6 +9,7 @@ import static org.firstinspires.ftc.teamcode.tests.ShooterTest.maxStep;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.Pose2d;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
@@ -245,6 +247,20 @@ public class TeleopV2 extends LinearOpMode {
double offset;
double robotX = drive.localizer.getPose().position.x;
double robotY = drive.localizer.getPose().position.y;
double goalX = -10;
double goalY = 0;
double dx = goalX - robotX; // delta x from robot to goal
double dy = goalY - robotY; // delta y from robot to goal
double angleRad = Math.atan2(dy, dx);
desiredTurretAngle = (Math.toDegrees(Math.atan2(dy, dx)) + 360) % 360;
offset = desiredTurretAngle - Math.toDegrees(drive.localizer.getPose().heading.toDouble());
if (offset > 90) {