Fix alvaroTest math

Changed the calculations of rightmotor and leftmotor.