       riseTimerStarted = false;
     }
   }
-
-  // Set motor direction
-  digitalWrite(DIR_PIN, cartPid.output < 0 ? LEFT : RIGHT);
-
-  // 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;
-  // dutyCycle = percent_output < 1 ? 0 : (percent_output + 10);
-  dutyCycle = cartPid.clamped(dutyCycle, 0, 100); // Ensure within 0-100%
-  analogWrite(PWM_PIN, dutyCycle/100*maxPwm);
+#endif
 
   if (printTimer >= printerval)
   {
     printTimer -= printerval;
     printStatus(cartPid, pendulum);
   }
+
+  // Inverted pendulum control actuation is active inside 180 +/- 18 degrees
+  if (fabs(penPid.setpoint - penPid.prevInput) < 18 && fabs(pendulum.omega) < 1)
+  {
+    // Set motor direction
+    digitalWrite(DIR_PIN, penPid.output > 0 ? LEFT : RIGHT);
+
+    // Set motor PWM value. Include a 1% output deadband to suppress jitter
+    float percent_output = 100*fabs(penPid.output);
+    dutyCycle = percent_output < 1 ? 0 : percent_output;
+    dutyCycle = penPid.clamped(dutyCycle, 0, 100); // Ensure within 0-100%
+    analogWrite(PWM_PIN, dutyCycle/100*maxPwm);
+  }
+  else if (fabs(pendulum.omega) < 2)
+  {
+    // If pendulum is not inverted, generate setpoint for swinging action
+    cartPid.setpoint = pendulum.swingX(0.4);
+
+    // Set motor direction
+    digitalWrite(DIR_PIN, cartPid.output < 0 ? LEFT : RIGHT);
+
+    // 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;
+    dutyCycle = cartPid.clamped(dutyCycle, 0, 100); // Ensure within 0-100%
+    analogWrite(PWM_PIN, dutyCycle/100*maxPwm);
+  }
+
 }
 
 // Bump switch ISRs

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

// Initialize with encoder period and swing-up limit
Pendulum pendulum(7200 /* counts/rev */, 120.0 /* deg */);

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


#define DIR_PIN 12
#define CART_RESET_PIN 21

// #define DEBUG 1 // Uncomment to print info for study/debugging

#ifdef DEBUG
elapsedMillis riseTimer = 0;
bool riseTimerStarted = false;
float deltaSetpoint = 0;
#endif

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