   // Update pendulum state
   pendulum.update(trackEncoder.read(), thetaEncoder.read(), millis());
 
-  // If pendulum is not inverted, generate setpoint for swinging action
-  cartPid.setpoint = pendulum.swingX(0.4);
-
   // Compute PID output for x position
   cartPid.update(pendulum.x, millis());
 
+  penPid.update(pendulum.theta, millis());
+
+#ifdef DEBUG
   // Report rise time
   if (riseTimerStarted && deltaSetpoint != 0)
   {

Encoder trackEncoder(TRACK_ENCODER_PIN_A, TRACK_ENCODER_PIN_B);
Encoder thetaEncoder(PENDULUM_ENCODER_PIN_A, PENDULUM_ENCODER_PIN_B);

// Initialize with encoder period and swing-up limit
Pendulum pendulum(7200 /* counts/rev */, 120.0 /* deg */);

// PID control for cart position
float kp = 2, ki = 0.0005, kd = 0; // Initial/default PID gain coeffs
PIDControl cartPid(kp, ki, kd, 0, 5); // p,i,d, initial setpoint, timestep [ms]
elapsedMillis printTimer = 0;
unsigned int printerval = 50; // ms


#define DIR_PIN 12
#define CART_RESET_PIN 21

// #define DEBUG 1 // Uncomment to print info for study/debugging

#ifdef DEBUG
elapsedMillis riseTimer = 0;
bool riseTimerStarted = false;
float deltaSetpoint = 0;
#endif

float maxPwm = 16383;  // 100% PWM value with 14-bit resolution
float pwmFreq = 2929.687; // See http://www.pjrc.com/teensy/td_pulse.html
