fixed the angle calculation code

it now properly calculates all of the angles and speeds

added a speed() function.