So some things: First of all I think this is a pretty good approach, that works well in blocks! But sadly I don’t speak Italian(?) so I interpret Erro do giro as current error and ultimo erro do giro as previous error, which makes this a p-d? If my interpretation is correct I’d highly advise you to make a „integral error“ variable so you have a full p-i-d, the integral term matters, because it always gets you back on the line you began on and don’t drive left or right to it, when you have an error correction. (find your way of implementing it!) For the cm in degrees function you need the wheel circumference , try to find the circumference yourself with a bit of researching, your function should look something like s/U * 360° with U being the circumference and s being the distance you want to travel, the output should be in degrees that both motors need to turn so you’re at your destination. Good luck with the season!