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

//PID Control for Sensor 1

Kp = 0.1;
Ti =2;
Td = 0;
N = 3;

e = (setPoint1 - temp1);

if (Ti == 0) {
u[3] = 0;
} else {
u[3] = u[3] + Kp*dt/Ti * e;
}
if (Td == 0) {
u[4] = 0;
} else {
u[4] = Td/(Td+N*dt)*u[4] - Kp*Td*N/(Td+N*dt)*(temp1 - u[5]);
}
u[5] = temp1;

if (u[3] < 0) u[3] = 0;
if (u[4] < 0) u[4] = 0;
if (u[2] > 5) u[2] = 5;
if (u[3] > 5) u[3] = 5;

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