   Serial.println(pendulum.xEncMax);
 
   resetTimer = 0;
-  pendulum.update(trackEncoder.read(), thetaEncoder.read());
+  pendulum.update(trackEncoder.read(), thetaEncoder.read(), millis());
   while (abs(pendulum.xEnc) > 2)
   {
     if (resetTimer > maxTime)

  handleCommands();

  // Update pendulum state
  pendulum.update(trackEncoder.read(), thetaEncoder.read());

  // If pendulum is not inverted, generate setpoint for swinging action
  // cartPid.setpoint = pendulum.swingX(0.5);

    digitalWrite(DIR_PIN, pendulum.x > 0 ? LEFT : RIGHT);
    analogWrite(PWM_PIN, (pendulum.x + 0.15)*maxPwm);
    delay(10);
    pendulum.update(trackEncoder.read(), thetaEncoder.read(), millis());
  }

  analogWrite(PWM_PIN, 0);
