diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index d0dbe76..f75a111 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -65,4 +65,7 @@ public class Front_Poses { public static double bOpenGateMiddleX = 36, bOpenGateMiddleY = -59, bOpenGateMiddleH = -50; public static Pose2d teleStart = new Pose2d(0, 0, 0); + + //For PedroPathing TODO: figure out how to change start poses in auto + public static double teleStartPoseX = 72, teleStartPoseY = 72, teleStartPoseH = 0; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index 51b533a..8f54aca 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -5,21 +5,21 @@ import com.acmerobotics.dashboard.config.Config; @Config public class ServoPositions { - public static double spindexer_intakePos1 = 0.26; //0.13; + public static double spindexer_intakePos1 = 0.18; //0.13; - public static double spindexer_intakePos2 = 0.45; //0.33;//0.5; + public static double spindexer_intakePos2 = 0.37; //0.33;//0.5; - public static double spindexer_intakePos3 = 0.63; //0.53;//0.66; + public static double spindexer_intakePos3 = 0.55; //0.53;//0.66; - public static double spindexer_outtakeBall3 = 0.91; //0.65; //0.24; - public static double spindexer_outtakeBall3b = 0.35; //0.65; //0.24; + public static double spindexer_outtakeBall3 = 0.83; //0.65; //0.24; + public static double spindexer_outtakeBall3b = 0.27; //0.65; //0.24; - public static double spindexer_outtakeBall2 = 0.73; //0.46; //0.6; - public static double spindexer_outtakeBall1 = 0.54; //0.27; //0.4; - public static double spinStartPos = 0.05; + public static double spindexer_outtakeBall2 = 0.65; //0.46; //0.6; + public static double spindexer_outtakeBall1 = 0.46; //0.27; //0.4; + public static double spinStartPos = 0; public static double spinEndPos = 0.6; - public static double shootAllSpindexerSpeedIncrease = 0.0095; + public static double shootAllSpindexerSpeedIncrease = 0.01; public static double transferServo_out = 0.15; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java index 20f8497..55cc2fc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -148,7 +148,7 @@ class LocalizationTest extends OpMode { @Override public void init() { - follower.setStartingPose(new Pose(0,0)); + follower.setStartingPose(new Pose(72,72, 0)); } /** This initializes the PoseUpdater, the drive motors, and the Panels telemetry. */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index 8ab4d34..937758b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -2,6 +2,9 @@ package org.firstinspires.ftc.teamcode.teleop; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStart; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseH; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseX; +import static org.firstinspires.ftc.teamcode.constants.Front_Poses.teleStartPoseY; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.shootAllSpindexerSpeedIncrease; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; @@ -12,6 +15,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.roadrunner.Pose2d; import com.arcrobotics.ftclib.controller.PIDFController; +import com.pedropathing.follower.Follower; import com.pedropathing.geometry.Pose; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -21,6 +25,7 @@ import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.pedroPathing.Constants; import org.firstinspires.ftc.teamcode.utils.Drivetrain; import org.firstinspires.ftc.teamcode.utils.Flywheel; import org.firstinspires.ftc.teamcode.utils.Light; @@ -38,10 +43,11 @@ import java.util.List; public class TeleopV3 extends LinearOpMode { public static double manualVel = 3000; public static double hoodDefaultPos = 0.5; - + private double predictedResetX, predictedResetY, predictedResetH; + public static double redPredictedResetX = 9, redPredictedResetY = 10.25, redPredictedResetH = 0; + public static double bluePredictedResetX = 135.0, bluePredictedResetY = 9, bluePredictedResetH = 180; public static double spinPow = 0.09; public static double tp = 0.8, ti = 0.001, td = 0.0315, tf = 0; - public static double spinSpeedIncrease = 0.03; public static int resetSpinTicks = 0; public static double hoodSpeedOffset = 0.01; public static double turretSpeedOffset = 0.01; @@ -56,12 +62,13 @@ public class TeleopV3 extends LinearOpMode { Light light; Servos servo; Flywheel flywheel; - MecanumDrive drive; +// MecanumDrive drive; Spindexer spindexer; Targeting targeting; Targeting.Settings targetingSettings; Drivetrain drivetrain; MeasuringLoopTimes loopTimes; + Follower follower; double autoHoodOffset = 0.0; int shooterTicker = 0; boolean intake = false; @@ -84,7 +91,6 @@ public class TeleopV3 extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); - robot.light.setPosition(0); List allHubs = hardwareMap.getAll(LynxModule.class); for (LynxModule hub : allHubs) { @@ -94,12 +100,15 @@ public class TeleopV3 extends LinearOpMode { TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); servo = new Servos(hardwareMap); flywheel = new Flywheel(hardwareMap); - drive = new MecanumDrive(hardwareMap, teleStart); +// drive = new MecanumDrive(hardwareMap, teleStart); + follower = Constants.createFollower(hardwareMap); + Pose start = new Pose(teleStartPoseX, teleStartPoseY, Math.toRadians(teleStartPoseH)); + follower.setStartingPose(start); spindexer = new Spindexer(hardwareMap); targeting = new Targeting(); targetingSettings = new Targeting.Settings(0.0, 0.0); - drivetrain = new Drivetrain(robot, drive); + drivetrain = new Drivetrain(robot, follower); loopTimes = new MeasuringLoopTimes(); loopTimes.init(); @@ -118,21 +127,33 @@ public class TeleopV3 extends LinearOpMode { Spindexer.teleop = true; while (opModeInInit()) { + //ONLY FOR TESTING: COMMENT OUT FOR COMPETITIONS + if (gamepad1.crossWasPressed()){ + redAlliance = !redAlliance; + } + robot.limelight.start(); if (redAlliance) { turret.pipelineSwitch(4); light.setManualLightColor(Color.LightRed); + predictedResetX = redPredictedResetX; + predictedResetY = redPredictedResetY; + predictedResetH = Math.toRadians(redPredictedResetH); } else { turret.pipelineSwitch(2); light.setManualLightColor(Color.LightBlue); - + predictedResetX = bluePredictedResetX; + predictedResetY = bluePredictedResetY; + predictedResetH = Math.toRadians(bluePredictedResetH); } - robot.light.setPosition(1); + limelightUsed = true; + + TELE.addData("Red Alliance?", redAlliance); + TELE.update(); light.update(); } - limelightUsed = true; waitForStart(); if (isStopRequested()) return; @@ -143,8 +164,8 @@ public class TeleopV3 extends LinearOpMode { while (opModeIsActive()) { //TELE.addData("Is limelight on?", robot.limelight.getStatus()); - drive.updatePoseEstimate(); - Pose2d currentPose = drive.localizer.getPose(); + follower.update(); + Pose currentPose = follower.getPose(); if (enableSpindexerManager) { //if (!shootAll) { @@ -226,20 +247,21 @@ public class TeleopV3 extends LinearOpMode { //TURRET TRACKING - double robX = currentPose.position.x; - double robY = currentPose.position.y; - double robH = currentPose.heading.toDouble(); + double robX = currentPose.getX(); + double robY = currentPose.getY(); + double robH = currentPose.getHeading(); double robotX = robX + xOffset; double robotY = robY + yOffset; double robotHeading = robH + hOffset; - double goalX = -15; - double goalY = 0; - - double dx = robotX - goalX; // delta x from robot to goal - double dy = robotY - goalY; // delta y from robot to goal - Pose deltaPose = new Pose(dx, dy, robotHeading); +// double goalX = -15; +// double goalY = 0; +// +// double dx = robotX - goalX; // delta x from robot to goal +// double dy = robotY - goalY; // delta y from robot to goal +// Pose deltaPose = new Pose(dx, dy, robotHeading); + Pose deltaPose = new Pose(robotX, robotY, robotHeading); // double distanceToGoal = Math.sqrt(dx * dx + dy * dy); @@ -253,14 +275,15 @@ public class TeleopV3 extends LinearOpMode { gamepad2.rumble(500); } - if (relocalize){ - turret.relocalize(); - xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; - yOffset = (turret.getLimelightX() * 39.3701) - robY; - hOffset = (Math.toRadians(turret.getLimelightH())) - robH; - } else { + //TODO: relocalize using limelight +// if (relocalize){ +// turret.relocalize(); +// xOffset = -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset) - robX; +// yOffset = (turret.getLimelightX() * 39.3701) - robY; +// hOffset = (Math.toRadians(turret.getLimelightH())) - robH; +// } else { turret.trackGoal(deltaPose); - } + //} //VELOCITY AUTOMATIC if (autoVel) { @@ -324,7 +347,8 @@ public class TeleopV3 extends LinearOpMode { } if (gamepad2.crossWasPressed()) { - drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); +// drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + follower.setPose(new Pose(predictedResetX, predictedResetY, predictedResetH)); gamepad2.rumble(200); sleep(500); } @@ -381,9 +405,9 @@ public class TeleopV3 extends LinearOpMode { TELE.addData("Avg Loop Time", loopTimes.getAvgLoopTime()); TELE.addData("Min Loop Time", loopTimes.getMinLoopTimeOneMin()); TELE.addData("Max Loop Time", loopTimes.getMaxLoopTimeOneMin()); - TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); - TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); - TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); +// TELE.addData("Tag Pos X", -((turret.getLimelightZ() * 39.3701) + Turret.limelightPosOffset)); +// TELE.addData("Tag Pos Y", turret.getLimelightX() * 39.3701); +// TELE.addData("Tag Pos H", Math.toRadians(turret.getLimelightH())); TELE.update(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index b058ea9..9c4f5bf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -8,7 +8,6 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_in; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; -import static org.firstinspires.ftc.teamcode.teleop.TeleopV3.spinSpeedIncrease; import static org.firstinspires.ftc.teamcode.utils.Targeting.turretInterpolate; import com.acmerobotics.dashboard.FtcDashboard; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java index edd598a..cb5fc64 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Drivetrain.java @@ -1,33 +1,31 @@ package org.firstinspires.ftc.teamcode.utils; -import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.ProfileAccelConstraint; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TranslationalVelConstraint; -import com.acmerobotics.roadrunner.Vector2d; -import com.acmerobotics.roadrunner.ftc.Actions; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.BezierLine; +import com.pedropathing.geometry.Pose; +import com.pedropathing.paths.PathChain; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.Gamepad; - -import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; public class Drivetrain { - Robot robot; + Robot robot; boolean autoDrive = false; - Pose2d brakePos = new Pose2d(0, 0, 0); + Pose brakePos = new Pose(0, 0, 0); - MecanumDrive drive; +// MecanumDrive drive; + Follower follower; private final TranslationalVelConstraint VEL_CONSTRAINT = new TranslationalVelConstraint(200); private final ProfileAccelConstraint ACCEL_CONSTRAINT = new ProfileAccelConstraint(-Math.abs(60), 200); - public Drivetrain (Robot rob, MecanumDrive mecanumDrive){ + public Drivetrain (Robot rob, Follower follower){ this.robot = rob; - this.drive = mecanumDrive; + this.follower = follower; } @@ -71,7 +69,7 @@ public class Drivetrain { robot.frontRight.setPower(frontRightPower); robot.backRight.setPower(backRightPower); } - Pose2d currentPos = drive.localizer.getPose(); + Pose currentPos = follower.getPose(); brakePos = currentPos; if (brake > 0.4 && robot.frontLeft.getZeroPowerBehavior() != DcMotor.ZeroPowerBehavior.BRAKE && !autoDrive) { @@ -84,13 +82,13 @@ public class Drivetrain { } else if (brake > 0.4) { - TrajectoryActionBuilder traj2 = drive.actionBuilder(currentPos) - .strafeToLinearHeading(new Vector2d(brakePos.position.x, brakePos.position.y), brakePos.heading.toDouble(), VEL_CONSTRAINT, ACCEL_CONSTRAINT); + PathChain traj2 = follower.pathBuilder() + .addPath(new BezierLine(currentPos, brakePos)) + .setLinearHeadingInterpolation(currentPos.getHeading(), brakePos.getHeading()) + .build(); - if (Math.abs(currentPos.position.x - brakePos.position.x) > 1 || Math.abs(currentPos.position.y - brakePos.position.y) > 1) { - Actions.runBlocking( - traj2.build() - ); + if (Math.abs(currentPos.getX() - brakePos.getX()) > 1 || Math.abs(currentPos.getY() - brakePos.getY()) > 1) { + follower.followPath(traj2); } } else { autoDrive = false; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java index c9edac2..83521ed 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Targeting.java @@ -3,7 +3,9 @@ package org.firstinspires.ftc.teamcode.utils; import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import java.lang.Math; +import org.firstinspires.ftc.teamcode.constants.Color; import org.firstinspires.ftc.teamcode.constants.ServoPositions; public class Targeting { @@ -71,7 +73,7 @@ public class Targeting { public final int TILE_UPPER_QUARTILE = 18; public final int TILE_LOWER_QUARTILE = 6; public double robotInchesX, robotInchesY = 0.0; - public int robotGridX, robotGridY = 0; + public int robotGridX = 0, robotGridY = 0; MultipleTelemetry TELE; double cancelOffsetX = 0.0; // was -40.0 double cancelOffsetY = 0.0; // was 7.0 @@ -89,45 +91,59 @@ public class Targeting { 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; + int remX = 0; + int remY = 0; + // Old code +// 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)); +// double rotatedY = (robotX + cancelOffsetX) * sin54 + (robotY + cancelOffsetY) * cos54; +// double rotatedX = (robotX + cancelOffsetX) * cos54 - (robotY + cancelOffsetY) * sin54; +// +// // Convert robot coordinates to inches +// robotInchesX = rotatedX * unitConversionFactor; +// robotInchesY = rotatedY * unitConversionFactor; +// +// // Find approximate location in the grid +// gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); +// gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); +// } +// +// + remX = Math.floorMod((int) robotX, tileSize); + remY = Math.floorMod((int) robotY, tileSize); +// +// //clamp +// +// 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)); +// } - // 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)); + // New code + if (redAlliance){ + gridY = Math.round((float) (((144-robotX)-12) / 24)); } else { - 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; - robotInchesY = rotatedY * unitConversionFactor; - - // Find approximate location in the grid - gridX = Math.abs(Math.floorDiv((int) robotInchesX, tileSize) + 1); - gridY = Math.abs(Math.floorDiv((int) robotInchesY, tileSize)); + gridY = Math.round((float) ((robotX-12) / 24)); } + gridX = Math.round((float) (((144-robotY)-12) / 24)); - - int remX = Math.floorMod((int) robotInchesX, tileSize); - int remY = Math.floorMod((int) robotInchesY, tileSize); - - //clamp - - 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)); - } + 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. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index ceb2432..48969da 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -25,7 +25,7 @@ public class Turret { public static double turretTolerance = 0.02; public static double turrPosScalar = 0.00011264432; - public static double turret180Range = 0.54; + public static double turret180Range = 0.55; public static double turrDefault = 0.35; public static double turrMin = 0; public static double turrMax = 0.69; @@ -262,7 +262,12 @@ public class Turret { double desiredTurretAngleDeg = Math.toDegrees(Math.atan2(posY, posX)) - 45; // Robot heading (field → robot) - double robotHeadingDeg = posH + 135; + double robotHeadingDeg; + if (Color.redAlliance){ + robotHeadingDeg = posH + 135; + } else { + robotHeadingDeg = posH + 45; + } // Turret angle needed relative to robot double turretAngleDeg = desiredTurretAngleDeg - robotHeadingDeg; @@ -362,16 +367,16 @@ public class Turret { /* ---------------- TELEMETRY ---------------- */ - TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); - TELE.addData("Target Pos", "%.3f", targetTurretPos); - TELE.addData("Current Localization Pos", "%.3f", deltaPos); - TELE.addData("Commanded Pos", "%.3f", turretPos); +// TELE.addData("Turret Angle (deg)", "%.2f", turretAngleDeg); +// TELE.addData("Target Pos", "%.3f", targetTurretPos); +// TELE.addData("Current Localization Pos", deltaPos); +// TELE.addData("Commanded Pos", "%.3f", turretPos); // TELE.addData("LL Valid", result.isValid()); // TELE.addData("LL getTx", result.getTx()); // TELE.addData("LL Offset", offset); // TELE.addData("Bearing Error", hasValidTarget ? String.format("%.2f", tagBearingDeg) : "NO TARGET"); // TELE.addData("Learned Offset", "%.2f", offset); - TELE.update(); +// TELE.update(); } }