Проблема с кодом PID, переменная error_x ведет себя странно

Я пытаюсь заставить error_x и error_y отображать разницу между целью и позицией. По какой-то причине я не могу заставить error_x работать правильно. error_y имеет тот же код, но работает корректно. Кто-нибудь может помочь?

Вот код на моем Arduino:

void setup() {
  Serial.begin(9600);
  Serial.print("Starting...\n");
}

int previousTime = 0;
float lastError_x = 0;
float lastError_y = 0;

void loop() {
  //переменные pid
  float Kp = 1;
  float Ki = 0;
  float Kd = 0;
  float pos_x;
  float pos_y;
  float target_x;
  float target_y;
  float error_x;
  float error_y;
  int currentTime;
  int elapsedTime;
  float cumError_x;
  float cumError_y;
  float rateError_x;
  float rateError_y;
  float output_x;
  float output_y;

  //включаем кинематические переменные
  float inVec[4] = {1, -1.5, 3};
  float height = 3.5;
  float B = 4;
  float L = 3;
  float U = 3.5;
  float plateRadius = 5.5;
  float alpha[4] = {0, 0, 0};
  float normVec[4];
  float p1[4] = {0,0,0};
  float p2[4] = {0,0,0};
  float p3[4] = {0,0,0};
  float crosskv[4] = {0,0,0};
  float normSave;
  float dotSave;
  float radius;
  float gamma;
  float beta;

  //последовательные переменные
  char buf[32];
  char str_temp1[6];
  char str_temp2[6];
  char str_temp3[6];


  //КОД ПИКСИЛЬНОЙ КАМЕРЫ (выводит x и y)

  pos_x = 1;
  pos_y = 1;

  // ОПРЕДЕЛЕНИЕ ЦЕЛЕВОГО МЕСТОПОЛОЖЕНИЯ
  //по умолчанию:(0,0)
  target_x = 0;
  target_y = 0;

  //PID CODE (вводит x и y и выводит вектор нормали и высоту)

  error_x = target_x - pos_x;
  error_y = target_y - pos_y;

  dtostrf(error_x, 5, 3, str_temp1);
  dtostrf(error_y, 5, 3, str_temp2);
  sprintf(buf, "error1:%s error2:%s\n", str_temp1, str_temp2);
  Serial.print(buf);

  currentTime = millis();
  elapsedTime = currentTime - previousTime;
  if(elapsedTime==0){elapsedTime = 1;}

  cumError_x += error_x * elapsedTime / 1000;
  cumError_y += error_y * elapsedTime / 1000;
  rateError_x = (error_x - lastError_x)/(elapsedTime/1000);
  rateError_y = (error_y - lastError_y)/(elapsedTime/1000);

  output_x = Kp*error_x + Ki*cumError_x + Kd * rateError_x;
  output_y = Kp*error_y + Ki*cumError_y + Kd * rateError_y;

  inVec[0] = output_x/plateRadius;
  inVec[1] = output_y/plateRadius;
  inVec[2] = plateRadius;

  lastError_x = error_x;
  lastError_y = error_y;
  previousTime = currentTime;

  //INVKINEMATIC (вводит вектор нормали и высоту и выводит альфа-значения)

  normVec[0] = inVec[0]/(sqrt(inVec[0]*inVec[0]+inVec[1]*inVec[1]+inVec[2]*inVec[2]));
  normVec[1] = inVec[1]/(sqrt(inVec[0]*inVec[0]+inVec[1]*inVec[1]+inVec[2]*inVec[2]));
  normVec[2] = inVec[2]/(sqrt(inVec[0]*inVec[0]+inVec[1]*inVec[1]+inVec[2]*inVec[2]));

  p1[0] = normVec[2];
  p1[2] = -1*normVec[0];
  normSave = sqrt(p1[0]*p1[0]+p1[1]*p1[1]+p1[2]*p1[2]);
  p1[0] = plateRadius*p1[0]/normSave;
  p1[1] = plateRadius*p1[1]/normSave;
  p1[2] = plateRadius*p1[2]/normSave;

  crosskv[0] = normVec[1]*p1[2] - normVec[2]*p1[1];
  crosskv[1] = normVec[2]*p1[0] - normVec[0]*p1[2];
  crosskv[2] = normVec[0]*p1[1] - normVec[1]*p1[0];
  dotSave = normVec[0]*p1[0] + normVec[1]*p1[1] + normVec[2]*p1[2];
  p2[0] = -0.5*p1[0] + crosskv[0]*0.866 + normVec[0]*dotSave*1.5;
  p2[1] = -0.5*p1[1] + crosskv[1]*0.866 + normVec[1]*dotSave*1.5;
  p2[2] = -0.5*p1[2] + crosskv[2]*0.866 + normVec[2]*dotSave*1.5;
  normSave = sqrt(p2[0]*p2[0]+p2[1]*p2[1]+p2[2]*p2[2]);
  p2[0] = plateRadius*p2[0]/normSave;
  p2[1] = plateRadius*p2[1]/normSave;
  p2[2] = plateRadius*p2[2]/normSave;

  p3[0] = -0.5*p1[0] + crosskv[0]*-0.866 + normVec[0]*dotSave*1.5;
  p3[1] = -0.5*p1[1] + crosskv[1]*-0.866 + normVec[1]*dotSave*1.5;
  p3[2] = -0.5*p1[2] + crosskv[2]*-0.866 + normVec[2]*dotSave*1.5;
  normSave = sqrt(p3[0]*p3[0]+p3[1]*p3[1]+p3[2]*p3[2]);
  p3[0] = plateRadius*p3[0]/normSave;
  p3[1] = plateRadius*p3[1]/normSave;
  p3[2] = plateRadius*p3[2]/normSave;

  p1[2] = height+p1[2];
  p2[2] = height+p2[2];
  p3[2] = height+p3[2];

  radius = sqrt((p1[0]-B)*(p1[0]-B)+p1[1]*p1[1]+p1[2]*p1[2]);
  gamma = atan2(p1[2],sqrt((p1[0]-B)*(p1[0]-B)+(p1[1])*(p1[1])));
  beta = acos((L*L+radius*radius-U*U)/(2*L*radius));
  alpha[0] = 3.1416 - (gamma + beta);

  radius = sqrt((p2[0]+0.5*B)*(p2[0]+0.5*B)+(p2[1]-0.866*B)*(p2[1]-0.866*B)+(p2[2])*(p2[2]));
  gamma = atan2(p2[2],sqrt((p2[0]+0.5*B)*(p2[0]+0.5*B)+(p2[1]-0.866*B)*(p2[1]-0.866*B)));
  beta = acos((L*L+radius*radius-U*U)/(2*L*radius));
  alpha[1] = 3.1416 - (gamma + beta);

  radius = sqrt((p3[0]+0.5*B)*(p3[0]+0.5*B)+(p3[1]+0.866*B)*(p3[1]+0.866*B)+(p3[2])*(p3[2]));
  gamma = atan2(p3[2],sqrt((p3[0]+0.5*B)*(p3[0]+0.5*B)+(p3[1]+0.866*B)*(p3[1]+0.866*B)));
  beta = acos((L*L+radius*radius-U*U)/(2*L*radius));
  alpha[2] = 3.1416 - (gamma + beta);

  dtostrf(normVec[0], 5, 3, str_temp1);
  dtostrf(normVec[1], 5, 3, str_temp2);
  dtostrf(normVec[2], 5, 3, str_temp3);
  sprintf(buf, "a1:%s a2:%s a3:%s\n", str_temp1, str_temp2, str_temp3);
  Serial.print(buf);

  // ВЫВОД альфа-канала на сервоприводы

  delay(1000);
}

Вот вывод с ошибкой:

Starting...
error1: error2:-1.000
a1:  NAN a2:  NAN a3:

error1: error2:-1.000
a1: a2:-0.033 a3:

error1: error2:-1.000
a1: a2:-0.033 a3:

error1: error2:-1.000
a1: a2:-0.033 a3:

error1: error2:-1.000
a1: a2:-0.033 a3: