 
 void printStatus(PIDControl &pid)
 {
-  String s = String("kp,ki,kd: ") + kp + ", " + ki + ", " + kd
-             + String("; p,i,d: ") + pid.kp + ", " + pid.ki + ", " + pid.kd
-             + String(" setpoint: ") + pid.setpoint
-             + String(" output: ") + pid.output
-             + String(" pwm: ") + dutyCycle;
+  String s = String("{\"pid\":[") + pid.kp + "," + pid.ki + "," + pid.kd + "]"
+             + String(", \"setpoint\":") + (100*pid.setpoint)
+             + String(", \"output\":") + pid.output
+             + String(", \"pwm\":") + dutyCycle
+             + String(", \"x\":") + (100*float(trackEncoder.read())/xMax)
+             + String(", \"theta\":") + thetaEncoder.read()
+             + String(", \"time\":") + millis()
+             + "}";
   Serial.println(s);
 }
 

#define DIR_PIN 12
#define CART_RESET_PIN 21

float prevSetpoint = 0; // temp - debug

// For incoming serial data
int incomingByte = 0;


  if (pidTimer >= timeStep)
  {
    // if (prevSetpoint != cartPid.setpoint) { printStatus(cartPid); }
    pidTimer -= timeStep;
    float input = float(trackEncoder.read())/xMax;
    cartPid.update(input, 0.001);
    digitalWrite(DIR_PIN, cartPid.output < 0 ? LEFT : RIGHT);
    analogWrite(PWM_PIN, (fabs(cartPid.output) + 0.1)*maxPwm);
    // prevSetpoint = cartPid.setpoint;

    printStatus(cartPid);
  }
}

