// PID
float goalAngle = 0;
float prev_diff = 0;
float Kp = 14.0, Ki = 0.14, Kd = 0; // pocz. Kp=10, Ki=0.05, Kd=0
float I = 0;

float calculatePid(float goalAngleAnalog) {
  float diff = goalAngleAnalog - angle_y;
  float P = diff;
  I += diff;
  float D = diff - prev_diff;
  prev_diff = diff;

  float pid = P * Kp + I * Ki + D * Kd;
  return pid;
}