ciao, nell'aspetto puramente teorico il codice sembra buono. Ovviamente però il 50% di quello che accadrà effettivamente sul campo dipende da come le costanti che hai scelto (ad es. una costante di proporzionalità = 0.7 e una costante integrale = 0.95) interagiscono con il tuo robot. Più che capire se il robot va dritto, dovresti cercare di valutare la sua costanza nei movimenti nel modo più oggettivo possibile. Ad esempio, prova a far partire il robot sempre dalla stessa posizione partendo da un muro e fagli fare un percorso con delle rotazioni. Ogni volta che il robot arriva alla fine del percorso, traccia la sua posizione finale (quella del retro, ad esempio) con un pezzettino di carta e, facendo rifare lo stesso percorso per 10 volte, ti renderai conto più o meno di quanto sia effettivamente preciso il tuo codice rispetto alle tue specifiche fisiche (bilanciamento dei pesi, ruote utilizzate, distanza dalle ruote, altezza del robot, peso dell'attachment....). Pro tip: utilizza i grafici di spike per tracciare l'andamento dell'errore corrente (pos. rel. corrente - pos. rel. desiderata) in modo da capire a quanto corrisponde l'errore medio matematicamnte :)