Program: 39003
Project: 39003
Buggy source:
public void update(){
  invalidateHeading();
  if (positionEstimationEnabled) {
    double[] rotations=getMotorRotations();
    double heading=getHeading();
    if (lastRotations != null) {
      double[] rotationDeltas=new double[4];
      for (int i=0; i < 4; i++) {
        rotationDeltas[i]=rotations[i] - lastRotations[i];
      }
      Vector2d robotPoseDelta=MecanumDrive.getPoseDelta(rotationDeltas).pos();
      Vector2d fieldPoseDelta=robotPoseDelta.rotated(heading);
      estimatedPosition=estimatedPosition.added(fieldPoseDelta);
    }
    lastRotations=rotations;
  }
  if (maintainHeading) {
    double heading=getHeading();
    double headingError=maintainHeadingController.getError(heading);
    double headingUpdate=0;
    if (Math.abs(targetOmega) > 0) {
      maintainHeadingController.setSetpoint(heading);
      internalSetVelocity(targetVel,targetOmega);
    }
 else {
      headingUpdate=maintainHeadingController.update(headingError);
      internalSetVelocity(targetVel,headingUpdate);
    }
  }
 else {
    internalSetVelocity(targetVel,targetOmega);
  }
switch (mode) {
case OPEN_LOOP:
    powers=targetPowers;
  break;
case OPEN_LOOP_RAMP:
double[] powerDeltas=new double[4];
double maxDesiredAbsPowerDelta=0;
for (int i=0; i < 4; i++) {
powerDeltas[i]=(targetPowers[i] - powers[i]);
double desiredAbsPowerDelta=Math.abs(powerDeltas[i]);
if (desiredAbsPowerDelta > maxDesiredAbsPowerDelta) {
maxDesiredAbsPowerDelta=desiredAbsPowerDelta;
}
}
double timestamp=TimestampedData.getCurrentTime();
double dt=timestamp - lastTimestamp;
lastTimestamp=timestamp;
double maxAbsPowerDelta=DriveConstants.RAMP_MAX_ACCEL * (dt / 1000.0);
double multiplier;
if (maxDesiredAbsPowerDelta > maxAbsPowerDelta) {
multiplier=maxAbsPowerDelta / maxDesiredAbsPowerDelta;
}
 else {
multiplier=1;
}
for (int i=0; i < 4; i++) {
powers[i]+=powerDeltas[i] * multiplier;
}
break;
case FOLLOW_PATH:
if (pathFollower.isFollowingPath()) {
Pose2d estimatedPose=getEstimatedPose();
Pose2d update=pathFollower.update(estimatedPose);
internalSetVelocity(update.pos(),update.heading());
}
 else {
stop();
revertMode();
}
powers=targetPowers;
break;
}
for (int i=0; i < 4; i++) {
if (lastPowers[i] != powers[i]) {
motors[i].setPower(powers[i]);
lastPowers[i]=powers[i];
}
}
}

Fixed version (ground truth):
public void update(){
  invalidateHeading();
  telemetryMap.put("String_Node_Str",mode);
  if (positionEstimationEnabled || mode == Mode.FOLLOW_PATH || maintainHeading) {
    double heading=getHeading();
    if (positionEstimationEnabled || mode == Mode.FOLLOW_PATH) {
      double[] rotations=getMotorRotations();
      if (lastRotations != null) {
        double[] rotationDeltas=new double[4];
        for (int i=0; i < 4; i++) {
          rotationDeltas[i]=rotations[i] - lastRotations[i];
        }
        Vector2d robotPoseDelta=MecanumDrive.getPoseDelta(rotationDeltas).pos();
        Vector2d fieldPoseDelta=robotPoseDelta.rotated(heading);
        estimatedPosition=estimatedPosition.added(fieldPoseDelta);
      }
      lastRotations=rotations;
      for (int i=0; i < 4; i++) {
        telemetryMap.put(MOTOR_NAMES[i] + "String_Node_Str",rotations[i]);
      }
      telemetryMap.put("String_Node_Str",getHeading());
    }
    if (maintainHeading) {
      double headingError=maintainHeadingController.getError(heading);
      double headingUpdate=0;
      if (Math.abs(targetOmega) > 0) {
        maintainHeadingController.setSetpoint(heading);
        internalSetVelocity(targetVel,targetOmega);
      }
 else {
        headingUpdate=maintainHeadingController.update(headingError);
        internalSetVelocity(targetVel,headingUpdate);
      }
      telemetryMap.put("String_Node_Str",headingError);
      telemetryMap.put("String_Node_Str",headingUpdate);
    }
 else {
      internalSetVelocity(targetVel,targetOmega);
    }
  }
switch (mode) {
case OPEN_LOOP:
    powers=targetPowers;
  break;
case OPEN_LOOP_RAMP:
double[] powerDeltas=new double[4];
double maxDesiredAbsPowerDelta=0;
for (int i=0; i < 4; i++) {
powerDeltas[i]=(targetPowers[i] - powers[i]);
double desiredAbsPowerDelta=Math.abs(powerDeltas[i]);
if (desiredAbsPowerDelta > maxDesiredAbsPowerDelta) {
maxDesiredAbsPowerDelta=desiredAbsPowerDelta;
}
}
double timestamp=TimestampedData.getCurrentTime();
double dt=timestamp - lastTimestamp;
lastTimestamp=timestamp;
double maxAbsPowerDelta=DriveConstants.RAMP_MAX_ACCEL * (dt / 1000.0);
double multiplier;
if (maxDesiredAbsPowerDelta > maxAbsPowerDelta) {
multiplier=maxAbsPowerDelta / maxDesiredAbsPowerDelta;
}
 else {
multiplier=1;
}
for (int i=0; i < 4; i++) {
powers[i]+=powerDeltas[i] * multiplier;
}
break;
case FOLLOW_PATH:
if (pathFollower.isFollowingPath()) {
Pose2d estimatedPose=getEstimatedPose();
Pose2d update=pathFollower.update(estimatedPose);
internalSetVelocity(update.pos(),update.heading());
telemetryMap.put("String_Node_Str",pathFollower.getAxialError());
telemetryMap.put("String_Node_Str",pathFollower.getAxialUpdate());
telemetryMap.put("String_Node_Str",pathFollower.getLateralError());
telemetryMap.put("String_Node_Str",pathFollower.getLateralUpdate());
telemetryMap.put("String_Node_Str",pathFollower.getHeadingError());
telemetryMap.put("String_Node_Str",pathFollower.getHeadingUpdate());
fieldOverlay.setStroke("String_Node_Str");
DrawingUtil.drawMecanumRobot(fieldOverlay,pathFollower.getPose());
}
 else {
stop();
revertMode();
}
powers=targetPowers;
break;
}
for (int i=0; i < 4; i++) {
if (lastPowers[i] != powers[i]) {
motors[i].setPower(powers[i]);
lastPowers[i]=powers[i];
}
telemetryMap.put(MOTOR_NAMES[i] + "String_Node_Str",powers[i]);
}
fieldOverlay.setStroke("String_Node_Str");
DrawingUtil.drawMecanumRobot(fieldOverlay,getEstimatedPose());
for (Map.Entry<String,Object> entry : telemetryMap.entrySet()) {
telemetry.addData(entry.getKey(),entry.getValue());
}
}