 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)

{
  handleCommands();

  // Compute PID output.
  cartPid.update(float(trackEncoder.read())/xMax);


#define DIR_PIN 12
#define CART_RESET_PIN 21

const float pi = 3.14159265358979;

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


// TODO: make a Pendulum struct or class instead of all these state globals
int xPrev = 0;
int xCart = 0;
int xMax = 0;

float theta = 0;
float thetaPrev = 0;
float omega = 0; // angular velocity

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