Variables used from your control code
x3kmod = theta2 mod PI
u = control effort
x4k = angular veloocity of theta2
setpoint = balance position for theta1
theta1 = enc1's angle value
x1k = theta1 - setpoint




...... your global variables ........

// new global variables to add
int swingup = 1;
int catchit =0;

....... your main ...........

void <yourClockfuncname>(void) {

	...... your control code developed so far all code before out_PWM ..........

	if (!catchit) { // Don't check if we can switch to balancing control if balancing control is already active 
		
		// x3kmod is within 0.3 radians of the balacing point and
		// the control effort to balance the links there is not too large then
		// switch to the balancing controller
		
        if (fabs(x3kmod) < .30) {
                
            // you should have calculated u above;
            if (fabs(u) < 50.0) { // if balancing control check is not too large 
                swingup = 0;          // switch from swing up control to balance 
                catchit = 1;
            
            }// endif 
        } // endif 
		
	} // endif 

	if (swingup) { 
		
		if (x4k*cos(x3kmod) > 0) {
			
				u = ????+/-????? constant less than 4;
			
		} else {
			
				u = ????-/+????? same constant less than 4;
			
		}
		

		setpoint=theta1;  // keep setting the theta1 balance point to the current location of theta1 when swinging up
				
	} else { // balancing controller 

		if ( fabs(u) > 60.0 ) { // if control gets too large or
																 //	link angle is a large distance
																 // from equilibrium,  switch back to 
																 // partial feedback linearization controller 
			u = 0.0; // set control effort to zero just for this sample rate since balancing control effort is soo high 
			swingup = 1;
			catchit = 0;

		} // endif 
	} // endif

...... the rest of your code starting at out_PWM ..........