modified the code for centroid, the angle fixed, we have problem of self stabilizing for one robot, when it is disconnect, it never updates its value forever