added the fully tested and working version which was used for baxter, lwr, youbot text and renamed the old version which ID in 3 sweeps