diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java index 1f3cedb..e8cd4e8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Poses.java @@ -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); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java index b570aba..12b6073 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV2.java @@ -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) {