Честно говоря не смотрел сильно вглубь программы но на первый взгляд вроде бы правильно. Проверка на диапазон - если измеряете температуру от +20 до +120 градусов - нет смысла вызывать регулятор при значении -1.0E31. Аналогично по коэффициентам пид. НАН получается при делении на 0 или извлечении корня из -1 и т.д.(переполнение плавающего) . Например, вы вызвали регулятор при к-те ti=0 в интеграле нан - и на выходе нан.