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

#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);
  }
}

