[@$3.1415rin]

well, if they both look into the sky or to the ground, this doesnt look like a problem with the algorithm, but with your code handling the angles, setting the ideal angles maybe ( have you set the ideal speed to zero if you don't yet calculate it ? )