Si lo que quieres implementar en un PID, hay librerías que lo hacen. Sería mas conveniente que plantearlo como lo haces.
bien por lo que veo esas planteando un lazo de control. Tienes la planta identificada y todo lo demás pero lo que se suele hacer es
Aca tienes un ejemplo usando la librería PID_v1.h y aca el link.
#include <PID_v1.h>
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
//Specify the links and initial tuning parameters
PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT);
void setup()
{
//initialize the variables we're linked to
Input = analogRead(0);
Setpoint = 100;
//turn the PID on
myPID.SetMode(AUTOMATIC);
}
void loop()
{
Input = analogRead(0);
myPID.Compute();
analogWrite(3,Output);
}
S