adds equality check to vector3
changes current and target positions to operate using servo angles
re-organizes stewartplatform methods
adds target/current angle/speed logic (without deceleration)