   // Set motor direction
   digitalWrite(DIR_PIN, cartPid.output < 0 ? LEFT : RIGHT);
 
-  // Set motor PWM value. Include a deadband to suppress jitter
-  float op = fabs(cartPid.output);
-  float pwmSetting = op < 0.01 ? 0 : (op + 0.1)*maxPwm;
-  analogWrite(PWM_PIN, pwmSetting);
+  // Set motor PWM value. Include a 1% output deadband to suppress jitter
+  float percent_output = 100*fabs(cartPid.output);
+  dutyCycle = percent_output < 1 ? 0 : (percent_output + 10);
+  dutyCycle = cartPid.clamped(dutyCycle, 0, 100); // Ensure within 0-100%
+  analogWrite(PWM_PIN, dutyCycle/100*maxPwm);
 
   if (printTimer >= printerval)
   {
     printTimer -= printerval;
-    printStatus(cartPid);
+    printStatus(cartPid, pendulum);
   }
 }
 

Encoder trackEncoder(TRACK_ENCODER_PIN_A, TRACK_ENCODER_PIN_B);
Encoder thetaEncoder(PENDULUM_ENCODER_PIN_A, PENDULUM_ENCODER_PIN_B);

// Initialize with ticks in pendulum encoder wheel, and limit in degrees for swing-up
Pendulum pendulum(1200, 100.0);

// PID control for cart position
float kp = 2, ki = 0.2, kd = 0; // Initial/default PID gain coeffs
PIDControl cartPid(kp, ki, kd, 0, 10); // p,i,d, initial setpoint, timestep [ms]
elapsedMillis printTimer = 0;
unsigned int printerval = 50; // ms

  pendulum.update(trackEncoder.read(), thetaEncoder.read());

  // If pendulum is not inverted, generate setpoint for swinging action
  // cartPid.setpoint = pendulum.swingX(0.5);

  // Compute PID output for x position
  cartPid.update(pendulum.x, millis());
