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

{
  handleCommands();

  // Update pendulum state and compute PID output
  pendulum.setX(trackEncoder.read());
  // TODO: read theta here; set pendulum theta in degrees
  if (fabs(pendulum.theta) < pendulum.swingThetaMax)
  {
    cartPid.setpoint = pendulum.swingX(0.5);
  }
  cartPid.update(pendulum.x, millis());

  // Report rise time

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

  analogWrite(PWM_PIN, 0);
