 
   if (pidTimer >= timeStep)
   {
-    if (prevSetpoint != cartPid.setpoint) { printStatus(cartPid); }
+    // 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;
+    // prevSetpoint = cartPid.setpoint;
+
+    printStatus(cartPid);
   }
 }
 


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;
  Serial.println(s);
}



// PID control for cart position
float kp = 2, ki = 10, kd = 0; // Initial/default PID gain coeffs
unsigned long timeStep = 20; // ms
PIDControl cartPid(kp, ki, kd, 0, timeStep);
elapsedMillis pidTimer = 0;

