Program: 39011
Project: 39011
Buggy source:
@Override public void runOpMode(){
  dashboard=RobotDashboard.getInstance();
  telemetry=new MultipleTelemetry(telemetry,dashboard.getTelemetry());
  drive=new MecanumDrive(hardwareMap);
  drive.enablePositionEstimation();
  File logRoot=new File(LoggingUtil.getLogRoot(this),"String_Node_Str" + System.currentTimeMillis());
  logRoot.mkdirs();
  final CSVLoggingTelemetry axialLog=new CSVLoggingTelemetry(new File(logRoot,"String_Node_Str"));
  final CSVLoggingTelemetry lateralLog=new CSVLoggingTelemetry(new File(logRoot,"String_Node_Str"));
  final CSVLoggingTelemetry headingLog=new CSVLoggingTelemetry(new File(logRoot,"String_Node_Str"));
  state=State.TUNE_LATERAL;
  lastPose=new Pose2d(0,0,0);
  waitForStart();
  startTimestamp=TimestampedData.getCurrentTime();
  while (opModeIsActive() && state != State.DONE) {
    telemetry.addData("String_Node_Str",state);
    double elapsedTime=TimestampedData.getCurrentTime() - startTimestamp;
    double dt=elapsedTime - lastTime;
    lastTime=elapsedTime;
    Pose2d estimatedPose=drive.getEstimatedPose();
switch (state) {
case TUNE_AXIAL:
{
        if (Math.abs(estimatedPose.x()) > AXIAL_DISTANCE) {
          drive.stop();
          reset();
          state=State.TUNE_HEADING;
        }
 else {
          double power=AXIAL_POWER_INCREMENT * elapsedTime;
          double axialDelta=estimatedPose.x() - lastPose.x();
          double axialSpeed=axialDelta / dt;
          drive.setVelocity(new Vector2d(power,0),0);
          axialLog.addData("String_Node_Str",power);
          axialLog.addData("String_Node_Str",axialSpeed);
          axialLog.update();
        }
        break;
      }
case TUNE_LATERAL:
{
      if (Math.abs(estimatedPose.y()) > LATERAL_DISTANCE) {
        drive.stop();
        reset();
        state=State.TUNE_AXIAL;
      }
 else {
        double power=LATERAL_POWER_INCREMENT * elapsedTime;
        double lateralDelta=estimatedPose.y() - lastPose.y();
        double lateralSpeed=lateralDelta / dt;
        drive.setVelocity(new Vector2d(0,power),0);
        lateralLog.addData("String_Node_Str",power);
        lateralLog.addData("String_Node_Str",lateralSpeed);
        lateralLog.update();
      }
      break;
    }
case TUNE_HEADING:
{
    double power=HEADING_POWER_INCREMENT * elapsedTime;
    if (Math.abs(power) > MAX_HEADING_POWER) {
      drive.stop();
      reset();
      state=State.DONE;
    }
 else {
      double headingDelta=estimatedPose.heading() - lastPose.heading();
      if (Math.abs(headingDelta) > Math.PI / 2) {
        headingDelta-=Math.signum(headingDelta) * 2 * Math.PI;
      }
      double omega=headingDelta / dt;
      drive.setVelocity(new Vector2d(0,0),power);
      headingLog.addData("String_Node_Str",power);
      headingLog.addData("String_Node_Str",omega);
      headingLog.addData("String_Node_Str",estimatedPose.heading());
      headingLog.update();
    }
    break;
  }
}
lastPose=estimatedPose;
drive.update();
telemetry.update();
}
}

Fixed version (ground truth):
@Override public void runOpMode(){
  dashboard=RobotDashboard.getInstance();
  telemetry=new MultipleTelemetry(telemetry,dashboard.getTelemetry());
  drive=new MecanumDrive(hardwareMap,telemetry);
  drive.enablePositionEstimation();
  File logRoot=new File(LoggingUtil.getLogRoot(this),"String_Node_Str" + System.currentTimeMillis());
  logRoot.mkdirs();
  final CSVLoggingTelemetry axialLog=new CSVLoggingTelemetry(new File(logRoot,"String_Node_Str"));
  final CSVLoggingTelemetry lateralLog=new CSVLoggingTelemetry(new File(logRoot,"String_Node_Str"));
  final CSVLoggingTelemetry headingLog=new CSVLoggingTelemetry(new File(logRoot,"String_Node_Str"));
  state=State.TUNE_LATERAL;
  lastPose=new Pose2d(0,0,0);
  waitForStart();
  startTimestamp=TimestampedData.getCurrentTime();
  while (opModeIsActive() && state != State.DONE) {
    telemetry.addData("String_Node_Str",state);
    double elapsedTime=TimestampedData.getCurrentTime() - startTimestamp;
    double dt=elapsedTime - lastTime;
    lastTime=elapsedTime;
    Pose2d estimatedPose=drive.getEstimatedPose();
switch (state) {
case TUNE_AXIAL:
{
        if (Math.abs(estimatedPose.x()) > AXIAL_DISTANCE) {
          drive.stop();
          reset();
          state=State.TUNE_HEADING;
        }
 else {
          double power=AXIAL_POWER_INCREMENT * elapsedTime;
          double axialDelta=estimatedPose.x() - lastPose.x();
          double axialSpeed=axialDelta / dt;
          drive.setVelocity(new Vector2d(power,0),0);
          axialLog.addData("String_Node_Str",power);
          axialLog.addData("String_Node_Str",axialSpeed);
          axialLog.update();
        }
        break;
      }
case TUNE_LATERAL:
{
      if (Math.abs(estimatedPose.y()) > LATERAL_DISTANCE) {
        drive.stop();
        reset();
        state=State.TUNE_AXIAL;
      }
 else {
        double power=LATERAL_POWER_INCREMENT * elapsedTime;
        double lateralDelta=estimatedPose.y() - lastPose.y();
        double lateralSpeed=lateralDelta / dt;
        drive.setVelocity(new Vector2d(0,power),0);
        lateralLog.addData("String_Node_Str",power);
        lateralLog.addData("String_Node_Str",lateralSpeed);
        lateralLog.update();
      }
      break;
    }
case TUNE_HEADING:
{
    double power=HEADING_POWER_INCREMENT * elapsedTime;
    if (Math.abs(power) > MAX_HEADING_POWER) {
      drive.stop();
      reset();
      state=State.DONE;
    }
 else {
      double headingDelta=estimatedPose.heading() - lastPose.heading();
      if (Math.abs(headingDelta) > Math.PI / 2) {
        headingDelta-=Math.signum(headingDelta) * 2 * Math.PI;
      }
      double omega=headingDelta / dt;
      drive.setVelocity(new Vector2d(0,0),power);
      headingLog.addData("String_Node_Str",power);
      headingLog.addData("String_Node_Str",omega);
      headingLog.addData("String_Node_Str",estimatedPose.heading());
      headingLog.update();
    }
    break;
  }
}
lastPose=estimatedPose;
drive.update();
telemetry.update();
}
}