 {
   handleCommands();
 
+  // swingUp(); // Dangerous and exciting
+
   // Compute PID output.
   cartPid.update(float(trackEncoder.read())/xMax);
 

#define DIR_PIN 12
#define CART_RESET_PIN 21

elapsedMillis riseTimer = 0;
bool riseTimerStarted = false;
float deltaSetpoint = 0;

int xPrev = 0;
int xCart = 0;
int xMax = 0;

int thetaPrev = 0;
int theta = 0;

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

PIDControl cartPid(kp, ki, kd, 0, 10); // p,i,d, initial setpoint, timestep [ms]
elapsedMillis printTimer = 0;
unsigned int printerval = 50; // ms

void swingUp()
{
  thetaPrev = theta;
  theta = thetaEncoder.read();
  float amplitude = theta > 0 ? 0.5 : -0.5;
  cartPid.setpoint = amplitude*cos(theta/300*pi);
}

// Command callbacks for use by SimpleShell
ExecStatus setPID(CommandLine *cl)
