You can try this PID controller as an example or program one by yourself:

Kp = 0.08;
Ti = 0.6;
Td = 0; // No derivative term; i.e. a PI, not a PID
N = 3; // Within the interval [3,20]

e = (Refth - th);
ew = (Refw - w);
if (Ti == 0){
uC[2] = 0;
}else{
uC[2] = uC[2] + Kp*dt/Ti * e;
}
if (Td == 0){
uC[3] = 0;
}else{
uC[3] = Td/(Td+N*dt)*uC[3] - Kp*Td*N/(Td+N*dt)*(th - uC[4]);
}
uC[4] = th;

if (uC[2] < -10) uC[2] = -10; //The auxiliar variables may need
if (uC[3] < -10) uC[3] = -10; //additional control rules in the code.
if (uC[2] > 10) uC[2] = 10;
if (uC[3] > 10) uC[3] = 10;


uC[0]= Kp*e + uC[2] + uC[3];
uC[1] =0;