diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java index 298c802..7b942f9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV4.java @@ -91,6 +91,7 @@ public class TeleopV4 extends LinearOpMode { } follower.update(); + Pose currentPose = follower.getPose(); if (gamepad1.dpadLeftWasPressed()){ shooter.setState(Shooter.ShooterState.MANUAL_FLYWHEEL_TRACK_TURR); @@ -158,6 +159,8 @@ public class TeleopV4 extends LinearOpMode { TELE.addData("Theoretical Velocity RPM", commander.getPredictedRPM()); TELE.addData("Actual Velocity RPM", flywheel.getAverageVelocity()); + TELE.addData("Current Position", currentPose); + TELE.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java index 9ee6419..f239aac 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilsv2/Turret.java @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.utilsv2; import com.acmerobotics.dashboard.config.Config; -import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.arcrobotics.ftclib.controller.PIDController; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; @@ -25,7 +24,7 @@ public class Turret { LLResult result; PIDController bearingPID; boolean bearingAligned = false; - public int LL_COAST_TICKS = 60; + public int LL_COAST_TICKS = 5; public static double TARGET_POSITION_TOLERANCE = 0.5; public static double alphaTX = 0.5; private double targetTx = 0;