content: package org.firstinspires.ftc.teamcode.tuning;
  
  import com.acmerobotics.dashboard.FtcDashboard;
  import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
  import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
  import com.acmerobotics.roadrunner.Pose2d;
  import com.acmerobotics.roadrunner.PoseVelocity2d;
  import com.acmerobotics.roadrunner.Vector2d;
  import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
  
  import org.firstinspires.ftc.teamcode.Drawing;
  import org.firstinspires.ftc.teamcode.MecanumDrive;
  import org.firstinspires.ftc.teamcode.TankDrive;
  
  public class LocalizationTest extends LinearOpMode {
      @Override
      public void runOpMode() throws InterruptedException {
          telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
  
          if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) {
              MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0));
  
              waitForStart();
  
              while (opModeIsActive()) {
                  drive.setDrivePowers(new PoseVelocity2d(
                          new Vector2d(
                                  -gamepad1.left_stick_y,
                                  -gamepad1.left_stick_x
                          ),
                          -gamepad1.right_stick_x
                  ));
  
                  drive.updatePoseEstimate();
  
                  Pose2d pose = drive.localizer.getPose();
                  telemetry.addData("x", pose.position.x);
                  telemetry.addData("y", pose.position.y);
                  telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
                  telemetry.update();
  
                  TelemetryPacket packet = new TelemetryPacket();
                  packet.fieldOverlay().setStroke("#3F51B5");
                  Drawing.drawRobot(packet.fieldOverlay(), pose);
                  FtcDashboard.getInstance().sendTelemetryPacket(packet);
              }
          } else if (TuningOpModes.DRIVE_CLASS.equals(TankDrive.class)) {
              TankDrive drive = new TankDrive(hardwareMap, new Pose2d(0, 0, 0));
  
              waitForStart();
  
              while (opModeIsActive()) {
                  drive.setDrivePowers(new PoseVelocity2d(
                          new Vector2d(
                                  -gamepad1.left_stick_y,
                                  0.0
                          ),
                          -gamepad1.right_stick_x
                  ));
  
                  drive.updatePoseEstimate();
  
                  Pose2d pose = drive.localizer.getPose();
                  telemetry.addData("x", pose.position.x);
                  telemetry.addData("y", pose.position.y);
                  telemetry.addData("heading (deg)", Math.toDegrees(pose.heading.toDouble()));
                  telemetry.update();
  
                  TelemetryPacket packet = new TelemetryPacket();
                  packet.fieldOverlay().setStroke("#3F51B5");
                  Drawing.drawRobot(packet.fieldOverlay(), pose);
                  FtcDashboard.getInstance().sendTelemetryPacket(packet);
              }
          } else {
              throw new RuntimeException();
          }
      }
  }
  