Made it work! but many bits need change, like finding the centroid, finding rotation speed using frame rate and so on