public void updateAcel() {
   double timestamp = Timer.getFPGATimestamp();
   double deltaTime = timestamp - lastTimestamp;
   acceleration = new Vector2(xFilter.pidGet(), yFilter.pidGet());
   velocity =
       Vector2.add(
           velocity, Vector2.rotate(Vector2.scale(acceleration, deltaTime), -getHeading()));
   position = Vector2.add(position, Vector2.scale(velocity, deltaTime));
   lastTimestamp = timestamp;
 }
  public void run() {
    SmartDashboard.putString("Status", "Started");
    double lastTime = Timer.getFPGATimestamp();
    double time = lastTime;
    double deltaTime;

    while (!Thread.interrupted()) {
      time = Timer.getFPGATimestamp();
      deltaTime = time - lastTime;
      acceleration = new Vector2(xFilter.pidGet(), yFilter.pidGet());
      velocity = Vector2.add(velocity, Vector2.scale(acceleration, deltaTime));
      position =
          Vector2.add(position, Vector2.rotate(Vector2.scale(velocity, deltaTime), -getHeading()));

      lastTime = time;
      try {
        Thread.sleep(20);
      } catch (InterruptedException e) {
        break;
      }
    }
  }