Designing an Embedded PI Controller for Speed Control
It's a very basic PI controller with lot of issues. You should make an incremental type also known as velocity algorithm (Astrom et al.), you can use Simpson formula for integrator or similar trapezoidal numerical integration. With velocity algorithm it is not allowed to use it without the integrator, but anti-windup is quite easy to implement. 

Other way is to construct an Astrom controller, with separate components. Then you can turn off the integrator, but you'll need a integral tracking control for anti-windup which is CPU demanding as it uses sqrt() function.

You'd better read at least some articles, strating with Astrom: Astriom 

Due to lot of undervotes in the past I won't publish PID algorithms, at least until you won't read out some basics. 

T0...sample time
Ti....integration time
uk...output
ek....error = setpoint - process value


//at init or parameter change compute q0, q1 
q0 = KP*(1 + T0/2*Ti);
q1 = −KP*(1 − T0/2Ti);

//every sample time ISR compute output
ek = SP-PV;
uk = q0*ek + q1*ek_1 + uk_1;
if uk &gt; max_value 
    uk = max_value;
elsif uk &lt; min_value 
    uk = min_value;
uk_1 =uk;
ek_1 = ek;