PID.cpp

float KP = 300, KI = 15, KD = 600;
float pTerm = 0, iTerm = 0,  dTerm = 0;
float last_pitch = 0;

void loop()
{
  float pitch = get_pitch(); // угол отклонения от вертикальной оси

  pTerm = KP * pitch;
  iTerm = KI * pitch + iTerm;
  dTerm = KD * (pitch - last_pitch);
  
  last_pitch = pitch;
  
  float spd = pTerm + iTerm + dTerm;
  set_motors(-spd, -spd);
}