eyy fixed the speed, realized i had to use Pythagorean theorem

Signed-off-by: Exikle <dixondcunha@hotmail.com>