So i read the position from the motors and then i want to compute the new positions to give them.
A servo motor has no idea where it is. At best, the Servo library will return, when you perform a read(), the last position that the servo was commanded to go to, whether it got there, or not.
But I prefer use Simulink becouse it could be more intuitive to write this kind of algorithm.
Someone can help me?
So, it's not that intuitive, huh?