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; // 0 No derivative term; i.e. PI
N = 3; // Interval [3,20]

e = (setPoint1 - linear_T1);

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)*(linear_T1 - u[5]);
}
u[5] = linear_T1;

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

u[0]= Kp*e + u[3] + u[4]; //Sensor 1 Control Signal
u[1] = 0; //Sensor 2 Control Signal
u[2] = 0; //Sensor 3 Control Signal