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

Kp = 0.2;
Ti = 5;
Td = 0; //0 No derivative action; i.e. PI by default
N = 3; //Within the interval [3,20]

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

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


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

if (u[0] < -10) u[0] = -10; //The control variables may need
if (u[1] < -10) u[1] = -10; //additional control rules in the code.
if (u[0] > 10) u[0] = 10;
if (u[1] > 10) u[1] = 10;