robot lento e stralento

Ancora io che vi stresso! Buonasera a tutti!
Il mio self-balancing è ancora molto lento… eppure credevo di avere fissato il tempo del loop a 5 millisecondi, perchè 10 erano troppi, a giudizio di astro che aveva guardato un video del mio robot.
Vorrei capire meglio, perchè non mi sembra che il programma dia comandi alle ruote più velocemente, quindi ho sicuramente fatto qualche errore…

void PID(double restAngle) {

 unsigned long now = millis();
 int timeChange = (now - lastTime);
 if (timeChange >= SampleTime) {

    double error = restAngle - pitch;
    double pTerm = Kp * error;
    double iTerm = iTerm + error;
    iTerm = Ki * iTerm;
    if (iTerm > 90) iTerm = 90;
    else if (iTerm < -90) iTerm = -90;
    double lastError;
    double dTerm = Kd * (error - lastError);
    double PIDValue = pTerm + iTerm - dTerm;
    if (PIDValue > 799) PIDValue = 799;
    else if (PIDValue < -799) PIDValue = -799;
    lastError = error;
    lastTime = now;
    double PIDLeft;
    double PIDRight;
    PIDLeft = PIDValue;
    PIDRight = PIDValue;

    moveMotor(left, PIDLeft);
    moveMotor(right, PIDRight);
    
   Serial.print(timeChange);
   Serial.println();
   }
}

Ora, se non stampo null’altro a seriale, timeChange risulta essere sempre=5; nel momento in cui mi metto stampare anche i valori della imu, a seriale stampa 6 8 10 11 5 (uno dopo l’altro): rallentamento dovuto alla trasmissione seriale, ho pensato io.
Io ho nel loop chiamo PID(targetAngle);
Tirando le somme: così facendo sto fissando la frequenza di calcolo del pid o dovrei fare una cosa del tipo:

 //PSEUDOCODICE
void loop{
 unsigned long now = millis();
 int timeChange = (now - lastTime);
 if (timeChange >= SampleTime) {
   
      PID(targetAngle);
}    
    lastError = error;
    lastTime = now;
}

void PID(double restAngle) {

    double error = restAngle - pitch;
    double pTerm = Kp * error;
    double iTerm = iTerm + error;
    .......................

def_1.0.ino (9.57 KB)

Non ho ben capito cosa chiedi ma per poter inviare più dati sulla seriale potresti accumularli in un array ed inviarli tutti ad intervalli di tempo maggiore, magari portando il baudrate al massimo consentito dal tuo sistema, così avresti più dati su cui lavorare. Una domanda ti faccio, perché non utilizzi la library PID ma ne hai creato uno tuo?